def addPgToUrdfRobot(robot):
# Configure Pattern Generator
import roslib
robot.pg = PatternGenerator('pg')
if robot.device.name == 'HRP2LAAS' or robot.device.name == 'HRP2JRL':
pkgLocation = str(roslib.packages.get_pkg_dir("hrp2_14_description"))
robot.urdfFile = str(pkgLocation + "/urdf/hrp2_14.urdf")
robot.srdfFile = str(pkgLocation + "/srdf/hrp2_14.srdf")
elif robot.device.name == 'HRP4LIRMM':
pkgLocation = roslib.packages.get_pkg_dir("hrp4_description")
robot.urdfFile = str(pkgLocation + "/urdf/hrp4.urdf")
robot.srdfFile = str(pkgLocation + "/srdf/hrp4.srdf")
elif robot.device.name == 'ROMEO':
pkgLocation = roslib.packages.get_pkg_dir("romeo_description")
robot.urdfFile = str(pkgLocation + "/urdf/romeo.urdf")
robot.srdfFile = str(pkgLocation + "/srdf/romeo.srdf")
else:
pkgLocation = str(roslib.packages.get_pkg_dir("hrp2_14_description"))
robot.urdfFile = str(pkgLocation + "/urdf/hrp2_14.urdf")
robot.srdfFile = str(pkgLocation + "/srdf/hrp2_14.srdf")
robot.pg.setURDFpath(robot.urdfFile)
robot.pg.setSRDFpath(robot.srdfFile)
if (hasattr(robot, 'jointMap')):
print "some joints need to be mapped"
for i in robot.jointMap:
robot.pg.addJointMapping(i, robot.jointMap[i])
# Build Pattern Generator
robot.pg.buildModel()
# Initialise Pattern Generator
initPg(robot)
def addPgToRobot(robot):
modelDir = robot.modelDir + '/'
robotName = robot.modelName
specificitiesPath = robot.specificitiesPath
jointRankPath = robot.jointRankPath
robot.pg = PatternGenerator('pg')
robot.pg.setVrmlDir(modelDir + '/')
robot.pg.setVrml(robotName)
robot.pg.setXmlSpec(specificitiesPath)
robot.pg.setXmlRank(jointRankPath)
print "At this stage"
robot.pg.buildModel()
# Standard initialization
robot.pg.parseCmd(":samplingperiod 0.005")
robot.pg.parseCmd(":previewcontroltime 1.6")
robot.pg.parseCmd(":walkmode 0")
robot.pg.parseCmd(":omega 0.0")
robot.pg.parseCmd(":stepheight 0.05")
robot.pg.parseCmd(":singlesupporttime 0.780")
robot.pg.parseCmd(":doublesupporttime 0.020")
robot.pg.parseCmd(":armparameters 0.5")
robot.pg.parseCmd(":LimitsFeasibility 0.0")
robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
robot.pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
robot.pg.parseCmd(":comheight 0.814")
robot.pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")
plug(robot.dynamic.position, robot.pg.position)
plug(robot.com, robot.pg.com)
plug(robot.dynamic.signal('left-ankle'), robot.pg.leftfootcurrentpos)
plug(robot.dynamic.signal('right-ankle'), robot.pg.rightfootcurrentpos)
robotDim = len(robot.dynamic.velocity.value)
robot.pg.motorcontrol.value = robotDim * (0, )
robot.pg.zmppreviouscontroller.value = (0, 0, 0)
robot.pg.initState()
omega = sqrt(g / h)
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf, dt)
# --- Initial feet and waist
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- PATTERN GENERATOR --------------------------
robot.pg = PatternGenerator('pg')
# Specify the robot model TODO: use get_parameter through ParameterServer
# DO USE FILE ANYMORE
rospack = RosPack()
talos_data_folder = rospack.get_path('talos_data')
robot.pg.setURDFpath(talos_data_folder + '/urdf/talos_reduced_wpg.urdf')
robot.pg.setSRDFpath(talos_data_folder + '/srdf/talos_wpg.srdf')
robot.pg.buildModel()
robot.pg.parseCmd(":samplingperiod 0.005")
robot.pg.parseCmd(":previewcontroltime 1.6")
robot.pg.parseCmd(":omega 0.0")
robot.pg.parseCmd(':stepheight 0.05')
robot.pg.parseCmd(':doublesupporttime 0.2')
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()
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import PatternGenerator, Selector
pg = PatternGenerator('pg')
pg.setVrmlDir(modelDir + '/')
pg.setVrml(modelName[robotName])
pg.setXmlSpec(specificitiesPath)
pg.setXmlRank(jointRankPath)
pg.buildModel()
# Standard initialization
pg.parseCmd(":samplingperiod 0.005")
pg.parseCmd(":previewcontroltime 1.6")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":omega 0.0")
pg.parseCmd(":stepheight 0.05")
pg.parseCmd(":singlesupporttime 0.780")
pg.parseCmd(":doublesupporttime 0.020")
pg.parseCmd(":armparameters 0.5")
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)
class MetaPG:
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)
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)
def plugWaistTask(self, taskWaist):
plug(self.waistReference.sout, taskWaist.featureDes.position)
taskWaist.feature.selec.value = '111100'
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 startHerdt(self, xyconstraint=True):
self.pg.parseCmd(':SetAlgoForZmpTrajectory Herdt')
self.pg.parseCmd(':doublesupporttime 0.1')
self.pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
self.pg.parseCmd(':numberstepsbeforestop 2')
# Set constraints on XY
if xyconstraint:
self.pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
# The next command must be runned after a OpenHRP.inc ... ???
# Start the robot with a speed of 0.1 m/0.8 s.
self.pg.parseCmd(':HerdtOnline 0.1 0.0 0.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
pg = PatternGenerator('pg')
pg.setVrmlDir(modelDir + '/')
pg.setVrml(modelName[robotName])
pg.setXmlSpec(specificitiesPath)
pg.setXmlRank(jointRankPath)
pg.buildModel()
# Standard initialization
pg.parseCmd(":samplingperiod 0.005")
pg.parseCmd(":previewcontroltime 1.6")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":omega 0.0")
pg.parseCmd(":stepheight 0.05")
pg.parseCmd(":singlesupporttime 0.780")
pg.parseCmd(":doublesupporttime 0.020")
pg.parseCmd(":armparameters 0.5")
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()
# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import PatternGenerator,Selector
pg = PatternGenerator('pg')
pg.setVrmlDir(modelDir+'/')
pg.setVrml(modelName[robotName])
pg.setXmlSpec(specificitiesPath)
pg.setXmlRank(jointRankPath)
pg.buildModel()
# Standard initialization
pg.parseCmd(":samplingperiod 0.005")
pg.parseCmd(":previewcontroltime 1.6")
pg.parseCmd(":comheight 0.814")
pg.parseCmd(":omega 0.0")
pg.parseCmd(":stepheight 0.05")
pg.parseCmd(":singlesupporttime 0.780")
pg.parseCmd(":doublesupporttime 0.020")
pg.parseCmd(":armparameters 0.5")
def init_online_walking(robot):
# 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10
cm_conf.CTRL_MAX = 1000.0 # temporary hack
dt = robot.device.getTimeStep()
robot.timeStep = dt
# --- Pendulum parameters
robot_name = 'robot'
robot.dynamic.com.recompute(0)
robotDim = robot.dynamic.getDimension()
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g / h)
# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf, dt)
# --- Initial feet and waist
robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF',
robot.OperationalPointsMap['right-ankle'])
robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
rospack = RosPack()
# -------------------------- PATTERN GENERATOR --------------------------
robot.pg = PatternGenerator('pg')
# MODIFIED WITH MY PATHS
talos_data_folder = rospack.get_path('talos_data')
robot.pg.setURDFpath(talos_data_folder + '/urdf/talos_reduced_wpg.urdf')
robot.pg.setSRDFpath(talos_data_folder + '/srdf/talos_wpg.srdf')
## END MODIFIED
robot.pg.buildModel()
robot.pg.parseCmd(":samplingperiod 0.005")
robot.pg.parseCmd(":previewcontroltime 1.6")
robot.pg.parseCmd(":omega 0.0")
robot.pg.parseCmd(':stepheight 0.05')
robot.pg.parseCmd(':doublesupporttime 0.2')
robot.pg.parseCmd(':singlesupporttime 1.0')
robot.pg.parseCmd(":armparameters 0.5")
robot.pg.parseCmd(":LimitsFeasibility 0.0")
robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.7 3.0")
robot.pg.parseCmd(":UpperBodyMotionParameters -0.1 -1.0 0.0")
robot.pg.parseCmd(":comheight 0.876681")
robot.pg.parseCmd(":setVelReference 0.1 0.0 0.0")
robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau")
plug(robot.dynamic.position, robot.pg.position)
plug(robot.dynamic.com, robot.pg.com)
#plug(robot.dynamic.com, robot.pg.comStateSIN)
plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
robotDim = len(robot.dynamic.velocity.value)
robot.pg.motorcontrol.value = robotDim * (0, )
robot.pg.zmppreviouscontroller.value = (0, 0, 0)
robot.pg.initState()
robot.pg.parseCmd(':setDSFeetDistance 0.162')
robot.pg.parseCmd(':NaveauOnline')
robot.pg.parseCmd(':numberstepsbeforestop 2')
robot.pg.parseCmd(':setfeetconstraint XY 0.091 0.0489')
robot.pg.parseCmd(':deleteallobstacles')
robot.pg.parseCmd(':feedBackControl false')
robot.pg.parseCmd(':useDynamicFilter true')
robot.pg.velocitydes.value = (0.1, 0.0, 0.0) # DEFAULT VALUE (0.1,0.0,0.0)
# -------------------------- TRIGGER --------------------------
robot.triggerPG = BooleanIdentity('triggerPG')
robot.triggerPG.sin.value = 0
plug(robot.triggerPG.sout, robot.pg.trigger)
# --------- Interface with controller entities -------------
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
# #wp.displaySignals()
wp.omega.value = omega
# 22.04 after modifying pg.cpp, new way to try and connect the waist
plug(robot.pg.waistattitudematrixabsolute, wp.waist)
plug(robot.pg.leftfootref, wp.footLeft)
plug(robot.pg.rightfootref, wp.footRight)
plug(robot.pg.comref, wp.com)
plug(robot.pg.dcomref, wp.vcom)
plug(robot.pg.ddcomref, wp.acom)
robot.wp = wp
# --- Compute the values to use them in initialization
robot.wp.comDes.recompute(0)
robot.wp.dcmDes.recompute(0)
robot.wp.zmpDes.recompute(0)
## END ADDED
# -------------------------- ESTIMATION --------------------------
# --- Base Estimation
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.m2qLF = MatrixHomoToPoseQuaternion('m2qLF')
plug(robot.dynamic.LF, robot.m2qLF.sin)
plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF')
plug(robot.dynamic.RF, robot.m2qRF.sin)
plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
# --- 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)
robot.cdc_estimator = cdc_estimator
# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega
estimator.mass.value = 1.0
plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc, estimator.momenta)
estimator.init()
robot.estimator = estimator
# --- Force calibration
robot.ftc = create_ft_calibrator(robot, ft_conf)
# --- ZMP estimation
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.rdynamic.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()
robot.zmp_estimator = zmp_estimator
# -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller
Kp_dcm = [8.0] * 3
Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl")
dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = mass
dcm_controller.omega.value = omega
plug(robot.cdc_estimator.c, dcm_controller.com)
plug(robot.estimator.dcm, dcm_controller.dcm)
plug(robot.wp.zmpDes, dcm_controller.zmpDes)
plug(robot.wp.dcmDes, dcm_controller.dcmDes)
dcm_controller.init(dt)
robot.dcm_control = dcm_controller
Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
# --- CoM admittance controller
Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
com_admittance_control = ComAdmittanceController("comAdmCtrl")
com_admittance_control.Kp.value = Kp_adm
plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
com_admittance_control.zmpDes.value = robot.wp.zmpDes.value
# should be plugged to robot.dcm_control.zmpRef
plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
com_admittance_control.init(dt)
com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
robot.com_admittance_control = com_admittance_control
Kp_adm = [15.0, 15.0, 0.0] # this value is employed later
# --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
robot.cm.addCtrlMode('sot_input')
robot.cm.setCtrlMode('all', 'sot_input')
robot.cm.addEmergencyStopSIN('zmp')
# -------------------------- SOT CONTROL --------------------------
# --- Upper body
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(18, True)
robot.taskUpperBody.feature.selectDof(19, True)
robot.taskUpperBody.feature.selectDof(20, True)
robot.taskUpperBody.feature.selectDof(21, True)
robot.taskUpperBody.feature.selectDof(22, True)
robot.taskUpperBody.feature.selectDof(23, True)
robot.taskUpperBody.feature.selectDof(24, True)
robot.taskUpperBody.feature.selectDof(25, True)
robot.taskUpperBody.feature.selectDof(26, True)
robot.taskUpperBody.feature.selectDof(27, True)
robot.taskUpperBody.feature.selectDof(28, True)
robot.taskUpperBody.feature.selectDof(29, True)
robot.taskUpperBody.feature.selectDof(30, True)
robot.taskUpperBody.feature.selectDof(31, True)
robot.taskUpperBody.feature.selectDof(32, True)
robot.taskUpperBody.feature.selectDof(33, True)
robot.taskUpperBody.feature.selectDof(34, True)
robot.taskUpperBody.feature.selectDof(35, True)
robot.taskUpperBody.feature.selectDof(36, True)
robot.taskUpperBody.feature.selectDof(37, True)
robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
# --- CONTACTS
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(300)
plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(300)
plug(robot.wp.footRightDes,
robot.contactRF.featureDes.position) #.errorIN?
locals()['contactRF'] = robot.contactRF
# --- COM height
robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
robot.taskComH.task.controlGain.value = 100.
robot.taskComH.feature.selec.value = '100'
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef,
robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)
robot.taskCom.feature.selec.value = '011'
# --- Waist
robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT',
robot.OperationalPointsMap['waist'])
robot.keepWaist.feature.frame('desired')
robot.keepWaist.gain.setConstant(300)
plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) #de base
robot.keepWaist.feature.selec.value = '111000'
locals()['keepWaist'] = robot.keepWaist
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager
plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control)
robot.sot.push(robot.taskUpperBody.name)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskComH.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.keepWaist.task.name)
# --- Fix robot.dynamic inputs
plug(robot.device.velocity, robot.dynamic.velocity)
robot.dvdt = Derivator_of_Vector("dv_dt")
robot.dvdt.dt.value = dt
plug(robot.device.velocity, robot.dvdt.sin)
plug(robot.dvdt.sout, robot.dynamic.acceleration)
# -------------------------- PLOTS --------------------------
# --- ROS PUBLISHER
## THIS PARAGRAPH QUITE DIFFERENT, TO CHECK
robot.publisher = create_rospublish(robot, 'robot_publisher')
## ADDED
create_topic(robot.publisher,
robot.pg,
'comref',
robot=robot,
data_type='vector') # desired CoM
create_topic(robot.publisher,
robot.pg,
'dcomref',
robot=robot,
data_type='vector')
create_topic(robot.publisher,
robot.wp,
'waist',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.keepWaist.featureDes,
'position',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.dynamic,
'WT',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.pg,
'waistattitudematrixabsolute',
robot=robot,
data_type='matrixHomo') ## que font ces lignes exactement ??
create_topic(robot.publisher,
robot.pg,
'leftfootref',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.wp,
'footLeft',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.pg,
'rightfootref',
robot=robot,
data_type='matrixHomo')
create_topic(robot.publisher,
robot.wp,
'footRight',
robot=robot,
data_type='matrixHomo')
## --- TRACER
robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
addTrace(robot.tracer, robot.pg, 'waistattitudeabsolute')
# fin
addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.pg, 'comref')
addTrace(robot.tracer, robot.pg, 'dcomref')
addTrace(robot.tracer, robot.pg, 'ddcomref')
addTrace(robot.tracer, robot.pg, 'rightfootref')
addTrace(robot.tracer, robot.pg, 'leftfootref')
addTrace(robot.tracer, robot.pg, 'rightfootcontact')
addTrace(robot.tracer, robot.pg, 'leftfootcontact')
addTrace(robot.tracer, robot.pg, 'SupportFoot')
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start()
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)
class MetaPG:
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)
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)
def plugWaistTask(self, taskWaist):
plug(self.waistReference.sout, taskWaist.featureDes.position)
taskWaist.feature.selec.value = "111100"
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 startHerdt(self, xyconstraint=True):
self.pg.parseCmd(":SetAlgoForZmpTrajectory Herdt")
self.pg.parseCmd(":doublesupporttime 0.1")
self.pg.parseCmd(":singlesupporttime 0.7")
# When velocity reference is at zero, the robot stops all motion after n steps
self.pg.parseCmd(":numberstepsbeforestop 2")
# Set constraints on XY
if xyconstraint:
self.pg.parseCmd(":setfeetconstraint XY 0.09 0.06")
# The next command must be runned after a OpenHRP.inc ... ???
# Start the robot with a speed of 0.1 m/0.8 s.
self.pg.parseCmd(":HerdtOnline 0.1 0.0 0.0")