what might cause the shoulder_lift_joint not to go all the way up?
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/p...
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/p...
The problematic trajectory file:
http://people.csail.mit.edu/liuhuan/p...
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 ...
