def writeFromMessage(self, msg):
t = msg.time
q = np.zeros(self._model.nq)
v = np.zeros(self._model.nv)
tau = np.zeros(self._model.njoints - 2)
p = dict()
pd = dict()
f = dict()
s = dict()
# Retrieve the generalized position and velocity, and joint torques
q[3] = msg.centroidal.base_orientation.x
q[4] = msg.centroidal.base_orientation.y
q[5] = msg.centroidal.base_orientation.z
q[6] = msg.centroidal.base_orientation.w
v[3] = msg.centroidal.base_angular_velocity.x
v[4] = msg.centroidal.base_angular_velocity.y
v[5] = msg.centroidal.base_angular_velocity.z
for j in range(len(msg.joints)):
jointId = self._model.getJointId(msg.joints[j].name) - 2
q[jointId + 7] = msg.joints[j].position
v[jointId + 6] = msg.joints[j].velocity
tau[jointId] = msg.joints[j].effort
pinocchio.centerOfMass(self._model, self._data, q, v)
q[0] = msg.centroidal.com_position.x - self._data.com[0][0]
q[1] = msg.centroidal.com_position.y - self._data.com[0][1]
q[2] = msg.centroidal.com_position.z - self._data.com[0][2]
v[0] = msg.centroidal.com_velocity.x - self._data.vcom[0][0]
v[1] = msg.centroidal.com_velocity.y - self._data.vcom[0][1]
v[2] = msg.centroidal.com_velocity.z - self._data.vcom[0][2]
# Retrive the contact information
for contact in msg.contacts:
name = contact.name
# Contact pose
pose = contact.pose
position = np.array(
[pose.position.x, pose.position.y, pose.position.z])
quaternion = pinocchio.Quaternion(pose.orientation.w,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z)
p[name] = pinocchio.SE3(quaternion, position)
# Contact velocity
velocity = contact.velocity
lin_vel = np.array(
[velocity.linear.x, velocity.linear.y, velocity.linear.z])
ang_vel = np.array(
[velocity.angular.x, velocity.angular.y, velocity.angular.z])
pd[name] = pinocchio.Motion(lin_vel, ang_vel)
# Contact wrench
wrench = contact.wrench
force = np.array([wrench.force.x, wrench.force.y, wrench.force.z])
torque = np.array(
[wrench.torque.x, wrench.torque.y, wrench.torque.z])
f[name] = [contact.type, pinocchio.Force(force, torque)]
# Surface normal and friction coefficient
normal = contact.surface_normal
nsurf = np.array([normal.x, normal.y, normal.z])
s[name] = [nsurf, contact.friction_coefficient]
return t, q, v, tau, p, pd, f, s
def test_subtree_masses(self):
pin.computeSubtreeMasses(self.model,self.data)
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
for i in range(self.model.njoints):
self.assertApprox(self.data.mass[i],data2.mass[i])
def test_subtree_masses(self):
pin.computeSubtreeMasses(self.model,self.data)
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
for i in range(self.model.njoints):
self.assertApprox(self.data.mass[i],data2.mass[i])
def calc(self, data, x, u=None):
self.assertImpactDataSet(data)
nq, nv = self.pinocchio.nq, self.pinocchio.nv
pinocchio.centerOfMass(self.pinocchio, data.pinocchio_dv, a2m(x[:nq]),
a2m(data.vnext - x[-nv:]))
data.residuals[:] = data.pinocchio_dv.vcom[0].flat
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
def test_com_0(self):
c0 = pin.centerOfMass(self.model,self.data,self.q)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q)
pin.centerOfMass(self.model,data2,0)
self.assertApprox(c0,data2.com[0])
self.assertApprox(self.data.com[0],data2.com[0])
def test_com_0(self):
c0 = pin.centerOfMass(self.model,self.data,self.q)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q)
pin.centerOfMass(self.model,data2,0)
self.assertApprox(c0,data2.com[0])
self.assertApprox(self.data.com[0],data2.com[0])
def test_com_2(self):
data = self.data
v = rand(self.model.nv)
a = rand(self.model.nv)
c0 = pin.centerOfMass(self.model, self.data, self.q, v, a)
c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, a, False)
self.assertApprox(c0, c0_bis)
data2 = self.model.createData()
pin.forwardKinematics(self.model, data, self.q, v, a)
c0 = pin.centerOfMass(self.model, data, pin.ACCELERATION)
pin.forwardKinematics(self.model, data2, self.q, v, a)
c0_bis = pin.centerOfMass(self.model, data2, pin.ACCELERATION, False)
self.assertApprox(c0, c0_bis)
c0_bis = pin.centerOfMass(self.model, data2, 2)
self.assertApprox(c0, c0_bis)
data3 = self.model.createData()
pin.centerOfMass(self.model, data3, self.q)
data4 = self.model.createData()
pin.centerOfMass(self.model, data4, self.q, v)
self.assertApprox(self.data.com[0], data2.com[0])
self.assertApprox(self.data.vcom[0], data2.vcom[0])
self.assertApprox(self.data.acom[0], data2.acom[0])
self.assertApprox(self.data.com[0], data3.com[0])
self.assertApprox(self.data.com[0], data4.com[0])
self.assertApprox(self.data.vcom[0], data4.vcom[0])
def com(self, q, v=None, a=None, update_kinematics=True):
if v is not None:
if a is None:
se3.centerOfMass(self.model, self.data, q,
v) #, update_kinematics)
return self.data.com[0], self.data.vcom[0]
se3.centerOfMass(self.model, self.data, q, v,
a) #, update_kinematics)
return self.data.com[0], self.data.vcom[0], self.data.acom[0]
return se3.centerOfMass(self.model, self.data,
q) #, update_kinematics)
def test_com_default(self):
v = rand(self.model.nv)
a = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v,a)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v,a)
pin.centerOfMass(self.model,data2)
for i in range(self.model.njoints):
self.assertApprox(self.data.com[i],data2.com[i])
self.assertApprox(self.data.vcom[i],data2.vcom[i])
self.assertApprox(self.data.acom[i],data2.acom[i])
def test_com_default(self):
v = rand(self.model.nv)
a = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v,a)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v,a)
pin.centerOfMass(self.model,data2)
for i in range(self.model.njoints):
self.assertApprox(self.data.com[i],data2.com[i])
self.assertApprox(self.data.vcom[i],data2.vcom[i])
self.assertApprox(self.data.acom[i],data2.acom[i])
def test_mass(self):
mass = pin.computeTotalMass(self.model)
self.assertIsNot(mass, np.nan)
mass_check = sum([inertia.mass for inertia in self.model.inertias[1:] ])
self.assertApprox(mass,mass_check)
mass_data = pin.computeTotalMass(self.model,self.data)
self.assertIsNot(mass_data, np.nan)
self.assertApprox(mass,mass_data)
self.assertApprox(mass_data,self.data.mass[0])
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
self.assertApprox(mass,data2.mass[0])
def test_mass(self):
mass = pin.computeTotalMass(self.model)
self.assertIsNot(mass, np.nan)
mass_check = sum([inertia.mass for inertia in self.model.inertias[1:] ])
self.assertApprox(mass,mass_check)
mass_data = pin.computeTotalMass(self.model,self.data)
self.assertIsNot(mass_data, np.nan)
self.assertApprox(mass,mass_data)
self.assertApprox(mass_data,self.data.mass[0])
data2 = self.model.createData()
pin.centerOfMass(self.model,data2,self.q)
self.assertApprox(mass,data2.mass[0])
def updateState(robot, q, v, sensor_data):
# Get dcm from current state
pin.forwardKinematics(robot.pinocchio_model_th, robot.pinocchio_data_th, q,
v)
comOut = pin.centerOfMass(robot.pinocchio_model_th,
robot.pinocchio_data_th, q, v)
vcomOut = robot.pinocchio_data_th.vcom[0]
dcmOut = comOut + vcomOut / omega
# Create zmp from forces
forces = np.asarray(sensor_data[ForceSensor.type])
newWrench = pin.Force.Zero()
for i, name in enumerate(contact_points):
update_frame(robot.pinocchio_model_th, robot.pinocchio_data_th, name)
placement = get_frame_placement(robot.pinocchio_model_th,
robot.pinocchio_data_th, name)
wrench = pin.Force(np.array([0.0, 0.0, forces[2, i]]), np.zeros(3))
newWrench += placement.act(wrench)
totalWrenchOut = newWrench
if totalWrenchOut.linear[2] > 0:
zmpOut = [
-totalWrenchOut.angular[1] / totalWrenchOut.linear[2],
totalWrenchOut.angular[0] / totalWrenchOut.linear[2]
]
else:
zmpOut = zmp_log
return comOut, vcomOut, dcmOut, zmpOut, totalWrenchOut
def createProblem(self, x0, comGoTo, timeStep, numKnots):
# Compute the current foot positions
q0 = a2m(x0[:self.rmodel.nq])
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = m2a(pinocchio.centerOfMass(self.rmodel, self.rdata, q0))
# Defining the action models along the time instances
comModels = []
# Creating the action model for the CoM task
comForwardModels = [
self.createModels(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comForwardTermModel = self.createModels(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + [comGoTo, 0., 0.])
comForwardTermModel.differential.costs['comTrack'].weight = 1e6
# Adding the CoM tasks
comModels += comForwardModels + [comForwardTermModel]
# Defining the shooting problem
problem = ShootingProblem(x0, comModels, comModels[-1])
return problem
def test_com_1(self):
data = self.data
v = rand(self.model.nv)
c0 = pin.centerOfMass(self.model, self.data, self.q, v)
c0_bis = pin.centerOfMass(self.model, self.data, self.q, v, False)
self.assertApprox(c0, c0_bis)
data2 = self.model.createData()
pin.forwardKinematics(self.model, data, self.q, v)
c0 = pin.centerOfMass(self.model, data, pin.VELOCITY)
pin.forwardKinematics(self.model, data2, self.q, v)
c0_bis = pin.centerOfMass(self.model, data2, pin.VELOCITY, False)
self.assertApprox(c0, c0_bis)
c0_bis = pin.centerOfMass(self.model, data2, 1)
self.assertApprox(c0, c0_bis)
data3 = self.model.createData()
pin.centerOfMass(self.model, data3, self.q)
self.assertApprox(self.data.com[0], data2.com[0])
self.assertApprox(self.data.vcom[0], data2.vcom[0])
self.assertApprox(self.data.com[0], data3.com[0])
def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots):
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
df = jumpLength[2] - rfFootPos0[2]
rfFootPos0[2] = 0.
rhFootPos0[2] = 0.
lfFootPos0[2] = 0.
lhFootPos0[2] = 0.
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
loco3dModel = []
takeOff = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(groundKnots)
]
flyingUpPhase = [
self.createSwingFootModel(
timeStep, [],
np.array([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]) * (k + 1) / flyingKnots + comRef)
for k in range(flyingKnots)
]
flyingDownPhase = []
for k in range(flyingKnots):
flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
f0 = jumpLength
footTask = [
crocoddyl.FramePlacement(self.lfFootId, pinocchio.SE3(np.eye(3), lfFootPos0 + f0)),
crocoddyl.FramePlacement(self.rfFootId, pinocchio.SE3(np.eye(3), rfFootPos0 + f0)),
crocoddyl.FramePlacement(self.lhFootId, pinocchio.SE3(np.eye(3), lhFootPos0 + f0)),
crocoddyl.FramePlacement(self.rhFootId, pinocchio.SE3(np.eye(3), rhFootPos0 + f0))
]
landingPhase = [
self.createFootSwitchModel([self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], footTask, False)
]
f0[2] = df
landed = [
self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
comTask=comRef + f0) for k in range(groundKnots)
]
loco3dModel += takeOff
loco3dModel += flyingUpPhase
loco3dModel += flyingDownPhase
loco3dModel += landingPhase
loco3dModel += landed
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep,
stepKnots, supportKnots):
""" Create a shooting problem for a simple walking gait.
:param x0: initial state
:param stepLength: step length
:param stepHeight: step height
:param timeStep: step time for each knot
:param stepKnots: number of knots for step phases
:param supportKnots: number of knots for double support phases
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.state.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfPos0 = self.rdata.oMf[self.rfId].translation
lfPos0 = self.rdata.oMf[self.lfId].translation
comRef = (rfPos0 + lfPos0) / 2
comRef[2] = np.asscalar(
pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
doubleSupport = [
self.createSwingFootModel(timeStep, [self.rfId, self.lfId])
for k in range(supportKnots)
]
# Creating the action models for three steps
if self.firstStep is True:
rStep = self.createFootstepModels(comRef, [rfPos0],
0.5 * stepLength, stepHeight,
timeStep, stepKnots, [self.lfId],
[self.rfId])
self.firstStep = False
else:
rStep = self.createFootstepModels(comRef, [rfPos0], stepLength,
stepHeight, timeStep, stepKnots,
[self.lfId], [self.rfId])
lStep = self.createFootstepModels(comRef, [lfPos0], stepLength,
stepHeight, timeStep, stepKnots,
[self.rfId], [self.lfId])
# We defined the problem as:
loco3dModel += doubleSupport + rStep
loco3dModel += doubleSupport + lStep
# Rescaling the terminal weights
costs = loco3dModel[-1].differential.costs.costs.todict()
for c in costs.values():
c.weight *= timeStep
problem = crocoddyl.ShootingProblem(x0, loco3dModel[:-1],
loco3dModel[-1])
return problem
def createCoMProblem(self, x0, comGoTo, timeStep, numKnots):
""" Create a shooting problem for a CoM forward/backward task.
:param x0: initial state
:param comGoTo: initial CoM motion
:param timeStep: step time for each knot
:param numKnots: number of knots per each phase
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
# lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
# rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
# lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
# rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
# Defining the action models along the time instances
comModels = []
# Creating the action model for the CoM task
comForwardModels = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comForwardTermModel = self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + np.matrix([comGoTo, 0., 0.]).T)
comForwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
comBackwardModels = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comBackwardTermModel = self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + np.matrix([-comGoTo, 0., 0.]).T)
comBackwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
# Adding the CoM tasks
comModels += comForwardModels + [comForwardTermModel]
comModels += comBackwardModels + [comBackwardTermModel]
# Defining the shooting problem
problem = crocoddyl.ShootingProblem(x0, comModels, comModels[-1])
return problem
def createPacingProblem(self, x0, stepLength, stepHeight, timeStep,
stepKnots, supportKnots):
""" Create a shooting problem for a simple pacing gait.
:param x0: initial state
:param stepLength: step length
:param stepHeight: step height
:param timeStep: step time for each knot
:param stepKnots: number of knots for step phases
:param supportKnots: number of knots for double support phases
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
comRef[2] = pinocchio.centerOfMass(self.rmodel, self.rdata,
q0)[2].item()
# Defining the action models along the time instances
loco3dModel = []
doubleSupport = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(supportKnots)
]
if self.firstStep is True:
rightSteps = self.createFootstepModels(
comRef, [rfFootPos0, rhFootPos0], 0.5 * stepLength, stepHeight,
timeStep, stepKnots, [self.lfFootId, self.lhFootId],
[self.rfFootId, self.rhFootId])
self.firstStep = False
else:
rightSteps = self.createFootstepModels(
comRef, [rfFootPos0, rhFootPos0], stepLength, stepHeight,
timeStep, stepKnots, [self.lfFootId, self.lhFootId],
[self.rfFootId, self.rhFootId])
leftSteps = self.createFootstepModels(comRef, [lfFootPos0, lhFootPos0],
stepLength, stepHeight, timeStep,
stepKnots,
[self.rfFootId, self.rhFootId],
[self.lfFootId, self.lhFootId])
loco3dModel += doubleSupport + rightSteps
loco3dModel += doubleSupport + leftSteps
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
def test_std_vector_field(self):
model = self.model
data = self.data
q = pin.neutral(model)
pin.centerOfMass(model, data, q)
com_list = data.com.tolist()
com = data.com[0]
with self.assertRaises(Exception) as context:
com = data.com[len(data.com) + 10]
print("com: ", com)
self.assertTrue('Index out of range' in str(context.exception))
with self.assertRaises(Exception) as context:
com = data.com['1']
print("com: ", com)
self.assertTrue('Invalid index type' in str(context.exception))
def init_viewer(enable_viewer):
# loadSolo(False) to load solo12
# loadSolo(True) to load solo8
solo = robots_loader.loadSolo(False)
if enable_viewer:
solo.initDisplay(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)
"""offset = np.zeros((19, 1))
offset[5, 0] = 0.7071067811865475
offset[6, 0] = 0.7071067811865475 - 1.0
temp = solo.q0 + offset"""
solo.display(solo.q0)
pin.centerOfMass(solo.model, solo.data, solo.q0, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
return solo
def test_com_0(self):
data = self.data
c0 = pin.centerOfMass(self.model, self.data, self.q)
c0_bis = pin.centerOfMass(self.model, self.data, self.q, False)
self.assertApprox(c0, c0_bis)
data2 = self.model.createData()
pin.forwardKinematics(self.model, data, self.q)
c0 = pin.centerOfMass(self.model, data, pin.POSITION)
pin.forwardKinematics(self.model, data2, self.q)
c0_bis = pin.centerOfMass(self.model, data2, pin.POSITION, False)
self.assertApprox(c0, c0_bis)
c0_bis = pin.centerOfMass(self.model, self.data, 0)
self.assertApprox(c0, c0_bis)
self.assertApprox(c0, data2.com[0])
self.assertApprox(self.data.com[0], data2.com[0])
def init_robot(q_init, enable_viewer):
"""Load the solo model and initialize the Gepetto viewer if it is enabled
Args:
q_init (array): the default position of the robot actuators
enable_viewer (bool): if the Gepetto viewer is enabled or not
"""
# Load robot model and data
# Initialisation of the Gepetto viewer
print(enable_viewer)
solo = load('solo12', display=enable_viewer)
q = solo.q0.reshape((-1, 1))
q[7:, 0] = q_init
"""if enable_viewer:
solo.initViewer(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)"""
if enable_viewer:
solo.display(q)
print("PASS")
# Initialisation of model quantities
pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
# Initialisation of the position of footsteps
fsteps_init = np.zeros((3, 4))
indexes = [10, 18, 26, 34]
for i in range(4):
fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
h_init = (solo.data.oMf[1].translation -
solo.data.oMf[indexes[0]].translation)[2]
fsteps_init[2, :] = 0.0
return solo, fsteps_init, h_init
def finiteDifferencesddA_dq(model, data, q, v, h):
'''
d(A_dot)/dq
'''
q0 = q.copy()
v0 = v.copy()
tensor_ddA_dq = np.zeros((6, model.nv, model.nv))
se3.forwardKinematics(model, data, q0, v0)
se3.computeJacobians(model, data, q0)
#se3.centerOfMass(model, data, q0)
pcom_ref = se3.centerOfMass(model, data, q0).copy()
vcom_ref = data.vcom[0].copy()
#se3.ccrba(model, data, q0, v0.copy())
se3.dccrba(model, data, q0, v0.copy())
dA0 = np.nan_to_num(data.dAg).copy()
oMc_ref = se3.SE3.Identity()
#oMc_ref.translation = pcom_ref
oMc_ref.translation = vcom_ref
for j in range(model.nv):
#vary q
vh = np.matrix(np.zeros((model.nv, 1)))
vh[j] = h
q_integrated = se3.integrate(model, q0.copy(), vh)
se3.forwardKinematics(model, data, q_integrated) #, v0.copy())
#se3.computeJacobians(model, data, q_integrated)
#se3.centerOfMass(model, data, q_integrated)
pcom_int = se3.centerOfMass(model, data, q_integrated).copy()
vcom_int = data.vcom[0].copy()
#se3.ccrba(model, data, q_integrated, v0.copy())
se3.dccrba(model, data, q_integrated, v0.copy())
dA0_int = np.nan_to_num(data.dAg).copy()
oMc_int = se3.SE3.Identity()
#oMc_int.translation = pcom_int
oMc_int.translation = vcom_int
cMc_int = oMc_ref.inverse() * oMc_int
dA0_int = cMc_int.dualAction * dA0_int
tensor_ddA_dq[:, :, j] = (dA0_int - dA0) / h
return tensor_ddA_dq
def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
""" Create a shooting problem for a simple walking gait.
:param x0: initial state
:param stepLength: step length
:param stepHeight: step height
:param timeStep: step time for each knot
:param stepKnots: number of knots for step phases
:param supportKnots: number of knots for double support phases
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rFrontFootPos0 = self.rdata.oMf[self.rfrontFootId].translation
rBackFootPos0 = self.rdata.oMf[self.rbackFootId].translation
lFrontFootPos0 = self.rdata.oMf[self.lfrontFootId].translation
lBackFootPos0 = self.rdata.oMf[self.lbackFootId].translation
comRef = (rFrontFootPos0 + rBackFootPos0 + lFrontFootPos0 + lBackFootPos0) / 4
comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
doubleSupport = [
self.createSwingFootModel(
timeStep,
[self.lfrontFootId, self.rfrontFootId, self.lbackFootId, self.rbackFootId],
) for k in range(supportKnots)
]
rbackStep = self.createFootstepModels(comRef, [rBackFootPos0], stepLength, stepHeight, timeStep, stepKnots,
[self.lfrontFootId, self.rfrontFootId, self.lbackFootId], [self.rbackFootId])
rfrontStep = self.createFootstepModels(comRef, [rFrontFootPos0], stepLength, stepHeight, timeStep, stepKnots,
[self.lfrontFootId, self.lbackFootId, self.rbackFootId], [self.rfrontFootId])
lbackStep = self.createFootstepModels(comRef, [lBackFootPos0], stepLength, stepHeight, timeStep, stepKnots,
[self.lfrontFootId, self.rfrontFootId, self.rbackFootId], [self.lbackFootId])
lfrontStep = self.createFootstepModels(comRef, [lFrontFootPos0], stepLength, stepHeight, timeStep, stepKnots,
[self.rfrontFootId, self.lbackFootId, self.rbackFootId], [self.lfrontFootId])
# Why do we need the double support? at leas for walking does not seem necessary, maybe for other gaits.
#loco3dModel += doubleSupport + rbackStep + rfrontStep
#loco3dModel += doubleSupport + lbackStep + lfrontStep
loco3dModel += rbackStep + rfrontStep
loco3dModel += lbackStep + lfrontStep
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel):
"""Compute the desired COM wrench (equation 1).
Args:
des_pos: desired center of mass position at time t
des_vel: desired center of mass velocity at time t
des_ori: desired base orientation at time t (quaternions)
des_angvel: desired base angular velocity at time t
Returns:
Computed w_com
"""
m = self.robot_mass
robot = self.robot
# com = np.reshape(np.array(q[0:3]), (3,))
com = pin.centerOfMass(robot.pin_robot.model, robot.pin_robot.data, q,
dq)
vcom = np.reshape(np.array(dq[0:3]), (3, ))
Ib = robot.pin_robot.mass(q)[3:6, 3:6]
quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori))
cur_angvel = arr(dq[3:6])
if self.rotate_vel_error:
# Rotate the des and current angular velocity into the world frame.
quat_des = pin.Quaternion(des_ori[3], des_ori[0], des_ori[1],
des_ori[2]).matrix()
des_angvel = quat_des.dot(des_angvel)
quat_cur = pin.Quaternion(q[6], q[3], q[4], q[5]).matrix()
cur_angvel = quat_cur.dot(cur_angvel)
w_com = np.hstack([
m * np.multiply(self.kc, des_pos - com) +
m * np.multiply(self.dc, des_vel - vcom),
arr(
arr(np.multiply(self.kb, quat_diff)) +
(Ib * mat(np.multiply(self.db, des_angvel - cur_angvel))).T)
])
# adding weight
# w_com[2] += m*9.81
return w_com
def test_com_1(self):
v = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v)
pin.centerOfMass(self.model,data2,1)
data3 = self.model.createData()
pin.centerOfMass(self.model,data3,self.q)
self.assertApprox(self.data.com[0],data2.com[0])
self.assertApprox(self.data.vcom[0],data2.vcom[0])
self.assertApprox(self.data.com[0],data3.com[0])
def test_com_1(self):
v = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v)
pin.centerOfMass(self.model,data2,1)
data3 = self.model.createData()
pin.centerOfMass(self.model,data3,self.q)
self.assertApprox(self.data.com[0],data2.com[0])
self.assertApprox(self.data.vcom[0],data2.vcom[0])
self.assertApprox(self.data.com[0],data3.com[0])
def test_com_2(self):
v = rand(self.model.nv)
a = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v,a)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v,a)
pin.centerOfMass(self.model,data2,2)
data3 = self.model.createData()
pin.centerOfMass(self.model,data3,self.q)
data4 = self.model.createData()
pin.centerOfMass(self.model,data4,self.q,v)
self.assertApprox(self.data.com[0],data2.com[0])
self.assertApprox(self.data.vcom[0],data2.vcom[0])
self.assertApprox(self.data.acom[0],data2.acom[0])
self.assertApprox(self.data.com[0],data3.com[0])
self.assertApprox(self.data.com[0],data4.com[0])
self.assertApprox(self.data.vcom[0],data4.vcom[0])
def test_com_2(self):
v = rand(self.model.nv)
a = rand(self.model.nv)
pin.centerOfMass(self.model,self.data,self.q,v,a)
data2 = self.model.createData()
pin.forwardKinematics(self.model,data2,self.q,v,a)
pin.centerOfMass(self.model,data2,2)
data3 = self.model.createData()
pin.centerOfMass(self.model,data3,self.q)
data4 = self.model.createData()
pin.centerOfMass(self.model,data4,self.q,v)
self.assertApprox(self.data.com[0],data2.com[0])
self.assertApprox(self.data.vcom[0],data2.vcom[0])
self.assertApprox(self.data.acom[0],data2.acom[0])
self.assertApprox(self.data.com[0],data3.com[0])
self.assertApprox(self.data.com[0],data4.com[0])
self.assertApprox(self.data.vcom[0],data4.vcom[0])
def test_staticRegressor(self):
model = pin.buildSampleModelHumanoidRandom()
data = model.createData()
data_ref = model.createData()
model.lowerPositionLimit[:7] = -1.
model.upperPositionLimit[:7] = 1.
q = pin.randomConfiguration(model)
pin.computeStaticRegressor(model, data, q)
phi = zero(4 * (model.njoints - 1))
for k in range(1, model.njoints):
Y = model.inertias[k]
phi[4 * (k - 1)] = Y.mass
phi[4 * k - 3:4 * k] = Y.mass * Y.lever
static_com_ref = pin.centerOfMass(model, data_ref, q)
static_com = data.staticRegressor * phi
self.assertApprox(static_com, static_com_ref)
def createCoMGoalProblem(self, x0, comGoTo, timeStep, numKnots):
""" Create a shooting problem for a CoM position goal task.
:param x0: initial state
:param comGoTo: CoM position change target
:param timeStep: step time for each knot
:param numKnots: number of knots per each phase
:return shooting problem
"""
# Compute the current foot positions
q0 = self.rmodel.referenceConfigurations["standing"]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
# Defining the action models along the time instances
comModels = []
# Creating the action model for the CoM task
comForwardModels = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comForwardTermModel = self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + np.array([comGoTo, 0., 0.]))
comForwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
# Adding the CoM tasks
comModels += comForwardModels + [comForwardTermModel]
# Defining the shooting problem
problem = crocoddyl.ShootingProblem(x0, comModels, comModels[-1])
return problem
def writeToMessage(self,
t,
q,
v=None,
tau=None,
p=dict(),
pd=dict(),
f=dict(),
s=dict()):
# Filling the time information
self._msg.header.stamp = rospy.Time(t)
self._msg.time = t
if v is None:
v = np.zeros(self._model.nv)
if tau is None:
tau = np.zeros(self._model.njoints - 2)
# Filling the centroidal state
pinocchio.centerOfMass(self._model, self._data, q, v)
c = self._data.com[0]
cd = self._data.vcom[0]
# Center of mass
self._msg.centroidal.com_position.x = c[0]
self._msg.centroidal.com_position.y = c[1]
self._msg.centroidal.com_position.z = c[2]
self._msg.centroidal.com_velocity.x = cd[0]
self._msg.centroidal.com_velocity.y = cd[1]
self._msg.centroidal.com_velocity.z = cd[2]
# Base
self._msg.centroidal.base_orientation.x = q[3]
self._msg.centroidal.base_orientation.y = q[4]
self._msg.centroidal.base_orientation.z = q[5]
self._msg.centroidal.base_orientation.w = q[6]
self._msg.centroidal.base_angular_velocity.x = v[3]
self._msg.centroidal.base_angular_velocity.y = v[4]
self._msg.centroidal.base_angular_velocity.z = v[5]
# Momenta
momenta = pinocchio.computeCentroidalMomentum(self._model, self._data)
momenta_rate = pinocchio.computeCentroidalMomentumTimeVariation(
self._model, self._data)
self._msg.centroidal.momenta.linear.x = momenta.linear[0]
self._msg.centroidal.momenta.linear.y = momenta.linear[1]
self._msg.centroidal.momenta.linear.z = momenta.linear[2]
self._msg.centroidal.momenta.angular.x = momenta.angular[0]
self._msg.centroidal.momenta.angular.y = momenta.angular[1]
self._msg.centroidal.momenta.angular.z = momenta.angular[2]
self._msg.centroidal.momenta_rate.linear.x = momenta_rate.linear[0]
self._msg.centroidal.momenta_rate.linear.y = momenta_rate.linear[1]
self._msg.centroidal.momenta_rate.linear.z = momenta_rate.linear[2]
self._msg.centroidal.momenta_rate.angular.x = momenta_rate.angular[0]
self._msg.centroidal.momenta_rate.angular.y = momenta_rate.angular[1]
self._msg.centroidal.momenta_rate.angular.z = momenta_rate.angular[2]
# Filling the joint state
njoints = self._model.njoints - 2
for j in range(njoints):
self._msg.joints[j].position = q[7 + j]
self._msg.joints[j].velocity = v[6 + j]
self._msg.joints[j].effort = tau[j]
# Filling the contact state
names = list(p.keys()) + list(pd.keys()) + list(f.keys()) + list(
s.keys())
names = list(dict.fromkeys(names))
self._msg.contacts = [None] * len(names)
if len(names) != 0 and (len(p.keys()) == 0 or len(pd.keys()) == 0):
pinocchio.forwardKinematics(self._model, self._data, q, v)
for i, name in enumerate(names):
contact_msg = ContactState()
contact_msg.name = name
frame_id = self._model.getFrameId(name)
# Retrive the contact position
if name in p:
pose = pinocchio.SE3ToXYZQUAT(p[name])
else:
oMf = pinocchio.updateFramePlacement(self._model, self._data,
frame_id)
pose = pinocchio.SE3ToXYZQUAT(oMf)
# Retrieve the contact velocity
if name in pd:
ovf = pd[name]
else:
ovf = pinocchio.getFrameVelocity(
self._model, self._data, frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# Storing the contact position and velocity inside the message
contact_msg.pose.position.x = pose[0]
contact_msg.pose.position.y = pose[1]
contact_msg.pose.position.z = pose[2]
contact_msg.pose.orientation.x = pose[3]
contact_msg.pose.orientation.y = pose[4]
contact_msg.pose.orientation.z = pose[5]
contact_msg.pose.orientation.w = pose[6]
contact_msg.velocity.linear.x = ovf.linear[0]
contact_msg.velocity.linear.y = ovf.linear[1]
contact_msg.velocity.linear.z = ovf.linear[2]
contact_msg.velocity.angular.x = ovf.angular[0]
contact_msg.velocity.angular.y = ovf.angular[1]
contact_msg.velocity.angular.z = ovf.angular[2]
# Retrieving and storing force data
if name in f:
contact_info = f[name]
ctype = contact_info[0]
force = contact_info[1]
contact_msg.type = ctype
contact_msg.wrench.force.x = force.linear[0]
contact_msg.wrench.force.y = force.linear[1]
contact_msg.wrench.force.z = force.linear[2]
contact_msg.wrench.torque.x = force.angular[0]
contact_msg.wrench.torque.y = force.angular[1]
contact_msg.wrench.torque.z = force.angular[2]
if name in s:
terrain_info = s[name]
norm = terrain_info[0]
friction = terrain_info[1]
contact_msg.surface_normal.x = norm[0]
contact_msg.surface_normal.y = norm[1]
contact_msg.surface_normal.z = norm[2]
contact_msg.friction_coefficient = friction
self._msg.contacts[i] = contact_msg
return copy.deepcopy(self._msg)
-0.005, # Right Arm
0.,
0. # Head
]
q = np.matrix(halfSitting).T
print("q:")
print(q.flatten().tolist()[0])
rospack = RosPack()
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data') + "/../"]
model = pin.buildModelFromUrdf(urdfPath, pin.JointModelFreeFlyer())
data = model.createData()
com = pin.centerOfMass(model, data, q)
pin.updateFramePlacements(model, data)
m = data.mass[0]
h = float(com[2])
g = 9.81
omega = sqrt(g / h)
leftName = param_server_conf.footFrameNames['Left']
leftId = model.getFrameId(leftName)
leftPos = data.oMf[leftId]
rightName = param_server_conf.footFrameNames['Right']
rightId = model.getFrameId(rightName)
rightPos = data.oMf[rightId]
centerTranslation = (data.oMf[rightId].translation +
def update_quantities(jiminy_model,
position,
velocity=None,
acceleration=None,
update_physics=True,
update_com=False,
update_energy=False,
update_jacobian=False,
use_theoretical_model=True):
"""
@brief Compute all quantities using position, velocity and acceleration
configurations.
@details Run multiple algorithms to compute all quantities
which can be known with the model position, velocity
and acceleration configuration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
(- center-of-mass jacobian : No Python binding available so far),
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
Computation results are stored in internal
data and can be retrieved with associated getters.
@note This function modifies internal data.
@param jiminy_model The Jiminy model
@param position Joint position vector
@param velocity Joint velocity vector
@param acceleration Joint acceleration vector
@pre model_.nq == positionIn.size()
@pre model_.nv == velocityIn.size()
@pre model_.nv == accelerationIn.size()
"""
if use_theoretical_model:
pnc_model = jiminy_model.pinocchio_model_th
pnc_data = jiminy_model.pinocchio_data_th
else:
pnc_model = jiminy_model.pinocchio_model
pnc_data = jiminy_model.pinocchio_data
if (update_physics and update_com and \
update_energy and update_jacobian and \
velocity is not None):
pnc.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if update_physics:
if velocity is not None:
pnc.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pnc.crba(pnc_model, pnc_data, position)
if update_jacobian:
# if update_com:
# pnc.getJacobianComFromCrba(pnc_model, pnc_data)
pnc.computeJointJacobians(pnc_model, pnc_data)
if update_com:
if velocity is None:
pnc.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity,
acceleration)
else:
if velocity is None:
pnc.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pnc.kineticEnergy(pnc_model, pnc_data, position, velocity,
False)
pnc.potentialEnergy(pnc_model, pnc_data, position, False)
def plotSolution(solver, bounds=True, figIndex=1, figTitle="", show=True):
import matplotlib.pyplot as plt
xs, us = [], []
if bounds:
us_lb, us_ub = [], []
xs_lb, xs_ub = [], []
if isinstance(solver, list):
rmodel = solver[0].problem.runningModels[0].state.pinocchio
for s in solver:
xs.extend(s.xs[:-1])
us.extend(s.us)
if bounds:
models = s.problem.runningModels + [s.problem.terminalModel]
for m in models:
us_lb += [m.u_lb]
us_ub += [m.u_ub]
xs_lb += [m.state.lb]
xs_ub += [m.state.ub]
else:
rmodel = solver.problem.runningModels[0].state.pinocchio
xs, us = solver.xs, solver.us
if bounds:
models = s.problem.runningModels + [s.problem.terminalModel]
for m in models:
us_lb += [m.u_lb]
us_ub += [m.u_ub]
xs_lb += [m.state.lb]
xs_ub += [m.state.ub]
# Getting the state and control trajectories
nx, nq, nu = xs[0].shape[0], rmodel.nq, us[0].shape[0]
X = [0.] * nx
U = [0.] * nu
if bounds:
U_LB = [0.] * nu
U_UB = [0.] * nu
X_LB = [0.] * nx
X_UB = [0.] * nx
for i in range(nx):
X[i] = [np.asscalar(x[i]) for x in xs]
if bounds:
X_LB[i] = [np.asscalar(x[i]) for x in xs_lb]
X_UB[i] = [np.asscalar(x[i]) for x in xs_ub]
for i in range(nu):
U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
if bounds:
U_LB[i] = [
np.asscalar(u[i]) if u.shape[0] != 0 else np.nan for u in us_lb
]
U_UB[i] = [
np.asscalar(u[i]) if u.shape[0] != 0 else np.nan for u in us_ub
]
# Plotting the joint positions, velocities and torques
plt.figure(figIndex)
plt.suptitle(figTitle)
legJointNames = ['1', '2', '3', '4', '5', '6']
# left foot
plt.subplot(2, 3, 1)
plt.title('joint position [rad]')
[
plt.plot(X[k], label=legJointNames[i])
for i, k in enumerate(range(7, 13))
]
if bounds:
[plt.plot(X_LB[k], '--r') for i, k in enumerate(range(7, 13))]
[plt.plot(X_UB[k], '--r') for i, k in enumerate(range(7, 13))]
plt.ylabel('LF')
plt.legend()
plt.subplot(2, 3, 2)
plt.title('joint velocity [rad/s]')
[
plt.plot(X[k], label=legJointNames[i])
for i, k in enumerate(range(nq + 6, nq + 12))
]
if bounds:
[
plt.plot(X_LB[k], '--r')
for i, k in enumerate(range(nq + 6, nq + 12))
]
[
plt.plot(X_UB[k], '--r')
for i, k in enumerate(range(nq + 6, nq + 12))
]
plt.ylabel('LF')
plt.legend()
plt.subplot(2, 3, 3)
plt.title('joint torque [Nm]')
[plt.plot(U[k], label=legJointNames[i]) for i, k in enumerate(range(0, 6))]
if bounds:
[plt.plot(U_LB[k], '--r') for i, k in enumerate(range(0, 6))]
[plt.plot(U_UB[k], '--r') for i, k in enumerate(range(0, 6))]
plt.ylabel('LF')
plt.legend()
# right foot
plt.subplot(2, 3, 4)
[
plt.plot(X[k], label=legJointNames[i])
for i, k in enumerate(range(13, 19))
]
if bounds:
[plt.plot(X_LB[k], '--r') for i, k in enumerate(range(13, 19))]
[plt.plot(X_UB[k], '--r') for i, k in enumerate(range(13, 19))]
plt.ylabel('RF')
plt.xlabel('knots')
plt.legend()
plt.subplot(2, 3, 5)
[
plt.plot(X[k], label=legJointNames[i])
for i, k in enumerate(range(nq + 12, nq + 18))
]
if bounds:
[
plt.plot(X_LB[k], '--r')
for i, k in enumerate(range(nq + 12, nq + 18))
]
[
plt.plot(X_UB[k], '--r')
for i, k in enumerate(range(nq + 12, nq + 18))
]
plt.ylabel('RF')
plt.xlabel('knots')
plt.legend()
plt.subplot(2, 3, 6)
[
plt.plot(U[k], label=legJointNames[i])
for i, k in enumerate(range(6, 12))
]
if bounds:
[plt.plot(U_LB[k], '--r') for i, k in enumerate(range(6, 12))]
[plt.plot(U_UB[k], '--r') for i, k in enumerate(range(6, 12))]
plt.ylabel('RF')
plt.xlabel('knots')
plt.legend()
plt.figure(figIndex + 1)
rdata = rmodel.createData()
Cx = []
Cy = []
for x in xs:
q = x[:rmodel.nq]
c = pinocchio.centerOfMass(rmodel, rdata, q)
Cx.append(np.asscalar(c[0]))
Cy.append(np.asscalar(c[1]))
plt.plot(Cx, Cy)
plt.title('CoM position')
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.grid(True)
if show:
plt.show()
import pinocchio as pin from pinocchio.romeo_wrapper import RomeoWrapper ## Load Romeo with RomeoWrapper import os current_path = os.getcwd() # The model of Romeo is contained in the path PINOCCHIO_GIT_REPOSITORY/models/romeo model_path = current_path + "/" + "../../models/romeo" mesh_dir = model_path urdf_filename = "romeo_small.urdf" urdf_model_path = model_path + "/romeo_description/urdf/" + urdf_filename robot = RomeoWrapper(urdf_model_path, [mesh_dir]) ## alias model = robot.model data = robot.data ## do whatever, e.g. compute the center of mass position in the world frame q0 = robot.q0 com = robot.com(q0) # This last command is similar to com2 = pin.centerOfMass(model,data,q0)
def calc_vc(q,vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel,rdata,q,vq)
return rdata.vcom[0]