The kinematic tree is represented by two C\++ objects called Model (which
contains the model constants: lengths, masses, names, etc) and Data (which
contains the working memory used by the model algorithms). Both C\++ objects are
contained in a unique Python class called RobotWrapper.
Import the class +RobotWrapper+ and create an instance of this class in the
python terminal. At initialization, RobotWrapper will read the model
description in the URDF file given as argument. In the following, we will use
the model of the Romeo robot, available in the directory "models" of Pinocchio.
Basic mathematical objects
~~~~~~~~~~~~~~~~~~~~~~~~~~
In the following, we will use numpy Matrix class to represent matrices and vectors. In numpy, vectors simply are matrices with one column. See the following example.
A = np.matrix([ [1,2,3,4],[5,6,7,8]]) # Define a 6x4 matrix
b = np.zeros([4,1]) # Define a 4 vector (ie a 4x1 matrix) initialized to 0.
c = A*b # Obtain c by multiplying A by b.
----
Utils functions
~~~~~~~~~~~~~~~
A bunch of useful functions are packages in the utils of pinocchio.
[source,python]
...
...
@@ -75,6 +64,57 @@ cross(rand(3),rand(3)) # Cross product of R^3
rotate('x',0.4) # Build a rotation matrix of 0.4rad around X.
----
Specific classes are defined to reprensent object of SE(3), se(3) and se(3)^*. Rigid displacement, elements of SE(3), are represented by the class SE3.
[source,python]
----
import pinocchio as se3
R = eye(3); p = zero(3)
M0 = se3.SE3(R,p)
M = se3.SE3.Random()
M.translation = p; M.rotation = R
----
Spatial velocities, elements of se(3) = M^6, are represented by the class Motion.
[source,python]
----
v = zero(3); w = zero(3)
nu0 = se3.Motion(v,w)
nu = se3.Motion.Random()
nu.linear = v; nu.angular = w
----
Spatial forces, elements of se(3)^* = F^6, are represented by the class Force.
[source,python]
----
f = zero(3); tau = zero(3)
phi0 = se3.Force(f,tau)
phi = se3.Force.Random()
phi.linear = f; phi.angular = tau
----
Robot kinematic tree
~~~~~~~~~~~~~~~~~~~~
The kinematic tree is represented by two C\++ objects called Model (which
contains the model constants: lengths, masses, names, etc) and Data (which
contains the working memory used by the model algorithms). Both C\++ objects are
contained in a unique Python class called RobotWrapper.
Import the class +RobotWrapper+ and create an instance of this class in the
python terminal. At initialization, RobotWrapper will read the model
description in the URDF file given as argument. In the following, we will use
the model of the Romeo robot, available in the directory "models" of Pinocchio.
The code of the RobotWrapper class is in /home/student/src/pinocchio/src/python/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.
Exploring the model
~~~~~~~~~~~~~~~~~~~
...
...
@@ -110,6 +150,12 @@ idx = [ idx for idx,name in enumerate(robot.model.names) if name=="RWristPitch"
placement = robot.data.oMi[idx]
----
Browse the list 'robot.model.names' to recognize the operation points needed in
the end of this page. Shortcuts are implemented for the right wrist (rh), left
wrist (lh), right ankle (rf) and left ankle (lf). You can get the placement
corresponding to the right hand with robot.Mrh(q) and the corresponding
Jacobian (6D, expressed in the body frame) with robot.Jrh.
Finally, the classical functions with Model and Data have been wrapped in some
python shortcuts, also available in RobotWrapper. The size of the robot
configuration is given by nq. The dimension of its tangent space (velocity) is
...
...
@@ -125,7 +171,6 @@ robot.com(q) # Compute the robot center of mass.
robot.position(q,23) # Compute the placement of joint 23
----
Launching RobotViewer
~~~~~~~~~~~~~~~~~~~~~
...
...
@@ -175,10 +220,21 @@ corresponding homogeneous matrix is given by M.homogeneous. A 3x3 rotation
matrix can be converted to a roll-pitch-yaw vector using the rpy script:
[source,python]
----
from pinocchio.rpy import matrixToRpy
M = robot.position(q,1)
rpy = matrixToRpy(M.rotation)
----
Using the matrixToRpy function, the SE3 placement of the body can be converted
to a 6-dimension vectors using the following se3ToPoseRPY function.
[source,python]
----
def se3ToPoseRpy(M):
'''Return a tuple of length 6 containing [X Y Z R P Y] corresponding to the input SE3 object.'''
