Commit 9fe5912d authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[python] update Class HumanoidRobot and Dynamic

parent ceb6b84c
from dynamic import Dynamic
from dynamic import Dynamic as DynamicOld
from angle_estimator import AngleEstimator
from zmp_from_forces import ZmpFromForces
import numpy as np
from numpy import arctan2, arcsin, sin, cos, sqrt
DynamicOld = Dynamic
#DynamicOld = Dynamic
class Dynamic (DynamicOld):
def __init__(self, name):
DynamicOld.__init__(self, name)
self.model = None
self.data = None
def setData(self, pinocchio_data):
dynamic.wrap.set_pinocchio_data(self.obj,pinocchio_data)
self.data = pinocchio_data
return
def setModel(self, pinocchio_model):
dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model)
self.model = pinocchio_model
return
def fromSotToPinocchio(q_sot, freeflyer=True):
......
......@@ -207,9 +207,9 @@ class AbstractHumanoidRobot (object):
# model.setProperty('ComputeMomentum', 'true')
def initializeOpPoints(self, model):
def initializeOpPoints(self):
for op in self.OperationalPoints:
model.createOpPoint(self.OperationalPointsMap[op], self.OperationalPointsMap[op])
self.dynamic.createOpPoint(self.OperationalPointsMap[op], self.OperationalPointsMap[op])
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
......@@ -263,7 +263,7 @@ class AbstractHumanoidRobot (object):
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
self.initializeOpPoints(self.dynamic)
self.initializeOpPoints()
#TODO: hand parameters through srdf --- additional frames ---
#self.frames = dict()
......@@ -376,22 +376,43 @@ class AbstractHumanoidRobot (object):
self.dynamic.signal(self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
self.dynamic.signal('J'+self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
from dynamic_graph.sot.dynamics import Dynamic
class HumanoidRobot(AbstractHumanoidRobot):
def __init__(self, name, pinocchio_model, pinocchio_data, initialConfig, OperationalPointsMap = None, tracer = None):
AbstractHumanoidRobot.__init__(self, name, tracer)
halfSitting = [] #FIXME
name = None
filename = None
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = OperationalPointsMap
def __init__(self, name, filename, tracer = None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.filename = filename
self.OperationalPointsMap ={'left-wrist' : 'left-wrist',
'right-wrist' : 'right-wrist',
'left-ankle' : 'left-ankle',
'right-ankle' : 'right-ankle',
'gaze' : 'gaze'}
self.dynamic = self.loadModelFromKxml (self.name + '_dynamics', self.filename)
self.dynamic = Dynamic(self.name + "_dynamic")
self.dynamic.setModel(pinocchio_model)
self.dynamic.setData(pinocchio_data)
self.dimension = self.dynamic.getDimension()
self.halfSitting = self.dimension*(0.,)
self.initializeRobot()
self.device = RobotSimu(self.name + "_device")
self.device.resize(self.dynamic.getDimension())
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
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