xm.py

Go to the documentation of this file.

#
# Copyright (c) 2016 CNRS
#
 
import numpy as np
import pinocchio as pin
from dcrba import DCRBA, DRNEA, Coriolis
from numpy.linalg import norm
from pinocchio.robot_wrapper import RobotWrapper
from pinocchio.utils import rand
 
np.random.seed(0)
 
robot = RobotWrapper(
    "/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf",
    [
        "/home/nmansard/src/pinocchio/pinocchio/models/romeo/",
    ],
    pin.JointModelFreeFlyer(),
)
q = rand(robot.model.nq)
q[3:7] /= norm(q[3:7])
vq = rand(robot.model.nv)
aq = rand(robot.model.nv)
 
# d/dq M(q)
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
 
# d/dvq RNEA(q,vq) = C(q,vq)
coriolis = Coriolis(robot)
C = coriolis(q, vq)
 
# d/dq RNEA(q,vq,aq)
drnea = DRNEA(robot)
aq = rand(robot.model.nv)
R = drnea(q, vq, aq)