Ask Your Question
0

wait until pr2 joint trajectory motion ends

asked 2011-06-23 13:38:21 -0500

Kei Okada gravatar image

updated 2011-06-23 13:39:03 -0500

Due to the hardware limitation, the robot did not follow the trajectory as we send via joint_trajectory_action, especially for a torso_controller/joint_trajectory_action of spine joint. We'd like to wait the program until the pr2 torso movement has finished. Currently, the robot send back failed status/result after the commanded time period, but the robot still continue to move. Is there a way to know if the robot still moving joint or finished interpolation.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2011-06-23 15:13:10 -0500

liuhuanjim013 gravatar image

have you tried your_action_client.wait_for_result() or your_action_client.send_goal_and_wait() (in python)?

edit flag offensive delete link more

Comments

send_goal_and_wait returns with aborted after commanded period has passed, but if the period is too small, say 1 sec for 300mm up of spine joint, the robot continue moving towards the original goal. I'd like to know when the robot reached that point.
Kei Okada gravatar image Kei Okada  ( 2011-06-24 00:50:45 -0500 )edit
I see. So you used your_action_client.get_state()? I also do this to see if the trajectory has completed: print 'arm action state:',actionlib.get_name_of_constant(actionlib.GoalStatus,arm_joint_action_client.get_state()). But you are right, sometimes the robot doesn't complete the trajectory.
liuhuanjim013 gravatar image liuhuanjim013  ( 2011-06-24 03:44:15 -0500 )edit
It sounds like this would work fine if you made your planner plan feasible plans with timeouts which would not be broken in normal operations.
tfoote gravatar image tfoote  ( 2011-07-06 05:17:01 -0500 )edit
0

answered 2011-06-24 03:47:34 -0500

liuhuanjim013 gravatar image

What about keep tracking of /joint_states or /l_arm_controller/state? /l_arm_controller/state does give you the error between desired and actual, for example:

---
header: 
  seq: 422241
  stamp: 
    secs: 4226
    nsecs: 27000000
  frame_id: ''
joint_names: ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
desired: 
  positions: [-0.45063500000000001, -0.453629, 2.9684300000000001, -1.73685, 2.6124100000000001, -1.3944000000000001, 0.28170399999999973]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  time_from_start: 
    secs: 0
    nsecs: 0
actual: 
  positions: [-0.4506183813835607, -0.3535970257554899, 2.9684618999661581, -1.7368655244971523, 2.6124013457752877, -1.3943968838867686, 0.28170416664188558]
  velocities: [-0.0028369264248514835, 0.0021461982864228284, 0.0030925275806860746, 0.0022533051752136402, -0.00019810275617700331, -0.0011666050558722749, 0.00023889403521337726]
  accelerations: []
  time_from_start: 
    secs: 0
    nsecs: 0
error: 
  positions: [1.6618616439312284e-05, 0.1000319742445101, 3.1899966157933335e-05, -1.5524497152252792e-05, -8.6542247124121729e-06, 3.1161132314494466e-06, 1.6664188584591955e-07]
  velocities: [-0.0028369264248514835, 0.0021461982864228284, 0.0030925275806860746, 0.0022533051752136402, -0.00019810275617700331, -0.0011666050558722749, 0.00023889403521337726]
  accelerations: []
  time_from_start: 
    secs: 0
    nsecs: 0
---
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-06-23 13:38:21 -0500

Seen: 353 times

Last updated: Jun 24 '11