Reset joint limits with setzeroconfig

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view
|

Reset joint limits with setzeroconfig

Mathew Schwartz
I am using the kuka youbot downloaded from the robot list and trying to use the TranslationDirection5D IK. 


In ROS, the youbot is physically configured to set zero on the hardware limit.  In Openrave the zero configuration is set to be the candle pose (totally vertical).  So I would like to reset the zero configuration, however, the limits are not updated. For example, this code:

    joint_angles = np.array([-2.79252,-0.785398,1.57079,-2.09439,-2.70525])
    ikmodel.robot.SetDOFValues(joint_angles,ikmodel.manip.GetArmIndices())
    env.UpdatePublishedBodies()
    time.sleep(10)
    print 'Matching hardware zero: '
    print ikmodel.robot.GetDOFValues()

    ikmodel.robot.SetZeroConfiguration()
    print 'Set Zero: '
    print ikmodel.robot.GetDOFValues()

    print 'Midpoint: '
    joint_angles = np.array([2.949606, 1.134464, -2.548181, 1.788962, 2.923426])
    ikmodel.robot.SetDOFValues(joint_angles,ikmodel.manip.GetArmIndices())
    env.UpdatePublishedBodies()
    time.sleep(10)

Generates This output:

Robot Candle Pose:
[  0.00000000e+00  -6.18426209e-17   0.00000000e+00   0.00000000e+00
   0.00000000e+00   0.00000000e+00   0.00000000e+00]
Matching hardware zero:
[  0.00000000e+00  -9.71445147e-17  -2.79252000e+00  -7.85398000e-01
   1.57079000e+00  -2.09439000e+00  -2.70525000e+00]
Set Zero:
[  0.00000000e+00  -9.71445147e-17   0.00000000e+00   0.00000000e+00
   0.00000000e+00   0.00000000e+00   0.00000000e+00]
Midpoint:
[kinbody.cpp:1163] dof 2 value is not in limits 2.949606e+00<2.792527e+00
[kinbody.cpp:1152] dof 4 value is not in limits -2.548181e+00<-1.570796e+00
[kinbody.cpp:1163] dof 6 value is not in limits 2.923426e+00<2.705260e+00

So even though the robot has moved to a new configuration and returns its position as 0, where it was previously -2.79, it gives an error trying to move to 2.9 from its new 0 position.

Is this an openrave problem or am I doing it wrong? 

Also, the joint limits dont seem to be right, even in the dae file it says the limit is +/-169 degrees, which is 2.949 radians, the limit shows up as 2.792527. 

Finally, will setting the zero configuration after having generated the IK database work as I hope, or should this be defined before generating the IK?

Advice on fixing this issue or a different workflow is appreciated.

Thanks,

Mat

------------------------------------------------------------------------------

_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users