PhantomX Reactor - problems with the third joint

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view
|

PhantomX Reactor - problems with the third joint

henryk
This post has NOT been accepted by the mailing list yet.
This post was updated on .
Thanks for the amazing solver. For PhantomX Pincher (aka the turtlebot arm) I am getting correct results, but in the case of PhantomX Reactor the solver has problems in computing the third joint.

Urdf/Xacro of PhantomX Reactor

The whole pipeline compiles correctly according to the instructions.

However, running ikfastdemo.cpp with the cpp file generated for PhantomX Reactor I am consistently getting 0 on the third joint, like in the following output:

===============================

Joint angles:  -2.507510  2.056816  -1.128068
Found fk solution for end frame:

  Translation:  x: -0.081801  y: -0.060156  z: 0.217283

     Rotation     0.000000   0.000000   0.000000
       Matrix:    0.000000   0.000000   0.000000
                  0.000000   0.000000   0.000000

 Euler angles:
       Yaw:   1.570796    (1st: rotation around vertical blue Z-axis in ROS Rviz)
       Pitch: 0.000000
       Roll:  0.000000

  Quaternion:  0.500000   0.500000   0.500000   0.500000
               0.500000 + 0.500000i + 0.500000j + 0.500000k   (alternate convention)


Found 2 ik solutions:
sol0 (free=0): -2.507510185241699, 2.056815862655639, 0.000000000000000,
sol1 (free=0): 0.634082468348094, 0.564002763409552, 0.000000000000000

===============================

I am assuming that there is a problem with the urdf file or maybe in the fact, that in the translation I am not supplying information from the srdf file.
Can you suggest what should be done in this situation?
Reply | Threaded
Open this post in threaded view
|

Re: PhantomX Reactor - problems with the third joint

henryk
This post has NOT been accepted by the mailing list yet.
This post was updated on .
It seems that using the  or_urdf  plugin leads to more reasonable solutions.

=====================

./compute fk 1.5 1.5 1.5 0 0


Found fk solution for end frame:

  Translation:  x: -0.007079  y: -0.075079  z: 0.047637  

     Rotation     0.000000   0.000000   0.000000  
       Matrix:    0.000000   0.000000   0.000000  
                  0.000000   0.000000   0.000000  

 Euler angles:
       Yaw:   1.570796    (1st: rotation around vertical blue Z-axis in ROS Rviz)
       Pitch: 0.000000  
       Roll:  0.000000  

  Quaternion:  0.500000   0.500000   0.500000   0.500000  
               0.500000 + 0.500000i + 0.500000j + 0.500000k   (alternate convention)


root@henryk-michalewski-pc:~/ikfast# ./compute ik -0.007079  -0.075079  0.047637 0.5 0.5 0.5 0.5 0. 0.
     Rotation     0.000000   0.000000   0.000000  
                  0.000000   0.000000   0.000000  
                  0.000000   0.000000   0.000000  

Found 4 ik solutions:
sol0 (free=0): -1.688013691983715, -2.020768096037361, 2.162362540171424, 0.000000000000000, 0.000000000000000,
sol1 (free=0): -1.688013691983715, -0.299422628641114, 1.500004140942973, 0.000000000000000, 0.000000000000000,
sol2 (free=0): 1.499994844972752, -0.221351398883498, 2.162362540171424, 0.000000000000000, 0.000000000000000,
sol3 (free=0): 1.499994844972752, 1.499994068512753, 1.500004140942973, 0.000000000000000, 0.000000000000000,
========================

However, I am a bit puzzled by the following output

========================
In [7]: body.GetJoints()
Out[7]:
[RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('elbow_pitch_joint'),
 RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('gripper_revolute_joint'),
 RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('gripper_right_joint'),
 RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('shoulder_pitch_joint'),
 RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('shoulder_yaw_joint'),
 RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('wrist_pitch_joint'),
 RaveGetEnvironment(1).GetKinBody('phantomx_reactor_arm').GetJoint('wrist_roll_joint')]

========================

Not being sure what is the right decision I decided for

chaintree = solver.generateIkSolver(baselink=1,eelink=10,freeindices=[5,6],solvefn=ikfast.IKFastSolver.solveFullIK_Translation3D)

It seems that the last two joint should not be used for Translation3D, but is it a correct approach? Also, the number in compute stop to behave reasonably if I give non-zero inputs on the free joints (the output of IK seems to be always 0 on the free joints).