Commit 9f28999a authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Add acceleromter and gyrometer as additional frames in Hrp2.

parent 191096ef
......@@ -27,6 +27,14 @@ from dynamic_graph import enableTrace, plug
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
# Internal helper tool.
def matrixToTuple(M):
tmp = M.tolist()
res = []
for i in tmp:
res.append(tuple(i))
return tuple(res)
class Hrp2(AbstractHumanoidRobot):
"""
This class instanciates a Hrp2 robot
......@@ -41,6 +49,20 @@ class Hrp2(AbstractHumanoidRobot):
(0.,0.,1.,-0.105),
(0.,0.,0.,1.))
accelerometerPosition = np.matrix ((
(1., 0., 0., -.13,),
(0., 1., 0., 0.,),
(0., 0., 1., .118,),
(0., 0., 0., 1.,),
))
gyrometerPosition = np.matrix ((
(1., 0., 0., -.13,),
(0., 1., 0., 0.,),
(0., 0., 1., .118,),
(0., 0., 0., 1.,),
))
def smallToFull(self, config):
res = (config + 10*(0.,))
return res
......@@ -57,6 +79,13 @@ class Hrp2(AbstractHumanoidRobot):
self.specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
self.jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
self.AdditionalFrames.append(
("accelerometer",
matrixToTuple(self.accelerometerPosition), "chest"))
self.AdditionalFrames.append(
("gyrometer",
matrixToTuple(self.gyrometerPosition), "chest"))
self.dynamic = self.loadModelFromJrlDynamics(
self.name + '_dynamic',
modelDir,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment