CalculateJacobian and CalculateAngularVelocityJacobian

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

CalculateJacobian and CalculateAngularVelocityJacobian

เปี๊ยก ....
Dear Rosen

I try to find jacobian of the sample robot

"A simple 3DOF planar arm in XZ"


from openrave website.

This is my source code
///////////////////////////////////////////////////
from openravepy import *
env=Environment()
env.SetViewer('qtcoin')
env.Load('RobotTest1.robot.xml')
hand=env.GetKinBody('3DOFRobot')
part=hand.GetLinks()

Pindex=[]

print "linear"
for i in range(0,len(part)):
   Pindex.append(part[i].GetIndex())
   hand.CalculateJacobian(Pindex[i],[0,0,0])


print "angular"
for i in range(0,len(part)):
   hand.CalculateAngularVelocityJacobian(Pindex[i])

for i in range(0,len(part)):
   part[i].GetTransform()
////////////////////////////////////////////////////////

However, in my opinion, the result is quite strange. I try to derive these jacobian by hand using transformation matrix of each link and rotation axis form xml file.
This is jacobian from CalculateJacobian

Jacobian (velocity)                                            
                                                               
array([[ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)
array([[ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)              
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  , -0.14,  0.  ,  0.  ,  0.  ]], dtype=float32)(link2)    
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  , -0.14, -0.28,  0.  ,  0.  ]], dtype=float32)(link3)    
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  , -0.14, -0.28,  0.01,  0.  ]], dtype=float32)(link4)    
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  , -0.14, -0.28,  0.  , -0.01]], dtype=float32)(link5)

Jacobian  (angular)                                            
                                                               
array([[ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)              
array([[-0.,  0.,  0.,  0.,  0.],                              
       [-1.,  0.,  0.,  0.,  0.],                              
       [-0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)
array([[-0., -0.,  0.,  0.,  0.],
       [-1., -1.,  0.,  0.,  0.],
       [-0., -0.,  0.,  0.,  0.]], dtype=float32)(link2)
array([[-0., -0., -0.,  0.,  0.],
       [-1., -1., -1.,  0.,  0.],
       [-0., -0., -0.,  0.,  0.]], dtype=float32)(link3)
array([[-0., -0., -0., -1.,  0.],
       [-1., -1., -1., -0.,  0.],
       [-0., -0., -0., -0.,  0.]], dtype=float32)(link4)
array([[-0., -0., -0.,  0.,  1.],
       [-1., -1., -1.,  0., -0.],
       [-0., -0., -0.,  0., -0.]], dtype=float32)(link5)

Transformation matrix of each link

array([[ 1.,  0.,  0.,  0.],
       [ 0.,  1.,  0.,  0.],
       [ 0.,  0.,  1.,  0.],
       [ 0.,  0.,  0.,  1.]], dtype=float32)(base)
array([[ 1.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   ,  1.   ,  0.   ,  0.005],
       [ 0.   ,  0.   ,  1.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  1.   ]], dtype=float32)(link1)
array([[ 1.   ,  0.   ,  0.   ,  0.14 ],
       [ 0.   ,  1.   ,  0.   ,  0.015],
       [ 0.   ,  0.   ,  1.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  1.   ]], dtype=float32)(link2)
array([[ 1.   ,  0.   ,  0.   ,  0.28 ],
       [ 0.   ,  1.   ,  0.   ,  0.005],
       [ 0.   ,  0.   ,  1.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  1.   ]], dtype=float32)(link3)
array([[ 0.  ,  1.  ,  0.  ,  0.34],
       [ 0.  ,  0.  , -1.  ,  0.01],
       [-1.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  1.  ]], dtype=float32)(link4)
array([[ 0.  ,  1.  ,  0.  ,  0.34],
       [ 0.  ,  0.  , -1.  ,  0.01],
       [-1.  ,  0.  ,  0.  ,  0.  ],
       [ 0.  ,  0.  ,  0.  ,  1.  ]], dtype=float32)(link5)


This is my Jacobian.

Jacobian (velocity)                                            
                                                               
array([[ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)
array([[ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)              
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ -0.14  , 0. ,  0.  ,  0.  ,  0.  ]], dtype=float32)(link2)    
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ -0.28, -0.14, 0. ,  0.  ,  0.  ]], dtype=float32)(link3)    
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ -0.34  , -0.2, -0.06,  0.  ,  0.  ]], dtype=float32)(link4)    
array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],                    
       [ -0.34  , -0.2, -0.06,  0.  ,  0.  ]], dtype=float32)(link5)

Jacobian  (angular)                                            
                                                               
array([[ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.],                              
       [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)              
array([[-0.,  0.,  0.,  0.,  0.],                              
       [1.,  0.,  0.,  0.,  0.],                              
       [-0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)
array([[-0., -0.,  0.,  0.,  0.],
       [1., 1.,  0.,  0.,  0.],
       [-0., -0.,  0.,  0.,  0.]], dtype=float32)(link2)
array([[-0., -0., -0.,  0.,  0.],
       [1., 1., 1.,  0.,  0.],
       [-0., -0., -0.,  0.,  0.]], dtype=float32)(link3)
array([[-0., -0., -0., 1.,  0.],
       [1., 1., 1., -0.,  0.],
       [-0., -0., -0., -0.,  0.]], dtype=float32)(link4)
array([[-0., -0., -0.,  0.,  -1.],
       [1., 1., 1.,  0., -0.],
       [-0., -0., -0.,  0., -0.]], dtype=float32)(link5)


I also wonder about the transformation matrix of link 4 and link5 from link.GetTransform(). Since the rotation matrix form xml file is 0 0 1 -1 0 0 0 1 0, then the rotation component of transfromation matrix should be transpose(R) not -transpose(R).

I found these function from openravepy_int.cpp. Actually, I would like to check these functions before contact you, but I cannot find the description and calculation detail of these functions. Could you please consider them and tell me about the location of these functions.

Best Regards,
Papat



------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Reply | Threaded
Open this post in threaded view
|

Re: CalculateJacobian and CalculateAngularVelocityJacobian

Rosen Diankov
Administrator
hi papat,

i don't think there's any problems with the jacobian computations,
perhaps there's a misunderstanding about how they are used.

hand.CalculateJacobian(endlink,offset)

means it will calculate how all the links up to 'endlink' affect
'offset', which is in world coordinates. there's a test_jacobian()
function in test/testkinbody.py which makes sure the jacobian
computations are correct.

in xml, if "0 1 0 0 0 -1 -1 0 0" is specified, the matrix becomes
0 1 0
0 0 -1
-1 0 0

the problem with the transform on the wiki is that it wasn't a valid
rotation matrix (it is fixed now), so openrave was converting it to
the nearest rotation matrix

rosen,

2010/3/23 เปี๊ยก .... <[hidden email]>:

> Dear Rosen
>
> I try to find jacobian of the sample robot
>
> "A simple 3DOF planar arm in XZ"
>
> from openrave website.
>
> This is my source code
> ///////////////////////////////////////////////////
> from openravepy import *
> env=Environment()
> env.SetViewer('qtcoin')
> env.Load('RobotTest1.robot.xml')
> hand=env.GetKinBody('3DOFRobot')
> part=hand.GetLinks()
>
> Pindex=[]
>
> print "linear"
> for i in range(0,len(part)):
>    Pindex.append(part[i].GetIndex())
>    hand.CalculateJacobian(Pindex[i],[0,0,0])
>
>
> print "angular"
> for i in range(0,len(part)):
>    hand.CalculateAngularVelocityJacobian(Pindex[i])
>
> for i in range(0,len(part)):
>    part[i].GetTransform()
> ////////////////////////////////////////////////////////
>
> However, in my opinion, the result is quite strange. I try to derive these
> jacobian by hand using transformation matrix of each link and rotation axis
> form xml file.
> This is jacobian from CalculateJacobian
>
> Jacobian (velocity)
>
> array([[ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)
> array([[ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  , -0.14,  0.  ,  0.  ,  0.  ]], dtype=float32)(link2)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  , -0.14, -0.28,  0.  ,  0.  ]], dtype=float32)(link3)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  , -0.14, -0.28,  0.01,  0.  ]], dtype=float32)(link4)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  , -0.14, -0.28,  0.  , -0.01]], dtype=float32)(link5)
>
> Jacobian  (angular)
>
> array([[ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)
> array([[-0.,  0.,  0.,  0.,  0.],
>        [-1.,  0.,  0.,  0.,  0.],
>        [-0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)
> array([[-0., -0.,  0.,  0.,  0.],
>        [-1., -1.,  0.,  0.,  0.],
>        [-0., -0.,  0.,  0.,  0.]], dtype=float32)(link2)
> array([[-0., -0., -0.,  0.,  0.],
>        [-1., -1., -1.,  0.,  0.],
>        [-0., -0., -0.,  0.,  0.]], dtype=float32)(link3)
> array([[-0., -0., -0., -1.,  0.],
>        [-1., -1., -1., -0.,  0.],
>        [-0., -0., -0., -0.,  0.]], dtype=float32)(link4)
> array([[-0., -0., -0.,  0.,  1.],
>        [-1., -1., -1.,  0., -0.],
>        [-0., -0., -0.,  0., -0.]], dtype=float32)(link5)
>
> Transformation matrix of each link
>
> array([[ 1.,  0.,  0.,  0.],
>        [ 0.,  1.,  0.,  0.],
>        [ 0.,  0.,  1.,  0.],
>        [ 0.,  0.,  0.,  1.]], dtype=float32)(base)
> array([[ 1.   ,  0.   ,  0.   ,  0.   ],
>        [ 0.   ,  1.   ,  0.   ,  0.005],
>        [ 0.   ,  0.   ,  1.   ,  0.   ],
>        [ 0.   ,  0.   ,  0.   ,  1.   ]], dtype=float32)(link1)
> array([[ 1.   ,  0.   ,  0.   ,  0.14 ],
>        [ 0.   ,  1.   ,  0.   ,  0.015],
>        [ 0.   ,  0.   ,  1.   ,  0.   ],
>        [ 0.   ,  0.   ,  0.   ,  1.   ]], dtype=float32)(link2)
> array([[ 1.   ,  0.   ,  0.   ,  0.28 ],
>        [ 0.   ,  1.   ,  0.   ,  0.005],
>        [ 0.   ,  0.   ,  1.   ,  0.   ],
>        [ 0.   ,  0.   ,  0.   ,  1.   ]], dtype=float32)(link3)
> array([[ 0.  ,  1.  ,  0.  ,  0.34],
>        [ 0.  ,  0.  , -1.  ,  0.01],
>        [-1.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  1.  ]], dtype=float32)(link4)
> array([[ 0.  ,  1.  ,  0.  ,  0.34],
>        [ 0.  ,  0.  , -1.  ,  0.01],
>        [-1.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  1.  ]], dtype=float32)(link5)
>
>
> This is my Jacobian.
>
> Jacobian (velocity)
>
> array([[ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)
> array([[ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ -0.14  , 0. ,  0.  ,  0.  ,  0.  ]], dtype=float32)(link2)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ -0.28, -0.14, 0. ,  0.  ,  0.  ]], dtype=float32)(link3)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ -0.34  , -0.2, -0.06,  0.  ,  0.  ]], dtype=float32)(link4)
> array([[ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ 0.  ,  0.  ,  0.  ,  0.  ,  0.  ],
>        [ -0.34  , -0.2, -0.06,  0.  ,  0.  ]], dtype=float32)(link5)
>
> Jacobian  (angular)
>
> array([[ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.],
>        [ 0.,  0.,  0.,  0.,  0.]], dtype=float32)(base)
> array([[-0.,  0.,  0.,  0.,  0.],
>        [1.,  0.,  0.,  0.,  0.],
>        [-0.,  0.,  0.,  0.,  0.]], dtype=float32)(link1)
> array([[-0., -0.,  0.,  0.,  0.],
>        [1., 1.,  0.,  0.,  0.],
>        [-0., -0.,  0.,  0.,  0.]], dtype=float32)(link2)
> array([[-0., -0., -0.,  0.,  0.],
>        [1., 1., 1.,  0.,  0.],
>        [-0., -0., -0.,  0.,  0.]], dtype=float32)(link3)
> array([[-0., -0., -0., 1.,  0.],
>        [1., 1., 1., -0.,  0.],
>        [-0., -0., -0., -0.,  0.]], dtype=float32)(link4)
> array([[-0., -0., -0.,  0.,  -1.],
>        [1., 1., 1.,  0., -0.],
>        [-0., -0., -0.,  0., -0.]], dtype=float32)(link5)
>
>
> I also wonder about the transformation matrix of link 4 and link5 from
> link.GetTransform(). Since the rotation matrix form xml file is 0 0 1 -1 0 0
> 0 1 0, then the rotation component of transfromation matrix should be
> transpose(R) not -transpose(R).
>
> I found these function from openravepy_int.cpp. Actually, I would like to
> check these functions before contact you, but I cannot find the description
> and calculation detail of these functions. Could you please consider them
> and tell me about the location of these functions.
>
> Best Regards,
> Papat
>
>
>
> ------------------------------------------------------------------------------
> Download Intel&#174; Parallel Studio Eval
> Try the new software tools for yourself. Speed compiling, find bugs
> proactively, and fine-tune applications for parallel performance.
> See why Intel Parallel Studio got high marks during beta.
> http://p.sf.net/sfu/intel-sw-dev
> _______________________________________________
> Openrave-users mailing list
> [hidden email]
> https://lists.sourceforge.net/lists/listinfo/openrave-users
>
>

------------------------------------------------------------------------------
Download Intel&#174; Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users