Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

Why such a long default duration? #391

Closed
wants to merge 1 commit into from

Conversation

awesomebytes
Copy link
Contributor

I've noticed that when asking for a pick plan my gripper time_to_start times get this time added which makes the closing of the gripper very slow.
As I don't fully understand the need of this value (there is one most probably) I propose lowering it.

I've noticed that when asking for a pick plan my gripper time_to_start times get this time added which makes the closing of the gripper very slow.
As I don't fully understand the need of this value (there is one most probably) I propose lowering it.
@@ -46,7 +46,7 @@

const std::string PickPlace::DISPLAY_PATH_TOPIC = planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC;
const std::string PickPlace::DISPLAY_GRASP_TOPIC = "display_grasp_markers";
const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0; // seconds
const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 1.0; // seconds
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This duration really depends on how slow your gripper is. The correct fix would be to make this a dynamic parameter.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, maybe the problem here is that this time is added to the time you specify on the grasp (pre_grasp_posture and grasp_posture).
So I put on my grasp message:
grasp_data.pre_grasp_posture.points[0].time_from_start = ros::Duration(3,0);
grasp_data.grasp_posture.points[0].time_from_start = ros::Duration(4,0);

So I'm expecting to wait for 3s when doing the pre_grasp_posture, and for 4s for the grasp_posture

But on execution the goal sent to the gripper controller (a hand in our case) is (for the pre_grasp_posture):

joint_names: ['hand_right_index_joint', 'hand_right_middle_joint', 'hand_right_thumb_joint']
points:
-
positions: [0.10000347159257128, 0.10000347318852754, 0.09998135899197091]
velocities: [0.0, 0.0, 0.0]
accelerations: [0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 7
nsecs: 0
-
positions: [0.0, 0.0, 1.3]
velocities: [0.0, 0.0, 0.0]
accelerations: [0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 10
nsecs: 0
path_tolerance: []
goal_tolerance: []
goal_time_tolerance:
secs: 0
nsecs: 0

This is set here: https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/pick_place/src/plan_stage.cpp#L88
pre_approach_traj->addPrefixWayPoint(pre_approach_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);

My first focus is to make this work on my robot, so changing that to 1.0s made the trick. I see it's only used for opening and closing the gripper. Shouldn't then the time_to_start field of a grasp be checked somewhere and if it's 0 then add this default value? Cause right now it seems it's adding it all the time.

Sorry if I'm not so clear explaining myself! But I'm tinkering around a lot for making the pick work (later on the place... and pick and place I hope).

@hersh
Copy link
Contributor

hersh commented Apr 2, 2014

Closing this pull request, as #443 seems like a better solution.

@hersh hersh closed this Apr 2, 2014
Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants