wait until pr2 joint trajectory motion ends

pr2_controllers

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

1186 ●29 ●52 ●57

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

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

add a comment

1

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

liuhuanjim013
152 ●6 ●6 ●12

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

edit flag offensive delete link

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 ( 2011-06-24 00:50:45 -0600 )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 ( 2011-06-24 03:44:15 -0600 )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 ( 2011-07-06 05:17:01 -0600 )edit

add a comment

0

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

liuhuanjim013
152 ●6 ●6 ●12

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

add a comment

wait until pr2 joint trajectory motion ends

2 Answers

Comments

Question Tools

Stats

Related questions

wait until pr2 joint trajectory motion ends edit

2 Answers

Comments

Question Tools

Stats

Related questions

wait until pr2 joint trajectory motion ends