def create_tracer(device, traj_gen=None, estimator_ft=None, estimator_kin=None,
inv_dyn=None, torque_ctrl=None):
tracer = TracerRealTime('motor_id_trace');
tracer.setBufferSize(80*(2**20));
tracer.open('/tmp/','dg_','.dat');
device.after.addSignal('{0}.triger'.format(tracer.name));
addSignalsToTracer(tracer, device);
with open('/tmp/dg_info.dat', 'a') as f:
if(estimator_ft!=None):
f.write('Estimator F/T sensors delay: {0}\n'.format(robot.estimator_ft.getDelayFTsens()));
f.write('Estimator use reference velocities: {0}\n'.format(robot.estimator_ft.getUseRefJointVel()));
f.write('Estimator use reference accelerations: {0}\n'.format(robot.estimator_ft.getUseRefJointAcc()));
f.write('Estimator accelerometer delay: {0}\n'.format(robot.estimator_ft.getDelayAcc()));
f.write('Estimator gyroscope delay: {0}\n'.format(robot.estimator_ft.getDelayGyro()));
f.write('Estimator use raw encoders: {0}\n'.format(robot.estimator_ft.getUseRawEncoders()));
f.write('Estimator use f/t sensors: {0}\n'.format(robot.estimator_ft.getUseFTsensors()));
f.write('Estimator f/t sensor offsets: {0}\n'.format(robot.estimator_ft.getFTsensorOffsets()));
if(estimator_kin!=None):
f.write('Estimator encoder delay: {0}\n'.format(robot.estimator_kin.getDelay()));
if(inv_dyn!=None):
f.write('Inv dyn Ks: {0}\n'.format(inv_dyn.Kp.value));
f.write('Inv dyn Kd: {0}\n'.format(inv_dyn.Kd.value));
f.write('Inv dyn Kf: {0}\n'.format(inv_dyn.Kf.value));
f.write('Inv dyn Ki: {0}\n'.format(inv_dyn.Ki.value));
if(torque_ctrl!=None):
f.write('Torque ctrl KpTorque: {0}\n'.format (robot.torque_ctrl.KpTorque.value ));
f.write('Torque ctrl KpCurrent: {0}\n'.format(robot.torque_ctrl.KpCurrent.value));
f.write('Torque ctrl K_tau: {0}\n'.format(robot.torque_ctrl.k_tau.value));
f.write('Torque ctrl K_v: {0}\n'.format(robot.torque_ctrl.k_v.value));
f.close();
return tracer;
def generate(self):
if self.parameters["addTimerToSotControl"]:
# init tracer
from dynamic_graph.tracer_real_time import TracerRealTime
SoTtracer = TracerRealTime("tracer_of_timers")
self.tracers[SoTtracer.name] = SoTtracer
SoTtracer.setBufferSize(10 * 1048576) # 10 Mo
SoTtracer.open("/tmp", "sot-control-trace", ".txt")
self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
self.supervisor.SoTtracer = SoTtracer
super(Factory, self).generate()
self.supervisor.sots = {}
self.supervisor.grasps = {(gh, w): t
for gh, ts in self.tasks._grasp.items()
for w, t in ts.items()}
self.supervisor.hpTasks = self.hpTasks
self.supervisor.lpTasks = self.lpTasks
self.supervisor.postActions = {}
self.supervisor.preActions = {}
self.supervisor.tracers = self.tracers
self.supervisor.controllers = self.controllers
from dynamic_graph import plug
self.supervisor.sots_indexes = dict()
for tn, sot in self.sots.iteritems():
# Pre action
if self.preActions.has_key(tn):
self.supervisor.addPreAction(tn, self.preActions[tn])
# Action
self.supervisor.addSolver(tn, sot)
# Post action
if self.postActions.has_key(tn):
self.supervisor.addPostActions(tn, self.postActions[tn])
def create_tracer(device,
traj_gen=None,
estimator_kin=None,
inv_dyn=None,
torque_ctrl=None):
tracer = TracerRealTime('motor_id_trace')
tracer.setBufferSize(80 * (2**20))
tracer.open('/tmp/', 'dg_', '.dat')
device.after.addSignal('{0}.triger'.format(tracer.name))
addSignalsToTracer(tracer, device)
with open('/tmp/dg_info.dat', 'a') as f:
if (estimator_kin != None):
f.write('Estimator encoder delay: {0}\n'.format(
robot.filters.estimator_kin.getDelay()))
if (inv_dyn != None):
f.write('Inv dyn Ks: {0}\n'.format(inv_dyn.Kp.value))
f.write('Inv dyn Kd: {0}\n'.format(inv_dyn.Kd.value))
f.write('Inv dyn Kf: {0}\n'.format(inv_dyn.Kf.value))
f.write('Inv dyn Ki: {0}\n'.format(inv_dyn.Ki.value))
if (torque_ctrl != None):
f.write('Torque ctrl KpTorque: {0}\n'.format(
robot.torque_ctrl.KpTorque.value))
f.close()
return tracer
def initializeTracer(self):
if not self.tracer:
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
self.tracer.open('/tmp/','dg_','.dat')
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def addTracer(name, prefix, dir="/tmp", suffix=".txt", size=10 * 1048576):
# default size: 10Mo
tracer = TracerRealTime (name)
self.tracers[tracer.name] = tracer
tracer.setBufferSize (size)
tracer.open (dir, prefix, suffix)
self.sotrobot.device.after.addSignal(name + ".triger")
return tracer
def create_tracer(robot, entity, tracer_name, outputs=None):
tracer = TracerRealTime(tracer_name)
tracer.setBufferSize(80 * (2**20))
tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
if outputs is not None:
addSignalsToTracer(tracer, entity, outputs)
return tracer
def create_tracer(robot, dir_name):
if not os.path.exists(dir_name):
os.makedirs(dir_name)
tracer = TracerRealTime('motor_id_trace')
tracer.setBufferSize(80 * (2**20))
tracer.open(dir_name, 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addSignalsToTracer(tracer, robot)
return tracer
def initialize_tracer(self):
"""
Initialize the tracer and by default dump the files in
~/dynamic_graph/[date_time]/
"""
if not self.tracer:
self.tracer = TracerRealTime('trace')
self.tracer.setBufferSize(self.tracerSize)
# Recompute trace.triger at each iteration to enable tracing.
self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
def dump_sot_sigs(robot, list_of_sigs, duration):
'''dumps several sot signals in /tmp
ex: dump_sot_sig(robot,[entity,signals],1.)'''
tracer = TracerRealTime('tmp_tracer')
tracer.setBufferSize(80 * (2**20))
tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
for sigs in list_of_sigs:
entity = sigs[0]
for sig in sigs[1:]:
full_sig_name = entity.name + '.' + sig
addTrace(tracer, entity, sig)
robot.device.after.addSignal(full_sig_name)
tracer.start()
sleep(duration)
dump_tracer(tracer)
tracer.clear()
def addTracerRealTime (self, robot):
from dynamic_graph.tracer_real_time import TracerRealTime
from agimus_sot.tools import filename_escape
self._tracer = TracerRealTime (self.name + "_tracer")
self._tracer.setBufferSize (10 * 1048576) # 10 Mo
self._tracer.open ("/tmp", filename_escape(self.name), ".txt")
self._tracer.add (self.omega2theta.name + ".sin1", "_theta_current")
self._tracer.add (self.omega2theta.name + ".sin2", "_omega_desired")
self._tracer.add (self.omega2theta.name + ".sout", "_theta_desired")
self._tracer.add (self.torque_controller.referenceName, "_reference_torque")
self._tracer.add (self.torque_controller.measurementName, "_measured_torque")
if hasattr(self, "_current_selec"):
self._tracer.add (self._current_selec.name + ".sout", "_measured_current")
robot.device.after.addSignal(self._tracer.name + ".triger")
return self._tracer
def generate(self):
if self.addTimerToSotControl:
# init tracer
from dynamic_graph.tracer_real_time import TracerRealTime
self.SoTtracer = TracerRealTime("tracer_of_timers")
self.timerIds = []
self.SoTtracer.setBufferSize(10 * 1048576) # 10 Mo
self.SoTtracer.open("/tmp", "sot-control-trace", ".txt")
self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
self.supervisor.SoTtracer = self.SoTtracer
self.supervisor.SoTtimerIds = self.timerIds
super(Factory, self).generate()
self.supervisor.sots = self.sots
self.supervisor.grasps = {(gh, w): t
for gh, ts in self.tasks._grasp.items()
for w, t in ts.items()}
self.supervisor.hpTasks = self.hpTasks
self.supervisor.lpTasks = self.lpTasks
self.supervisor.postActions = self.postActions
self.supervisor.preActions = self.preActions
self.supervisor.SoTtimers = self.timers
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')
create_topic(robot.publisher, robot.device, 'motorcontrol', 'motorcontrol', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.device, 'robotState', 'robotState', robot=robot, data_type='vector')
# # --- TRACER
robot.tracer = TracerRealTime("tau_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.inv_dyn, 'tau_des')
addTrace(robot.tracer, robot.inv_dyn, 'q_des')
addTrace(robot.tracer, robot.inv_dyn, 'v_des')
addTrace(robot.tracer, robot.inv_dyn, 'dv_des')
addTrace(robot.tracer, robot.errorPoseTSID, 'sout')
addTrace(robot.tracer, robot.errorComTSID, 'sout')
addTrace(robot.tracer, robot.device, 'robotState')
addTrace(robot.tracer, robot.device, 'motorcontrol')
robot.tracer.start()
def init_sot_talos_balance(robot, test_folder):
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()
data_folder = rospack.get_path('sot-talos-balance') + "/data/"
folder = data_folder + test_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
robot.comTrajGen.playTrajectoryFile(folder + 'CoM.dat')
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)
robot.lfTrajGen.playTrajectoryFile(folder + 'LeftFoot.dat')
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)
robot.rfTrajGen.playTrajectoryFile(folder + 'RightFoot.dat')
plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger)
# --- ZMP
robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot)
robot.zmpTrajGen.x.recompute(0) # trigger computation of initial value
# robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)
# --- 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)
robot.waistMix.addSelec(2, 3, 3)
robot.waistMix.default.value = [0.0] * 6
robot.waistMix.signal("sin1").value = [0.0] * 3
plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
plug(robot.waistMix.sout, robot.waistToMatrix.sin)
robot.waistTrajGen.playTrajectoryFile(folder + 'WaistOrientation.dat')
plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
# --- Interface with controller entities
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
#wp.waist.value = robot.dynamic.WT.value # wait receives a full homogeneous matrix, but only the rotational part is controlled
#wp.footLeft.value = robot.dynamic.LF.value
#wp.footRight.value = robot.dynamic.RF.value
#wp.com.value = robot.dynamic.com.value
#wp.vcom.value = [0.]*3
#wp.acom.value = [0.]*3
plug(robot.waistToMatrix.sout, wp.waist)
plug(robot.lfToMatrix.sout, wp.footLeft)
plug(robot.rfToMatrix.sout, wp.footRight)
plug(robot.comTrajGen.x, wp.com)
plug(robot.comTrajGen.dx, wp.vcom)
plug(robot.comTrajGen.ddx, wp.acom)
#plug(robot.zmpTrajGen.x, wp.zmp)
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)
# -------------------------- 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
# --- 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
# robotDim = robot.dynamic.getDimension() # 38
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
#define contactLF and contactRF
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)
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
robot.publisher = create_rospublish(robot, 'robot_publisher')
create_topic(robot.publisher,
robot.device,
'state',
robot=robot,
data_type='vector')
create_topic(robot.publisher,
robot.base_estimator,
'q',
robot=robot,
data_type='vector')
#create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')
create_topic(robot.publisher,
robot.comTrajGen,
'x',
robot=robot,
data_type='vector') # generated CoM
create_topic(robot.publisher,
robot.comTrajGen,
'dx',
robot=robot,
data_type='vector') # generated CoM velocity
create_topic(robot.publisher,
robot.comTrajGen,
'ddx',
robot=robot,
data_type='vector') # generated CoM acceleration
create_topic(robot.publisher,
robot.wp,
'comDes',
robot=robot,
data_type='vector') # desired CoM
create_topic(robot.publisher,
robot.cdc_estimator,
'c',
robot=robot,
data_type='vector') # estimated CoM
create_topic(robot.publisher,
robot.cdc_estimator,
'dc',
robot=robot,
data_type='vector') # estimated CoM velocity
create_topic(robot.publisher,
robot.com_admittance_control,
'comRef',
robot=robot,
data_type='vector') # reference CoM
create_topic(robot.publisher,
robot.dynamic,
'com',
robot=robot,
data_type='vector') # resulting SOT CoM
create_topic(robot.publisher,
robot.dcm_control,
'dcmDes',
robot=robot,
data_type='vector') # desired DCM
create_topic(robot.publisher,
robot.estimator,
'dcm',
robot=robot,
data_type='vector') # estimated DCM
create_topic(robot.publisher,
robot.zmpTrajGen,
'x',
robot=robot,
data_type='vector') # generated ZMP
create_topic(robot.publisher,
robot.wp,
'zmpDes',
robot=robot,
data_type='vector') # desired ZMP
create_topic(robot.publisher,
robot.dynamic,
'zmp',
robot=robot,
data_type='vector') # SOT ZMP
create_topic(robot.publisher,
robot.zmp_estimator,
'zmp',
robot=robot,
data_type='vector') # estimated ZMP
create_topic(robot.publisher,
robot.dcm_control,
'zmpRef',
robot=robot,
data_type='vector') # reference ZMP
create_topic(robot.publisher,
robot.waistTrajGen,
'x',
robot=robot,
data_type='vector') # desired waist orientation
create_topic(robot.publisher,
robot.lfTrajGen,
'x',
robot=robot,
data_type='vector') # desired left foot pose
create_topic(robot.publisher,
robot.rfTrajGen,
'x',
robot=robot,
data_type='vector') # desired right foot pose
create_topic(robot.publisher,
robot.ftc,
'left_foot_force_out',
robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher,
robot.ftc,
'right_foot_force_out',
robot=robot,
data_type='vector') # calibrated right wrench
create_topic(robot.publisher,
robot.dynamic,
'LF',
robot=robot,
data_type='matrixHomo') # left foot
create_topic(robot.publisher,
robot.dynamic,
'RF',
robot=robot,
data_type='matrixHomo') # right foot
# --- 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.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.com_admittance_control,
'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
addTrace(robot.tracer, robot.ftc,
'left_foot_force_out') # calibrated left wrench
addTrace(robot.tracer, robot.ftc,
'right_foot_force_out') # calibrated right wrench
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start()
task_wrist2.add(feature_wrist2.name)
# Create task for the wrist3
I4 = (
(1., 0, 0, 0.321),
(0, 1., 0, 0.109),
(0, 0, 1., 0.848),
(0, 0, 0, 1.),
)
feature_wrist = FeaturePosition('position_wrist', robot.dynamic.wrist_3_joint,
robot.dynamic.Jwrist_3_joint, I4)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1
task_wrist.add(feature_wrist.name)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(robot.device, tracer, robot.device.name, "state")
addTrace(robot.device, tracer, feature_wrist._feature.name, "position")
addTrace(robot.device, tracer, feature_wrist._reference.name, "position")
# solver
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_waist.name)
solver.push(task_wrist.name)
solver.push(task_elbow.name)
solver.push(task_lift.name)
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.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
robot.device.after.addSignal('{0}.zmpRef'.format(robot.dcm_control.name))
robot.device.after.addSignal('{0}.zmp'.format(robot.dynamic.name)) # why needed?
robot.device.after.addSignal('{0}.comRef'.format(robot.com_admittance_control.name)) # why needed?
addTrace(robot.tracer, robot.dynamic, 'zmp')
addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
addTrace(robot.tracer, robot.estimator, 'dcm')
addTrace(robot.tracer, robot.dynamic, 'com')
addTrace(robot.tracer, robot.com_admittance_control, 'comRef')
# SIMULATION
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()
robot.forceCalibrator,
'rightWristForceOut',
robot=robot,
data_type='vector') # calibrated right wrench
# # --- ROS SUBSCRIBER
robot.subscriber = RosSubscribe("end_effector_subscriber")
robot.subscriber.add("vector", "w_force", "/sot/controller/w_force")
robot.subscriber.add("vector", "force", "/sot/controller/force")
robot.subscriber.add("vector", "dq", "/sot/controller/dq")
robot.subscriber.add("vector", "w_dq", "/sot/controller/w_dq")
robot.subscriber.add("vector", "w_forceDes", "/sot/controller/w_forceDes")
robot.subscriber.add("vector", "leftWristForceOut",
"/sot/forceCalibrator/leftWristForceOut")
robot.subscriber.add("vector", "rightWristForceOut",
"/sot/forceCalibrator/rightWristForceOut")
# --- TRACER ------------------------------------------------------------------
robot.tracer = TracerRealTime("force_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.controller, 'force')
addTrace(robot.tracer, robot.controller, 'w_force')
addTrace(robot.tracer, robot.controller, 'w_dq')
addTrace(robot.tracer, robot.controller, 'dq')
robot.tracer.start()
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
create_topic(robot.publisher,
robot.ftc,
'left_foot_force_out',
robot=robot,
data_type='vector') # calibrated left wrench
create_topic(robot.publisher,
robot.ftc,
'right_foot_force_out',
robot=robot,
data_type='vector') # calibrated right wrench
# --- 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.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.com_admittance_control, 'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
def main(robot):
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 10
robot.estimator = create_dummy_dcm_estimator(robot)
# --- Admittance controller
Kp = [0.0, 0.0, 0.0]
robot.com_admittance_control = create_com_admittance_controller(
Kp, dt, robot)
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
# --- TRACER
robot.tracer = TracerRealTime("zmp_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.dynamic, 'zmp')
addTrace(robot.tracer, robot.estimator, 'dcm')
# SIMULATION
plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
sleep(1.0)
os.system("rosservice call \start_dynamic_graph")
sleep(2.0)
plug(robot.estimator.dcm, robot.com_admittance_control.zmpDes)
robot.com_admittance_control.setState(robot.dynamic.com.value,
[0.0, 0.0, 0.0])
robot.com_admittance_control.Kp.value = [10.0, 10.0, 0.0]
robot.tracer.start()
sleep(5.0)
dump_tracer(robot.tracer)
# --- DISPLAY
zmp_data = np.loadtxt('/tmp/dg_' + robot.dynamic.name + '-zmp.dat')
zmpDes_data = np.loadtxt('/tmp/dg_' + robot.estimator.name + '-dcm.dat')
plt.figure()
plt.plot(zmp_data[:, 1], 'b-')
plt.plot(zmpDes_data[:, 1], 'b--')
plt.plot(zmp_data[:, 2], 'r-')
plt.plot(zmpDes_data[:, 2], 'r--')
plt.title('ZMP real vs desired')
plt.legend(['Real x', 'Desired x', 'Real y', 'Desired y'])
plt.figure()
plt.plot(zmp_data[:, 1] - zmpDes_data[:, 1], 'b-')
plt.plot(zmp_data[:, 2] - zmpDes_data[:, 2], 'r-')
plt.title('ZMP error')
plt.legend(['Error on x', 'Error on y'])
plt.show()
