def initWaistCoMTasks(robot):
# ---- TASKS -------------------------------------------------------------------
# Make sure that the CoM is not controlling the Z
robot.featureCom.selec.value='011'
# Build the reference waist pos h**o-matrix from PG.
# extract yaw
YawFromLeftRightRPY = Multiply_matrix_vector('YawFromLeftRightRPY')
YawFromLeftRightRPY.sin1.value=matrixToTuple(array([[ 0., 0., 0.], \
[ 0., 0., 0., ],
[ 0., 0., 1., ]]))
plug(robot.pg.signal('comattitude'),YawFromLeftRightRPY.sin2)
# Build a reference vector from init waist pos and
# init left foot roll pitch representation
waistReferenceVector = Stack_of_vector('waistReferenceVector')
plug(robot.pg.initwaistposref,waistReferenceVector.sin1)
#plug(robot.pg.initwaistattref,waistReferenceVector.sin2)
plug(YawFromLeftRightRPY.sout,waistReferenceVector.sin2)
waistReferenceVector.selec1(0,3)
waistReferenceVector.selec2(0,3)
robot.pg.waistReference=PoseRollPitchYawToMatrixHomo('waistReference')
# Controlling also the yaw.
robot.waist.selec.value = '111100'
robot.addTrace(robot.pg.waistReference.name,'sout')
robot.addTrace(robot.geom.name,'position')
robot.addTrace(robot.pg.name,'initwaistposref')
plug(waistReferenceVector.sout, robot.pg.waistReference.sin)
plug(robot.pg.waistReference.sout,robot.waist.reference)
robot.tasks ['waist'].controlGain.value = 200
def initPr2RosSimuProblem():
robot = Pr2('pr2', device=RobotSimu('pr2'))
plug(robot.device.state, robot.dynamic.position)
ros = Ros(robot)
sot = SotPr2(robot)
[robot,sot] = initializePr2(robot,sot)
return [robot,ros,sot]
def initChestTask(robot):
name = "chest"
dim = 1
index = 18
feature = FeatureGeneric('feature_'+name)
featureDes = FeatureGeneric('featureDes_'+name)
featureDes.errorIN.value=(0.2,)
feature.setReference('featureDes_'+name)
featureDes.errorIN.value = (0,) * dim;
robot.features[name] = feature
robot.features['Des'+name] = feature
# create jacobian.
jacobianGripper = eye(dim,robot.dimension) * 0;
jacobianGripper[0][index] = 1;
feature.jacobianIN.value = jacobianGripper
# only selec some dofs
selecFeatureChest = Selec_of_vector('selecfeature_'+name)
selecFeatureChest.selec(index, index+dim)
plug(robot.dynamic.position, selecFeatureChest.sin)
plug(selecFeatureChest.sout, feature.errorIN)
# 2\ Define the task. Associate to the task the position feature.
task = Task('task_'+name)
task.add('feature_'+name)
task.controlGain.value = 1
robot.tasks[name] = task
def test_ros(robot):
# SCRIPT PARAMETERS
j = 0; # index of joint under analysis
N = 300; # test duration (in number of timesteps)
dt = 0.001; # time step
estimationDelay = 10; # delay introduced by the estimation [number of time steps]
createRosTopics = 1; # 1=true, 0=false
# CONSTANTS
nj = 30; # number of joints
rad2deg = 180/3.14;
app = Application(robot);
dq_des = nj*(0.0,);
app.robot.device.control.value = dq_des;
if(createRosTopics==1):
ros = RosExport('rosExport');
ros.add('vector', 'robotStateRos', 'state');
plug(robot.device.state, ros.robotStateRos);
# robot.device.after.addSignal('rosExport.robotStateRos')
return ros
def __init__(self, name, signalPosition=None, signalJacobian = None,
referencePosition = None):
self._feature = FeaturePoint6d(name)
self.obj = self._feature.obj
self._reference = FeaturePoint6d(name + '_ref')
if referencePosition:
self._reference.signal('position').value = tuple(referencePosition)
if signalPosition:
plug(signalPosition, self._feature.signal('position'))
if signalJacobian:
plug(signalJacobian, self._feature.signal('Jq'))
self._feature.setReference(self._reference.name)
self._feature.signal('selec').value = '111111'
self._feature.frame('current')
# Signals stored in members
self.position = self._feature.signal('position')
self.reference = self._reference.signal('position')
self.velocity = self._reference.signal ('velocity')
self.Jq = self._feature.signal('Jq')
self.error = self._feature.signal('error')
self.errordot = self._feature.signal ('errordot')
self.selec = self._feature.signal('selec')
self.signalMap = {'position':self.position,
'reference':self.reference,
'Jq':self.Jq,
'error':self.error,
'selec':self.selec}
def start(self):
"""
Initialize the motion capture system.
Before calling this function, make sure:
a. evart-to-client is launched and successfully track
the body used to compute the error,
b. the robot is placed at its starting position.
"""
if len(self.corba.signals()) == 3:
print ("evart-to-client not launched, abandon.")
return False
if len(self.corba.signal(self.localizationPerceivedBody).value) != 3:
print ("waist not tracked, abandon.")
return False
self.sMm = self.computeWorldTransformationFromFoot()
print("World transformation:")
print(HomogeneousMatrixToXYZTheta(self.sMm))
self.errorEstimator.setSensorToWorldTransformation(self.sMm)
plug(self.corba.signal(self.localizationPerceivedBody),
self.errorEstimator.position)
plug(self.corba.signal(
self.localizationPerceivedBody + 'Timestamp'),
self.errorEstimator.positionTimestamp)
print ("Initial error:")
print (self.errorEstimator.error.value)
return True
def createTasks(robot):
# MetaTasks dictonary
robot.mTasks = dict()
robot.tasksIne = dict()
# Foot contacts
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(10)
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(10)
# MetaTasksKine6d for other operational points
robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist',
robot.OperationalPointsMap['waist'])
robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest',
robot.OperationalPointsMap['chest'])
robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['right-wrist'])
robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh',
robot.OperationalPointsMap['left-wrist'])
for taskName in robot.mTasks:
robot.mTasks[taskName].feature.frame('desired')
robot.mTasks[taskName].gain.setConstant(10)
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14)
robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip)
robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip)
# CoM Task
robot.mTasks['com'] = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value
robot.mTasks['com'].task.controlGain.value = 10
robot.mTasks['com'].feature.selec.value = '011'
# Posture Task
robot.mTasks['posture'] = MetaTaskKinePosture(robot.dynamic)
robot.mTasks['posture'].ref = robot.halfSitting
robot.mTasks['posture'].gain.setConstant(5)
## TASK INEQUALITY
# Task Height
featureHeight = FeatureGeneric('featureHeight')
plug(robot.dynamic.com,featureHeight.errorIN)
plug(robot.dynamic.Jcom,featureHeight.jacobianIN)
robot.tasksIne['taskHeight']=TaskInequality('taskHeight')
robot.tasksIne['taskHeight'].add(featureHeight.name)
robot.tasksIne['taskHeight'].selec.value = '100'
robot.tasksIne['taskHeight'].referenceInf.value = (0.,0.,0.) # Xmin, Ymin
robot.tasksIne['taskHeight'].referenceSup.value = (0.,0.,0.80771) # Xmax, Ymax
robot.tasksIne['taskHeight'].dt.value=robot.timeStep
def __init__(self, dyn, dt, name="lim"):
self.dyn = dyn
self.name = name
self.task = TaskDynLimits('task'+name)
plug(dyn.position, self.task.position)
plug(dyn.velocity, self.task.velocity)
self.task.dt.value = dt
self.task.controlGain.value = 10.0
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
self.task.referencePosInf.value = dyn.lowerJl.value
self.task.referencePosSup.value = dyn.upperJl.value
dqup = (0, 0, 0, 0, 0, 0,
200, 220, 250, 230, 290, 520,
200, 220, 250, 230, 290, 520,
250, 140, 390, 390,
240, 140, 240, 130, 270, 180, 330,
240, 140, 240, 130, 270, 180, 330)
dqup = (1000,)*len(dyn.upperJl.value)
self.task.referenceVelInf.value = tuple([-val*pi/180 for val in dqup])
self.task.referenceVelSup.value = tuple([ val*pi/180 for val in dqup])
def initPostureTask2(robot):
robot.features['featurePosition'] = FeatureGeneric('featurePosition')
robot.features['featurePositionDes'] = FeatureGeneric('featurePositionDes')
robot.features['featurePosition'].setReference('featurePositionDes')
plug(robot.dynamic.position,robot.features['featurePosition'].errorIN)
robot.features['featurePositionDes'].errorIN.value = robot.halfSitting
robot.features['featurePosition'].jacobianIN.value = totuple( identity(size(robot.dynamic.position.value)) )
robot.tasks['robot_task_position'] = Task('robot_task_position')
robot.tasks['robot_task_position'].add('featurePosition')
#gainPosition = GainAdaptive('gainPosition')
#gainPosition.set(0.1,0.1,125e3)
#gainPosition.gain.value = 5
#plug(robot.tasks['robot_task_position'].error,gainPosition.error)
#plug(gainPosition.gain,robot.tasks['robot_task_position'].controlGain)
robot.tasks['robot_task_position'].controlGain.value =2.
#TODO: this is ??? specific.
if robot.device.name == 'HRP2LAAS' or \
robot.device.name == 'HRP2JRL':
# reverse polish arms(1), head(1), chest(1), legs(0), waist(0)
robot.features['featurePosition'].selec.value = \
14*'1' + 2*'1' + 2*'1' + 12*'0' + 6*'0'
elif robot.device.name == 'HRP4LIRMM':
# reverse polish arms(1), head(1), chest(1), legs(0), waist(0)
robot.features['featurePosition'].selec.value = \
18*'1' + 2*'1' + 2*'1' + 12*'0' + 6*'0'
elif robot.device.name == 'ROMEO':
# reverse polish legs, arms, chest, waist
robot.features['featurePosition'].selec.value = \
14*'0' + 14*'1' + 5*'1' + 6*'0'
else:
robot.features['featurePosition'].selec.value = '1' * robot.dimension
def __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.gain = yamlData.get('gain', 1.)
self.name = yamlData['name']
self.reference = yamlData['reference']
self.task = Task('joint'+str(id(yamlData)))
self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
plug(motion.robot.device.state, self.feature.state)
jointId = self.nameToId[self.name]
posture = np.array(motion.robot.halfSitting)
posture[jointId] = self.reference
self.feature.setPosture(tuple(posture.tolist()))
for i in xrange(len(posture) - 6):
if i + 6 == jointId:
self.feature.selectDof(i + 6, True)
else:
self.feature.selectDof(i + 6, False)
self.task.add(self.feature.name)
self.task.controlGain.value = self.gain
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
(jointId,))
def setTaskGoal(robot, name, lower, upper, selec = '', gain = ()):
if name in robot.tasks and name in robot.features:
# Modification of the goal
if lower != [] and upper != []:
if lower == upper:
robot.features[name].reference.value = lower
else:
robot.features[name].reference.value = (0,) *len(lower)
robot.tasks[name].referenceInf.value = lower
robot.tasks[name].referenceSup.value = upper
# Fill the selec value
if selec != '':
robot.features[name].selec.value = selec
# If the gain vector is provided, fill it.
if name in robot.tasks:
if gain != []:
# damping factor for the gain
coeff = 1
if robot.tasks[name].className == 'TaskInequality':
coeff = robot.tasks[name].dt.value
if len(gain) == 1:
robot.tasks[name].controlGain.value = gain[0]*coeff
elif len(gain) == 3:
gain = GainAdaptive('gain_'+name)
gain.set(gain[0]*coeff,gain[1]*coeff,gain[2])
plug(robot.tasks[name].error, gain.error)
plug(gain.gain, robot.tasks[name].controlGain)
# otherwise do not modify the gain
else:
print "task " + name + " does not exists"
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not self.dynamic:
raise RunTimeError("robots models have to be initialized first")
if not self.device:
self.device = RobotSimu(self.name + "_device")
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
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.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.0,)
self.initializeOpPoints(self.dynamic)
# --- additional frames ---
self.frames = dict()
frameName = "rightHand"
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName), self.dynamic.getHandParameter(True), "right-wrist"
)
# rightGripper is an alias for the rightHand:
self.frames["rightGripper"] = self.frames[frameName]
frameName = "leftHand"
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName), self.dynamic.getHandParameter(False), "left-wrist"
)
# leftGripper is an alias for the leftHand:
self.frames["leftGripper"] = self.frames[frameName]
for (frameName, transformation, signalName) in self.AdditionalFrames:
self.frames[frameName] = self.createFrame(
"{0}_{1}".format(self.name, frameName), transformation, signalName
)
def main(dt=0.001, delay=0.01):
np.set_printoptions(precision=3, suppress=True);
dt = conf.dt;
q0 = conf.q0_urdf;
v0 = conf.v0;
nv = v0.shape[0];
simulator = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt, conf.model_path, conf.urdfFileName);
simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS;
simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS;
simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS;
simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
simulator.VIEWER_DT = conf.DT_VIEWER;
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3;
simulator.verb=0;
robot = simulator.r;
q0 = np.array(conf.q0_sot);
# q0[8] -= 1.0;
device = create_device(tuple(q0));
estimator = ForceTorqueEstimator("estimator");
plug(device.state, estimator.base6d_encoders);
estimator.accelerometer.value = (0.0, 0.0, 9.81);
estimator.gyroscope.value = (0.0, 0.0, 0.0);
estimator.ftSensRightFoot.value = 6*(0.0,);
estimator.ftSensLeftFoot.value = 6*(0.0,);
estimator.ftSensRightHand.value = 6*(0.0,);
estimator.ftSensLeftHand.value = 6*(0.0,);
estimator.currentMeasure.value = NJ*(0.0,);
estimator.dqRef.value = NJ*(0.0,);
estimator.ddqRef.value = NJ*(0.0,);
estimator.wCurrentTrust.value = tuple(NJ*[0.0,])
estimator.saturationCurrent.value = tuple(NJ*[5.0,])
estimator.motorParameterKt_p.value = tuple(Kt_p)
estimator.motorParameterKt_n.value = tuple(Kt_n)
estimator.motorParameterKf_p.value = tuple(Kf_p)
estimator.motorParameterKf_n.value = tuple(Kf_n)
estimator.motorParameterKv_p.value = tuple(Kv_p)
estimator.motorParameterKv_n.value = tuple(Kv_n)
estimator.motorParameterKa_p.value = tuple(Ka_p)
estimator.motorParameterKa_n.value = tuple(Ka_n)
estimator.init(dt,delay,delay,delay,delay,delay,True);
estimator.setUseRawEncoders(True);
estimator.setUseRefJointVel(True);
estimator.setUseRefJointAcc(True);
estimator.setUseFTsensors(False);
estimator.jointsTorques.recompute(0);
device.increment(dt);
q = np.matrix(device.state.value).T;
q_urdf = config_sot_to_urdf(q)
simulator.viewer.updateRobotConfig(q_urdf);
print "tau estimator=", np.array(estimator.jointsTorques.value);
print "tau pinocchio=", robot.bias(q_urdf, conf.v0)[6:,0].T;
return (simulator, estimator);
def initRobotGeom(robot):
robot.geom.createOpPoint('rf2',robot.OperationalPointsMap['right-ankle'])
robot.geom.createOpPoint('lf2',robot.OperationalPointsMap['left-ankle'])
plug(robot.dynamic.position,robot.geom.position)
robot.geom.ffposition.value = 6*(0,)
robotDim = len(robot.dynamic.velocity.value)
robot.geom.velocity.value = robotDim * (0,)
robot.geom.acceleration.value = robotDim * (0,)
def __init__(self, robot):
self.robot = robot
self.ros = RosExport("rosExportMocap")
self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf")
self.mocapFilter = MocapDataFilter("MocapDataFilter")
plug(self.ros.signal("chest"), self.mocapFilter.sin)
self.mocapSignal = self.mocapFilter.sout
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.dyn.velocity,self.task.qdot)
plug(self.task.error,self.gain.error)
plug(self.gain.gain,self.task.controlGain)
def createFrame(self, frameName, transformation, operationalPoint):
frame = OpPointModifier(frameName)
frame.setTransformation(transformation)
plug(self.dynamic.signal(operationalPoint), frame.positionIN)
plug(self.dynamic.signal("J{0}".format(operationalPoint)), frame.jacobianIN)
frame.position.recompute(frame.position.time + 1)
frame.jacobian.recompute(frame.jacobian.time + 1)
return frame
def createBalanceTask (robot, application, taskName, ingain = 1.):
task = Task (taskName)
task.add (application.featureCom.name)
task.add (application.leftAnkle.name)
task.add (application.rightAnkle.name)
gain = GainAdaptive('gain'+taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
def __init__(self,dyn,name="posture"):
self.dyn=dyn
self.name=name
self.feature = FeatureGeneric('feature'+name)
self.featureDes = FeatureGeneric('featureDes'+name)
self.gain = GainAdaptive('gain'+name)
plug(dyn.position,self.feature.errorIN)
robotDim = len(dyn.position.value)
self.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
self.feature.setReference(self.featureDes.name)
def __init__(self, robot):
self.robot = robot
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setNumberDofs(self.robot.dimension)
if robot.simu:
plug(self.sot.signal('control'), robot.simu.signal('control'))
plug(self.robot.simu.state,
self.robot.dynamic.position)
def createTrunkTask (self, robot, taskName, ingain=1.0):
task = Task (taskName)
task.add (self.features['chest'].name)
task.add (self.features['waist'].name)
task.add (self.features['gaze'].name)
gain = GainAdaptive(taskName+'gain')
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
def createRosExport(type, exportSignal, topic):
# get global ros instance
global exportcounter
exportcounter += 1
rosExportSignalIntern = 'rosExportIntern'+str(exportcounter)
ros.rosExport.add(type, rosExportSignalIntern, topic)
# cannot be initialized empty due to serialization errors
if (type == 'Vector'):
ros.rosExport.rosExportSignalIntern = (100000,)
plug(ros.rosExport.signal(rosExportSignalIntern), exportSignal)
def estimateCupFrameInHand(robot):
# consider the difference of position between the bottle and the hand.
# starting from that, define the robot (frame) corresponding to the top
# of the bottle.
topcupFrame = OpPointModifier('topcup')
#TODO find the correct Z, position of the topcup in the frame of the hand
topcupFrame.setTransformation(((1,0,0,0.0), (0,1,0,0.0), (0,0,1,-0.10), (0,0,0,1)))
plug(robot.frames['leftGripper'].position, topcupFrame.positionIN)
plug(robot.frames['leftGripper'].jacobian, topcupFrame.jacobianIN)
robot.frames['topcup'] = topcupFrame
def Pr2JointLimitsTask(robot,dt):
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
task = TaskJointLimits('taskJL')
plug(robot.dynamic.position,task.position)
task.controlGain.value = 10
task.referenceInf.value = robot.dynamic.lowerJl.value
task.referenceSup.value = robot.dynamic.upperJl.value
task.dt.value = dt
task.selec.value = toFlags(range(18,25)+range(26,27)+range(28,31)+range(32,40)+range(41,42)+range(43,46)+range(47,50))
return task
def plugComTask(self, taskCom):
plug(self.comRef.ref, taskCom.featureDes.errorIN)
plug(self.pg.dcomref, taskCom.featureDes.errordotIN)
taskCom.task = TaskPD("taskComPD")
taskCom.task.add(taskCom.feature.name)
# This next line is not very nice. The principle should be reported in Task.
plug(taskCom.feature.errordot, taskCom.task.errorDot)
plug(taskCom.task.error, taskCom.gain.error)
plug(taskCom.gain.gain, taskCom.task.controlGain)
taskCom.gain.setConstant(40)
taskCom.task.setBeta(-1)
def createTask(self,diag,gain,sampleInterval):
self.task = TaskJointWeights(self.name)
if(diag):
self.task.setWeights(diag)
else:
self.dyn.signal('inertia').recompute(0)
plug(self.dyn.signal('inertia'),self.task.signal('weights'))
#plug(self.dyn.position,self.task.velocity)
plug(self.robot.device.controlOut,self.task.velocity)
self.task.controlGain.value = gain
self.task.setSampleInterval(sampleInterval)
def createTask(self,selec,diag,gain,sampleInterval):
self.task = TaskJointWeights(self.name)
self.task.setSize(self.robot.dimension)
self.task.selec.value = selec
if(diag):
self.task.setWeights(diag)
else:
self.dyn.signal('inertia').recompute(0)
plug(self.dyn.signal('inertia'),self.task.signal('weights'))
self.task.controlGain.value = gain
self.task.setSampleInterval(sampleInterval)
def initializePr2(robot,sot):
robot.dynamic.velocity.value = robot.dimension*(0.,)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
robot.device.control.unplug()
plug(sot.control,robot.device.control)
return [robot,sot]
def initFeetTask(robot):
robot.selecFeet = Selector('selecFeet',
['matrixHomo','leftfootref', \
robot.dynamic.signal(robot.OperationalPointsMap['left-ankle']),\
robot.pg.leftfootref], \
['matrixHomo','rightfootref', \
robot.dynamic.signal(robot.OperationalPointsMap['right-ankle']), \
robot.pg.rightfootref])
plug(robot.pg.inprocess,robot.selecFeet.selec)
robot.tasks['right-ankle'].controlGain.value = 200
robot.tasks['left-ankle'].controlGain.value = 200
def estimateBottleFrameInHand(robot):
# consider the difference of position between the bottle and the hand.
# starting from that, define the robot (frame) corresponding to the top
# of the bottle.
bungFrame = OpPointModifier('bung')
#TODO find the correct Z, position of the bung in the frame of the hand
bungFrame.setTransformation(((1,0,0,0.0), (0,1,0,0.0), (0,0,1, 0.10), (0,0,0,1)))
plug(robot.frames['rightGripper'].position, bungFrame.positionIN)
plug(robot.frames['rightGripper'].jacobian, bungFrame.jacobianIN)
# frame.position.recompute(bungFrame.position.time + 1)
# frame.jacobian.recompute(bungFrame.jacobian.time + 1)
robot.frames['bung'] = bungFrame
def create_base_estimator(robot, dt, conf, robot_name="robot"):
from dynamic_graph.sot.torque_control.base_estimator import BaseEstimator
base_estimator = BaseEstimator('base_estimator');
plug(robot.encoders.sout, base_estimator.joint_positions);
plug(robot.device.forceRLEG, base_estimator.forceRLEG);
plug(robot.device.forceLLEG, base_estimator.forceLLEG);
plug(robot.estimator_kin.dx, base_estimator.joint_velocities);
plug(robot.imu_filter.imu_quat, base_estimator.imu_quaternion);
plug(robot.imu_offset_compensation.accelerometer_out, base_estimator.accelerometer);
base_estimator.K_fb_feet_poses.value = conf.K_fb_feet_poses;
base_estimator.set_imu_weight(conf.w_imu);
base_estimator.set_stiffness_right_foot(conf.K);
base_estimator.set_stiffness_left_foot(conf.K);
base_estimator.set_zmp_std_dev_right_foot(conf.std_dev_zmp)
base_estimator.set_zmp_std_dev_left_foot(conf.std_dev_zmp)
base_estimator.set_normal_force_std_dev_right_foot(conf.std_dev_fz)
base_estimator.set_normal_force_std_dev_left_foot(conf.std_dev_fz)
base_estimator.set_zmp_margin_right_foot(conf.zmp_margin)
base_estimator.set_zmp_margin_left_foot(conf.zmp_margin)
base_estimator.set_normal_force_margin_right_foot(conf.normal_force_margin)
base_estimator.set_normal_force_margin_left_foot(conf.normal_force_margin)
base_estimator.set_right_foot_sizes(conf.RIGHT_FOOT_SIZES)
base_estimator.set_left_foot_sizes(conf.LEFT_FOOT_SIZES)
base_estimator.init(dt, robot_name);
return base_estimator;
def create_admittance_ctrl(robot, dt=0.001):
admit_ctrl = AdmittanceController("adm_ctrl");
plug(robot.device.robotState, admit_ctrl.base6d_encoders);
plug(robot.estimator_kin.dx, admit_ctrl.jointsVelocities);
plug(robot.estimator_ft.contactWrenchRightSole, admit_ctrl.fRightFoot);
plug(robot.estimator_ft.contactWrenchLeftSole, admit_ctrl.fLeftFoot);
plug(robot.estimator_ft.contactWrenchRightHand, admit_ctrl.fRightHand);
plug(robot.estimator_ft.contactWrenchLeftHand, admit_ctrl.fLeftHand);
plug(robot.traj_gen.fRightFoot, admit_ctrl.fRightFootRef);
plug(robot.traj_gen.fLeftFoot, admit_ctrl.fLeftFootRef);
plug(robot.traj_gen.fRightHand, admit_ctrl.fRightHandRef);
plug(robot.traj_gen.fLeftHand, admit_ctrl.fLeftHandRef);
admit_ctrl.damping.value = 4*(0.05,);
admit_ctrl.Kd.value = NJ*(0,);
kf = -0.0005;
km = -0.008;
admit_ctrl.Kf.value = 3*(kf,)+3*(km,)+3*(kf,)+3*(km,)+3*(kf,)+3*(km,)+3*(kf,)+3*(km,);
robot.ctrl_manager.addCtrlMode("adm");
plug(admit_ctrl.qDes, robot.ctrl_manager.ctrl_adm);
plug(robot.ctrl_manager.joints_ctrl_mode_adm, admit_ctrl.controlledJoints);
admit_ctrl.init(dt);
return admit_ctrl;
def create_topic(ros_import, signal, name, data_type='vector', sleep_time=0.1):
ros_import.add(data_type, name+'_ros', name);
plug(signal, ros_import.signal(name+'_ros'));
from time import sleep
sleep(sleep_time);
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g / h)
# -------------------------- ESTIMATION --------------------------
# --- Base Estimation
robot.param_server = create_parameter_server(param_server_conf, dt)
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
# robot.be_filters = create_be_filters(robot, dt)
# --- Conversion
e2q = EulerToQuat('e2q')
plug(robot.base_estimator.q, e2q.euler)
robot.e2q = e2q
# --- Kinematic computations
robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData())
plug(robot.base_estimator.q, robot.rdynamic.position)
robot.rdynamic.velocity.value = [0.0] * robotDim
robot.rdynamic.acceleration.value = [0.0] * robotDim
# --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator')
cdc_estimator.init(dt, robot_name)
plug(robot.e2q.quaternion, cdc_estimator.q)
plug(robot.base_estimator.v, cdc_estimator.v)
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()
dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
dyn.upperJl.value = vectorToTuple(ulimit)
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
#-----------------------------------------------------------------------------
def __init__(self,
name,
basePosition=None,
otherPosition=None,
baseReference=None,
otherReference=None,
JqBase=None,
JqOther=None):
self._feature = FeaturePoint6dRelative(name)
self.obj = self._feature.obj
self._reference = FeaturePoint6dRelative(name + '_ref')
# Set undefined input parameters as identity matrix
if basePosition is None:
basePosition = I4
if otherPosition is None:
otherPosition = I4
if baseReference is None:
baseReference = I4
if otherReference is None:
otherReference = I4
# If input positions are signals, plug them, otherwise set values
for (sout, sin) in \
((basePosition, self._feature.signal('positionRef')),
(otherPosition, self._feature.signal('position')),
(baseReference, self._reference.signal('positionRef')),
(otherReference, self._reference.signal('position'))):
if isinstance(sout, SignalBase):
plug(sout, sin)
else:
sin.value = sout
if JqBase:
plug(JqBase, self._feature.signal('JqRef'))
if JqOther:
plug(JqOther, self._feature.signal('Jq'))
self._feature.setReference(self._reference.name)
self._feature.signal('selec').value = Flags('111111')
self._feature.frame('current')
# Signals stored in members
self.basePosition = self._feature.signal('positionRef')
self.otherPosition = self._feature.signal('position')
self.baseReference = self._reference.signal('positionRef')
self.otherReference = self._reference.signal('position')
self.JqBase = self._feature.signal('JqRef')
self.JqOther = self._feature.signal('Jq')
self.error = self._feature.signal('error')
self.jacobian = self._feature.signal('jacobian')
self.selec = self._feature.signal('selec')
self.signalMap = {
'basePosition': self.basePosition,
'otherPosition': self.otherPosition,
'baseReference': self.baseReference,
'otherReference': self.otherReference,
'JqBase': self.JqBase,
'JqOther': self.JqOther,
'error': self.error,
'jacobian': self.jacobian,
'selec': self.selec
}
@optionalparentheses
def dump():
history.dumpToOpenHRP(options.output)
# --- DYN ----------------------------------------------------------------------
dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state, dyn.position)
dyn.velocity.value = robotDim * (0., )
dyn.acceleration.value = robotDim * (0., )
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
robot.control.unplug()
# --- META TASKS AND OP POINT ------------------------------------------------
taskRF = MetaTask6d('rf', dyn, 'rf', 'right-ankle')
taskLF = MetaTask6d('lf', dyn, 'lf', 'left-ankle')
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import PatternGenerator, Selector
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
robot.dynamic = Dynamic("dyn")
robot.dynamic.setFiles(modelDir, modelName[robotName], specificitiesPath,
jointRankPath)
robot.dynamic.parse()
robot.dynamic.inertiaRotor.value = inertiaRotor[robotName]
robot.dynamic.gearRatio.value = gearRatio[robotName]
plug(robot.device.state, robot.dynamic.position)
robot.dynamic.velocity.value = robot.dimension * (0., )
robot.dynamic.acceleration.value = robot.dimension * (0., )
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics', 'true')
robot.dynamic.setProperty('ComputeAccelerationCoM', 'true')
robot.device.control.unplug()
# ------------------------------------------------------------------------------
# --- SOLVER ----------------------------------------------------------------
# ------------------------------------------------------------------------------
taskCom.feature.selec.value = '011'
# --- POSTURE
robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q
robot.taskUpperBody.feature.selectDof(27, True)
robot.taskUpperBody.feature.selectDof(35, True)
robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
# --- CONTACTS (define contactLF and contactRF)
contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
locals()['contactRF'] = contactRF
# --- DYN ----------------------------------------------------------------------
# --- DYN ----------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state,dyn.position)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
#dyn.setProperty('ComputeBackwardDynamics','true')
#dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# --- Task Dyn -----------------------------------------
class MetaTaskKine6d( MetaTask6d ):
def createTask(self):
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data,
initialConfig, OperationalPointsMap)
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
projection = HolonomicProjection("projection")
projection.setSize(robot.dynamic.getDimension())
projection.setLeftWheel(6)
projection.setRightWheel(7)
# The wheel separation could be obtained with pinocchio.
# See pmb2_description/urdf/base.urdf.xacro
projection.setWheelRadius(0.0985)
projection.setWheelSeparation(0.4044)
plug(robot.dynamic.mobilebase, projection.basePose)
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(projection.projection, sot.proj0)
plug(sot.control, robot.device.control)
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
target = (0.5, -0.2, 1.0)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state, dyn.position)
dyn.velocity.value = robotDim * (0., )
dyn.acceleration.value = robotDim * (0., )
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics', 'true')
dyn.setProperty('ComputeAccelerationCoM', 'true')
robot.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
def __init__(self, dyn):
self.pg = PatternGenerator('pg')
modelDir = dyn.getProperty('vrmlDirectory')
modelName = dyn.getProperty('vrmlMainFile')
specificitiesPath = dyn.getProperty('xmlSpecificityFile')
jointRankPath = dyn.getProperty('xmlRankFile')
robotDim = len(dyn.position.value)
# print(modelDir,modelName,specificitiesPath,jointRankPath,robotDim)
self.pg.setVrmlDir(modelDir + '/')
self.pg.setVrml(modelName)
self.pg.setXmlSpec(specificitiesPath)
self.pg.setXmlRank(jointRankPath)
self.pg.buildModel()
# Standard initialization
self.pg.parseCmd(":samplingperiod 0.005")
self.pg.parseCmd(":previewcontroltime 1.6")
self.pg.parseCmd(":comheight 0.814")
self.pg.parseCmd(":omega 0.0")
self.pg.parseCmd(":stepheight 0.05")
self.pg.parseCmd(":singlesupporttime 0.780")
self.pg.parseCmd(":doublesupporttime 0.020")
self.pg.parseCmd(":armparameters 0.5")
self.pg.parseCmd(":LimitsFeasibility 0.0")
self.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
self.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
self.pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
self.pg.parseCmd(":comheight 0.814")
self.pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
plug(dyn.position, self.pg.position)
plug(dyn.com, self.pg.com)
self.pg.motorcontrol.value = robotDim * (0, )
self.pg.zmppreviouscontroller.value = (0, 0, 0)
self.pg.initState()
# --- PG INIT FRAMES ---
self.geom = Dynamic("geom")
self.geom.setFiles(modelDir, modelName, specificitiesPath,
jointRankPath)
self.geom.parse()
self.geom.createOpPoint('rf', 'right-ankle')
self.geom.createOpPoint('lf', 'left-ankle')
plug(dyn.position, self.geom.position)
self.geom.ffposition.value = 6 * (0, )
self.geom.velocity.value = robotDim * (0, )
self.geom.acceleration.value = robotDim * (0, )
# --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
self.comRef = Selector('comRef',
['vector', 'ref', dyn.com, self.pg.comref])
plug(self.pg.inprocess, self.comRef.selec)
self.selecSupportFoot = Selector('selecSupportFoot', [
'matrixHomo', 'pg_H_sf', self.pg.rightfootref, self.pg.leftfootref
], ['matrixHomo', 'wa_H_sf', self.geom.rf, self.geom.lf])
plug(self.pg.SupportFoot, self.selecSupportFoot.selec)
self.sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(self.selecSupportFoot.wa_H_sf, self.sf_H_wa.sin)
self.pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(self.selecSupportFoot.pg_H_sf, self.pg_H_wa.sin1)
plug(self.sf_H_wa.sout, self.pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
self.wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
plug(self.pg_H_wa.sout, self.wa_H_pg.sin)
self.wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
plug(self.wa_H_pg.sout, self.wa_zmp.sin1)
plug(self.pg.zmpref, self.wa_zmp.sin2)
# --- Build the converter object for the waist task
self.waistReferenceVector = Stack_of_vector('waistReferenceVector')
plug(self.pg.initwaistposref, self.waistReferenceVector.sin1)
# plug(self.pg.initwaistattref,self.waistReferenceVector.sin2)
plug(self.pg.comattitude, self.waistReferenceVector.sin2)
self.waistReferenceVector.selec1(0, 3)
self.waistReferenceVector.selec2(0, 3)
self.waistReference = PoseRollPitchYawToMatrixHomo('waistReference')
plug(self.waistReferenceVector.sout, self.waistReference.sin)
folder = rospack.get_path('sot-talos-balance') + "/data/" + test_folder
else:
folder = test_folder
if folder[-1] != '/':
folder += '/'
# --- Trajectory generators
# --- General trigger
robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
robot.triggerTrajGen.sin.value = 0
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
robot.comTrajGen.x.recompute(0) # trigger computation of initial value
plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
def plugWaistTask(self, taskWaist):
plug(self.waistReference.sout, taskWaist.featureDes.position)
taskWaist.feature.selec.value = '111100'
print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension
# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)
# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine
def toList(solver):
return map(lambda x: x[1:-1], solver.dispStack().split('|')[1:])
SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1
robot.device.control.unplug()
plug(solver.control, robot.device.control)
# Close the control loop
plug(robot.device.state, robot.dynamic.position)
print("...done!")
# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
robot.com_traj_gen = create_com_traj_gen(robot, dt)
robot.com_traj_gen.x.recompute(0)
# --- Base orientation (SE3 on the waist) trajectory
robot.waist_traj_gen = create_waist_traj_gen("tg_waist_ref", robot, dt)
robot.waist_traj_gen.x.recompute(0)
# --- Simple inverse dynamic controller
robot.inv_dyn = create_simple_inverse_dyn_controller(robot, conf.balance_ctrl, dt)
robot.inv_dyn.setControlOutputType("torque")
# --- Connect control manager
connect_ctrl_manager(robot)
# --- Error on the CoM task
robot.errorComTSID = Substract_of_vector('error_com')
plug(robot.inv_dyn.com_ref_pos, robot.errorComTSID.sin2)
plug(robot.dynamic.com, robot.errorComTSID.sin1)
# --- Error on the Posture task
robot.errorPoseTSID = Substract_of_vector('error_pose')
plug(robot.inv_dyn.posture_ref_pos, robot.errorPoseTSID.sin2)
plug(robot.encoders.sout, robot.errorPoseTSID.sin1)
# # # --- ROS PUBLISHER ----------------------------------------------------------
robot.publisher = create_rospublish(robot, 'robot_publisher')
create_topic(robot.publisher, robot.errorPoseTSID, 'sout', 'errorPoseTSID', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.errorComTSID, 'sout', 'errorComTSID', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.dynamic, 'com', 'dynCom', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.inv_dyn, 'q_des', 'q_des', robot=robot, data_type='vector')
def plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error,self.gain.error)
plug(self.gain.gain,self.task.controlGain)
folder = rospack.get_path('sot_talos_balance') + "/data/" + test_folder
else:
folder = test_folder
if folder[-1] != '/':
folder += '/'
# --- Trajectory generators
# --- General trigger
robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
robot.triggerTrajGen.sin.value = 0
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
robot.comTrajGen.x.recompute(0) # trigger computation of initial value
plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# modify file /opt/ros/indigo/lib/python2.7/dist-packages/rqt_py_console/spyder_console_widget.py (moduleCompletion -> module_completion) # file to modify joint bounds: # nano ~/devel/hrp2-n14-system/src/direct-access/hrp2-io-boards-init.cpp from dynamic_graph.sot.torque_control.main import * from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal, stop_sot from dynamic_graph import plug robot.timeStep=0.0015 robot = main_v3(robot, startSoT=True, go_half_sitting=False) go_to_position(robot.traj_gen, 30*(0.0,), 5.0) robot.base_estimator.set_imu_weight(0.0) plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities); #plug(robot.filters.estimator_kin.dx, robot.current_ctrl.dq); #plug(robot.filters.estimator_kin.dx, robot.torque_ctrl.jointsVelocities); robot.inv_dyn.kp_com.value = (30.0, 30.0, 50.0) # create ros topics create_topic(robot.ros, robot.device.robotState, 'robotState') create_topic(robot.ros, robot.estimator_ft.jointsTorques, 'tau'); create_topic(robot.ros, robot.torque_ctrl.jointsTorquesDesired, 'tau_des'); create_topic(robot.ros, robot.inv_dyn.com, 'com') create_topic(robot.ros, robot.inv_dyn.com_vel, 'com_vel') create_topic(robot.ros, robot.inv_dyn.com_ref_pos, 'com_ref_pos') create_topic(robot.ros, robot.inv_dyn.com_ref_vel, 'com_ref_vel') create_topic(robot.ros, robot.inv_dyn.com_acc_des, 'com_acc_des') create_topic(robot.ros, robot.base_estimator.lf_xyzquat, 'lf_est') create_topic(robot.ros, robot.base_estimator.rf_xyzquat, 'rf_est') create_topic(robot.ros, robot.current_ctrl.i_real, 'i_real'); create_topic(robot.ros, robot.ctrl_manager.u, 'i_des')
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Trajectory generators
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
# robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
# robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(
dt, robot, 'WT')
# robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
dt = robot.timeStep
# -------------------------- DESIRED TRAJECTORY --------------------------
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# -------------------------- ESTIMATION --------------------------
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf, dt)
# --- filters
filters = Bunch()
filters.ft_RF_filter = create_butter_lp_filter_Wn_04_N_2("ft_RF_filter", dt, 6)
filters.ft_LF_filter = create_butter_lp_filter_Wn_04_N_2("ft_LF_filter", dt, 6)
plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
robot.device_filters = filters
# --- Force calibration
robot.ftc = create_ft_calibrator(robot, ft_conf)
# --- ZMP estimation (disconnected)
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.dynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.dynamic.createOpPoint('sole_RF', 'right_sole_link')
plug(robot.dynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.dynamic.sole_RF, zmp_estimator.poseRight)
plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
zmp_estimator.init()
omega = sqrt(g / h)
# -------------------------- ESTIMATION --------------------------
# --- REAL ESTIMATION - diagnostics only
# --- Base Estimation
robot.param_server = create_parameter_server(param_server_conf, dt)
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
# robot.be_filters = create_be_filters(robot, dt)
# --- Conversion
e2q = EulerToQuat('e2q')
plug(robot.base_estimator.q, e2q.euler)
robot.e2q = e2q
# --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator')
cdc_estimator.init(dt, robot_name)
plug(robot.e2q.quaternion, cdc_estimator.q)
plug(robot.base_estimator.v, cdc_estimator.v)
robot.cdc_estimator = cdc_estimator
# --- SOT ESTIMATION
# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega
estimator.mass.value = mass
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Trajectory generators
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
# robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
# robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(
dt, robot, 'WT')
# robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
plug(self.dyn.signal(self.opPointBase),
self.feature.signal('positionRef'))
plug(self.dyn.signal('J' + self.opPointBase),
self.feature.signal('JqRef'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def connect_ctrl_manager(ent):
# connect to device
plug(ent.device.robotState, ent.ctrl_manager.base6d_encoders);
plug(ent.device.currents, ent.ctrl_manager.currents);
plug(ent.estimator_kin.dx, ent.ctrl_manager.dq);
plug(ent.estimator_ft.jointsTorques, ent.ctrl_manager.tau);
plug(ent.ctrl_manager.pwmDes, ent.torque_ctrl.pwm);
ent.ctrl_manager.addCtrlMode("pos");
ent.ctrl_manager.addCtrlMode("torque");
plug(ent.torque_ctrl.controlCurrent, ent.ctrl_manager.ctrl_torque);
plug(ent.pos_ctrl.pwmDes, ent.ctrl_manager.ctrl_pos);
plug(ent.ctrl_manager.joints_ctrl_mode_torque, ent.inv_dyn.active_joints);
ent.ctrl_manager.setCtrlMode("all", "pos");
plug(ent.ctrl_manager.pwmDesSafe, ent.device.control);
return;
planeversor = FeatureAngleBtwPlaneAndVersor('planeversor')
pointpoint = FeaturePointToPoint('pointpoint')
planeversor.displaySignals()
# Define a task
taskExpg = Task('taskExpg')
#define the gain
taskExpg.controlGain.value = 1
# operational point used
opPoint1 = 'left-wrist'
opPoint2 = 'right-wrist'
# Get the positions/Jacobians of the operational points for the dynamic entity
plug(robot.dynamic.signal(opPoint1),planeversor.signal('w_T_o1'))
plug(robot.dynamic.signal('J'+opPoint1),planeversor.signal('w_J_o1'))
plug(robot.dynamic.signal(opPoint2),planeversor.signal('w_T_o2'))
plug(robot.dynamic.signal('J'+opPoint2),planeversor.signal('w_J_o2'))
plug(robot.dynamic.signal(opPoint1),pointpoint.signal('w_T_o1'))
plug(robot.dynamic.signal('J'+opPoint1),pointpoint.signal('w_J_o1'))
plug(robot.dynamic.signal(opPoint2),pointpoint.signal('w_T_o2'))
plug(robot.dynamic.signal('J'+opPoint2),pointpoint.signal('w_J_o2'))
# Associate the feature to the task:
taskExpg.add(pointpoint.name)
taskExpg.add(planeversor.name)
#desired value
planeversor.reference.value = numpy.pi/2
pointpoint.reference.value = 0.0001
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from numpy import *
def totuple( a ):
al=a.tolist()
res=[]
for i in range(a.shape[0]):
res.append( tuple(al[i]) )
return tuple(res)
# Create the robot.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('romeo', device=RobotSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)
# Binds with ros, export joint_state.
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver
solver = Solver(robot)
# Alternate visualization tool
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)
#-------------------------------------------------------------------------------
def plugZmp(self, robot):
# Connect the ZMPref to OpenHRP in the waist reference frame.
self.pg.parseCmd(':SetZMPFrame world')
plug(self.wa_zmp.sout, robot.zmp)