0
$\begingroup$

Rosanswers logo

Hi,

I'm using JointTrajectoryAction to control the arm to follow a trajectory that can be executed well by openrave's controller. Most of the time, it works. But I found one case where the trajectory can be executed by openrave's controller but not JointTrajectoryActionController.

Here's a video showing the problem:

http://people.csail.mit.edu/liuhuan/pr2/shoulder2.avi

The video first shows the ideal motion of the robot following a trajectory. Then it shows the result of the trajectory being executed on the robot via JointTrajectoryAction, where the shoulder_lift_joint does not reach its goal.

Below is a script showing the problematic motion:

http://people.csail.mit.edu/liuhuan/pr2/jointlimitproblem.py

The problematic trajectory file:

http://people.csail.mit.edu/liuhuan/pr2/shoulder.traj

After the motion completes, the result of /l_arm_controller/state is:


---
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
---

The trajectory does not contain a point that asks the shoulder_lift_joint to go beyond its soft limit defined in the urdf file which is ~ [-.52, 1.39], but the shoulder_lift_joint still stops at -.35 when it's commended to go to -.45.

This happens on both the simulated robot in Gazebo and the real robot.

I tried slowing down the trajectory as well (x10 times), but the shoulder_lift_joint still didn't go to -.45.

Below is the output of /joint_states when the motion completes. The effort for /l_shoulder_lift_joint is -0.0023171897976062093.

The arm also floats in a neutral position when it's not powered.

---
header: 
  seq: 10891
  stamp: 
    secs: 117
    nsecs: 22000000
  frame_id: ''
name: ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 'br_caster_r_wheel_joint', 'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 'r_upper_arm_roll_joint', 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_forearm_roll_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_joint', 'r_gripper_l_finger_joint', 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_l_finger_tip_joint', 'l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_joint', 'l_gripper_l_finger_joint', 'l_gripper_r_finger_joint', 'l_gripper_r_finger_tip_joint', 'l_gripper_l_finger_tip_joint']
position: [-1.8911007431121618e-05, 0.017315724103188046, 0.018450151123038161, -4.2352233703191189e-05, 0.015518171855746665, 0.01503525977753295, 9.623874763775575e-06, 0.013270571780459761, 0.0121751864435371, -2.3722479977728028e-05, 0.0079390893900841419, 0.0078856620877401085, 0.067548587109680494, -0.0030113676917249421, 0.36397195459308573, -0.16406783417664261, -9.0504940351010532e-06, -1.5707688803932429, 1.013406538774575e-05, 6.0780678108329766e-06, -0.14997776679068586, -0.10000287454784118, 4.2789370331419718e-06, 0.0015572890254982397, 0.010626329318313282, 0.010626329318313282, 0.010626329318313282, 0.010626329318313282, 2.9684229023394977, -0.45064403278428422, -0.35359079109074809, 2.612415939749134, -1.7368340148522607, -1.3944002154008768, 0.2817094449786417, 0.00089310259152319956, 0.0069875467278469328, 0.0069875467278469328, 0.0069875467278469328, 0.0069875467278469328]
velocity: [-0.00072446244741820474, -0.0097379001763039964, -0.0097262401388382522, -0.0013967477832816733, 0.0083109308326779555, 0.011713674780299171, 0.00067504834398672033, -0.0033504935635703245, -0.0080750352378221573, 0.00074654446307559981, 0.0079475120390310322, 0.00095459774625177279, -8.2790843763332348e-06, -0.00042483857497946842, -0.00045694122976036952, 0.00033039659595387409, -0.00059473925772518424, -0.0050961511637364725, -0.0024067622410066415, -0.00051510474573985741, 0.001409985561803666, 0.0003270120061952121, -0.00017799925155541243, 8.4491119593198071e-05, 0.00046295033000519321, 0.00046295033000519321, 0.00046295033000519321, 0.00046295033000519321, -0.0035433003411864039, 4.7825569839559257e-05, -0.00056668358968307582, 0.0014806109280868286, 0.0013254131779032075, 0.0018822638326020783, 0.0001260091201729591, 3.0076603867091915e-05, 0.00016475501509363873, 0.00016475501509363873, 0.00016475501509363873, 0.00016475501509363873]
effort: [0.034876710547126905, 0.023385458346301468, 0.019731940830663913, 0.027162876519764657, -0.020947980010554038, -0.02347952968928365, -0.013092152583383886, 0.021420898368881676, 0.0065593034938750527, 0.020469318761812544, -0.024140541333018017, -0.014556167522506436, 474.39943502232683, 0.0022344979275061732, -0.93049960573216084, -0.00023177228362657762, 0.017138836814558522, 0.033131599269361822, 0.0055921275574100093, -0.0040849597137056779, -0.012759147440222525, 0.00084884858700887231, 0.00010958438791097203, 0.0, 0.0, 0.0, 0.0, 0.0, 0.022701182443814416, 0.046628480113244261, -0.0023171897976062093, -0.0041342331521128641, -0.012882246104209353, -0.0084020901856182335, -0.00248784567748387, 0.0, 0.0, 0.0, 0.0, 0.0]

Thanks for your help!

Huan


Originally posted by liuhuanjim013 on ROS Answers with karma: 152 on 2011-06-18

Post score: 1

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

take shoulder lift urdf for example, you can see 'shoulder_lift_joint' limits are:

  <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
  <limit lower="-0.5236" upper="1.3963"
         effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.47}" /> <!-- alpha tested velocity and effort limits -->

with safety controller

  <safety_controller k_position="100" k_velocity="10"
                     soft_lower_limit="${-0.5236+0.17}" soft_upper_limit="${1.3963-0.10}" />

so safety controllers will start imposing limits before the hardware limits [-0.5236, 1.3963]. In this case, a counter force is applied in the range of [-0.3536, 1.2963] by the safety controller.

John


Originally posted by hsu with karma: 5780 on 2011-06-28

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by liuhuanjim013 on 2011-06-28:
Hi John, one more question: how do I get the soft limits? I couldn't find a ros parameter for it. Is there any way other than using the urdf API to get the soft limits?

Comment by liuhuanjim013 on 2011-06-28:
Thanks John! I thought the tag was already the soft limit. Problem solved! The dae file generated by openrave does not take into account the <safety_controller> tag.

$\endgroup$
0
$\begingroup$

Rosanswers logo

Are you sending commands inside the joint safety limits?


Originally posted by tfoote with karma: 58457 on 2011-06-19

This answer was NOT ACCEPTED on the original site

Post score: 3


Original comments

Comment by liuhuanjim013 on 2011-06-28:
Hi Tully, you are right! The soft limit is indeed violated. See my comment to John's answer above. Thanks for your help!

Comment by liuhuanjim013 on 2011-06-19:
Hi Tully, I just turned down the speed of the trajectory by 10 times, but the shoulder_lift_joint still didn't go all the way up.. And I saw no warning or error message in rosout. Any other way I can confirm that my trajectory doesn't violate any limit?

Comment by liuhuanjim013 on 2011-06-19:
good call tully! i'll slow down the trajectory and try it out.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.