def main(robot):
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
robot.traj_gen = create_config_trajectory_generator(dt, robot)
JOINT = 31
QJOINT = JOINT + 6
# --- Joint
robot.taskJoint = MetaTaskKineConfig(robot.dynamic, [QJOINT])
# robot.dynamic.com.recompute(0)
robot.taskJoint.featureDes.errorIN.value = N_CONFIG * [0.0]
robot.taskJoint.task.controlGain.value = 100
# --- 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.taskJoint.task.name)
robot.device.control.recompute(0)
plug(robot.traj_gen.x, robot.taskJoint.featureDes.errorIN)
sleep(1.0)
os.system('rosservice call /start_dynamic_graph')
sleep(1.0)
robot.traj_gen.move(QJOINT, -1.0, 1.0)
sleep(1.1)
robot.traj_gen.startSinusoid(QJOINT, 1.0, 8.0)
sleep(10.0)
robot.traj_gen.stop(QJOINT)
def createScrewTask(robot, TwoHandTool):
refToTwoHandToolMatrix = array(RPYToMatrix(TwoHandTool))
#RH-TwoHandTool Homogeneous Transformation Matrix (fixed in time)
robot.mTasks['rh'].feature.position.recompute(robot.device.control.time)
RHToTwoHandToolMatrix = dot(
linalg.inv(array(robot.mTasks['rh'].feature.position.value)),
refToTwoHandToolMatrix)
#!!!!!! RH and Support are different references, because the X rotation is not controlled in positioning!!!!!!!!!!!!!!!!!!!!!!!!!!
RHToScrewMatrix = dot(RHToTwoHandToolMatrix, TwoHandToolToScrewMatrix)
# Screw Lenght - unused at the moment
#screw_len = 0.03
# TASK Screw
robot.mTasks['screw'] = MetaTaskKine6d('screw', robot.dynamic, 'screw',
'right-wrist')
handMgrip = array(robot.mTasks['rh'].opmodif)
robot.mTasks['screw'].opmodif = matrixToTuple(
dot(handMgrip, RHToScrewMatrix))
robot.mTasks['screw'].feature.selec.value = '000111'
# TASK Screw orientation
robot.mTasks['screw'].featureVec = FeatureVector3("featureVecScrew")
plug(robot.mTasks['screw'].opPointModif.position,
robot.mTasks['screw'].featureVec.signal('position'))
plug(robot.mTasks['screw'].opPointModif.jacobian,
robot.mTasks['screw'].featureVec.signal('Jq'))
robot.mTasks['screw'].featureVec.vector.value = array([0., 0., 1.])
robot.mTasks['screw'].task.add(robot.mTasks['screw'].featureVec.name)
def __init__(self, footname, sotrobot, selec='111111'):
robotname, sotjoint = parseHppName(footname)
self.taskFoot = MetaTaskKine6d(Foot.sep + footname, sotrobot.dynamic,
sotjoint, sotjoint)
self.taskFoot.feature.selec.value = selec
super(Foot, self).__init__(
tasks=[
self.taskFoot.task,
],
topics={
footname: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": footname,
"signalGetters": [self._signalPositionRef]
},
# "vel_" + self.gripper.name: {
"vel_" + footname: {
"velocity": True,
"type": "vector",
"handler": "hppjoint",
"hppjoint": footname,
"signalGetters": [self._signalVelocityRef]
},
})
self.taskFoot.gain.value = 5
def Pr2ContactTask(robot):
task = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')
task.feature.frame('desired')
task.feature.selec.value = '011100'
task.gain.setConstant(10)
#locals()['contact'] = task
return task
def init_appli(robot):
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')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
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
from dynamic_graph import plug
from dynamic_graph.sot.core.sot import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
from dynamic_graph.ros import RosPublish
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
from dynamic_graph import plug
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
robot.device.control.recompute(0)
def Pr2BaseTask(robot):
task = MetaTaskKine6d('base', robot.dynamic, 'waist', 'waist')
baseMground = eye(4)
baseMground[0:3, 3] = (0, 0, 0)
task.opmodif = matrixToTuple(baseMground)
task.feature.frame('desired')
task.feature.selec.value = '100011'
task.gain.setConstant(10)
return task
def Pr2ContactTask(robot):
task = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')
task.feature.frame('desired')
task.feature.selec.value = '011100'
task.featureDes.position.value = \
array([[1,0,0,0],[0,1,0,0],[0,0,1,0.051],[0,0,0,1]])
task.gain.setConstant(10)
#locals()['contact'] = task
return task
def main(robot, conf=None):
if (conf is None):
conf = get_default_conf()
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
NJ = robot.dimension - 7
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- 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
# --- 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
# --- SOT
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.taskCom.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.device.control.recompute(0)
# --- ENTITIES
robot.param_server = create_parameter_server(conf.param_server, dt)
robot.example = create_example()
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 makeTasks(self, sotrobot):
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier
from dynamic_graph import plug
if self.relative:
self.graspTask = MetaTaskKine6d (
Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
sotrobot.dynamic,
self.gripper.sotjoint,
self.gripper.sotjoint)
# Creates the matrix transforming the goal gripper's position into the desired moving gripper's position:
# on the other handle of the object
#M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0))
M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse())
self.graspTask.opmodif = matrixToTuple(M)
self.graspTask.feature.frame("current")
setGain(self.graspTask.gain,(100,0.9,0.01,0.9))
self.graspTask.task.setWithDerivative (False)
# Sets the position and velocity of the other gripper as the goal of the first gripper pose
if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint):
sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint)
sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint)
plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position)
plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity)
#plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq)
# We will need a less barbaric method to do this...
# Zeroes the jacobian for the joints we don't want to use.
# - Create the selection matrix for the desired joints
mat = ()
for i in range(38):
mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),)
# - Multiplies it with the computed jacobian matrix of the gripper
mult = Multiply_of_matrix('mult')
mult.sin2.value = mat
plug(self.graspTask.opPointModif.jacobian, mult.sin1)
plug(mult.sout, self.graspTask.feature.Jq)
self.tasks = [ self.graspTask.task ]
#self.tasks = []
self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
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()
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')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
#define contactLF and contactRF
for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']],
['RF', robot.OperationalPointsMap['right-ankle']]]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
robot.e2q = EulerToQuat("e2q")
plug(robot.baseEstimator.q, robot.e2q.euler)
robot.forceCalibrator = create_ft_wrist_calibrator(robot, endEffectorWeight,
rightOC, leftOC)
robot.controller = create_end_effector_admittance_controller(
robot, endEffector, "EEAdmittance")
robot.controlManager = create_ctrl_manager(controlManagerConfig,
robot.timeStep)
robot.controlManager.addCtrlMode('sot_input')
robot.controlManager.setCtrlMode('all', 'sot_input')
# --- HAND TASK ----------------------------------------------------------------
taskRightHand = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'arm_right_7_joint')
handMgrip = np.eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRightHand.opmodif = matrixToTuple(handMgrip)
taskRightHand.feature.frame('desired')
taskRightHand.feature.selec.value = '111111'
taskRightHand.task.setWithDerivative(True)
taskRightHand.task.controlGain.value = 0
taskRightHand.feature.position.value = np.eye(4)
taskRightHand.feature.velocity.value = [0., 0., 0., 0., 0., 0.]
taskRightHand.featureDes.position.value = np.eye(4)
plug(robot.controller.dq, taskRightHand.featureDes.velocity)
# --- BASE TASK ----------------------------------------------------------------
taskWaist = MetaTaskKine6d('taskWaist', robot.dynamic, 'WT',
def Pr2LeftHandTask(robot):
task = MetaTaskKine6d('lh', robot.dynamic, 'lh', 'left-wrist')
task.opmodif = matrixToTuple(Pr2handMgrip)
task.feature.frame('desired')
return task
# --- 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 = 100.
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)
def _makeAbsolute(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
name = PreGrasp.sep.join(
["", "pregrasp", self.gripper.name, self.handle.fullName])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.gripper.joint, self.gripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.gripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Planned handle pose H_p = object_planned_pose * self.handle.pose
# Real handle pose H_r = object_real_pose * self.handle.pose
# Displacement M = H_p^-1 * H_r
# planned gripper pose G_p= joint_planned_pose * self.gripper.pose
# The derised position is
# G*_r = G_p * M = G_p * h^-1 * O_p^-1 * O_r * h
# = J_p * g * h^-1 * O_p^-1 * O_r * h
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
nsignals = 2
if withMeasurementOfGripperPos: nsignals += 1
if withMeasurementOfObjectPos: nsignals += 5
self.gripper_desired_pose.setSignalNumber(nsignals)
isignal = 0
def sig(i):
return self.gripper_desired_pose.signal("sin" + str(i))
# Object calibration
if withMeasurementOfObjectPos:
h = self.handle.pose
# TODO Integrate measurement of h_r: position error of O_r^-1 * G_r
# (for the release phase and computed only at time 0)
# self.gripper_desired_pose.sin0 -> plug to handle link planning pose
self.topics[self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [lambda: self.gripper_desired_pose.sin0],
}
isignal += 1
sig(isignal).value = se3ToTuple(h.inverse())
isignal += 1
delta_h = sig(isignal)
self.topics["delta_" + self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.handle.fullLink,
"frame1": self.handle.fullLink + "_estimated",
"signalGetters": [lambda: delta_h],
}
isignal += 1
sig(isignal).value = se3ToTuple(h)
isignal += 1
self._oMh_p_inv = Inverse_of_matrixHomo(self.handle.fullLink +
"_invert_planning_pose")
self.topics[self.handle.fullLink]["signalGetters"] \
.append (lambda: self._oMh_p_inv.sin)
plug(self._oMh_p_inv.sout, sig(isignal))
isignal += 1
# Joint planning pose
joint_planning_pose = sig(isignal)
self.topics[self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [lambda: joint_planning_pose],
}
isignal += 1
# Joint calibration
if withMeasurementOfGripperPos:
self._delta_g_inv = Inverse_of_matrixHomo(self.gripper.fullJoint +
"_delta_inv")
self.topics["delta_" + self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.gripper.fullJoint,
"frame1": self.gripper.fullJoint + "_estimated",
"signalGetters": [lambda: self._delta_g_inv.sin],
}
plug(self._delta_g_inv.sout, self.gripper_desired_pose.sin6)
isignal += 1
sig(isignal).value = se3ToTuple(self.gripper.pose)
isignal += 1
assert isignal == nsignals
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
display=True)
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data,
initialConfig, OperationalPointsMap)
njoints = pinocchioRobot.model.nv - 6
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
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')
taskLH = MetaTaskKine6d('lh', robot.dynamic, 'lh',
robot.OperationalPointsMap['left-wrist'])
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
def Pr2ChestTask(robot):
task = MetaTaskKine6d('chest', robot.dynamic, 'chest', 'chest')
task.feature.frame('desired')
return task
def _makeAbsoluteBasedOnOther(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.fullName, self.handle.fullName,
"based", self.otherGripper.name, self.otherHandle.fullName
])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.otherGripper.joint,
self.otherGripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.otherGripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Displacement gripper1: M = G1_p^-1 * G1_r
# The derised position of handle1 is
# H1*_r = H1_p * M = H1_p * G1_p^-1 * G1_r
#
# Moreover, the relative position of handle2 wrt to gripper2 may not be perfect so
# h2_r = O_r^-1 * G2_r
#
# G2*_r = O*_r * h2_r = H1_p * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * g1^-1 * J1_p^-1 * J1_r * g1 * h1^-1 * h2_r
# where h2_r can be a constant value of the expression above.
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
self.gripper_desired_pose.setSignalNumber(5)
## self.gripper_desired_pose.setSignalNumber (7)
# self.gripper_desired_pose.sin0 -> plug to object1 planning pose
# self.gripper_desired_pose.sin1.value = se3ToTuple(self.handle.pose)
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.handle.pose * self.gripper.pose.inverse())
self._invert_planning_pose = Inverse_of_matrixHomo(
self.gripper.fullLink + "_invert_planning_pose")
# self._invert_planning_pose.sin -> plug to link1 planning pose
plug(self._invert_planning_pose.sout,
self.gripper_desired_pose.sin2)
# self.gripper_desired_pose.sin3 -> plug to link1 real pose
# self.gripper_desired_pose.sin4.value = se3ToTuple (self.handle.pose.inverse() * self.otherHandle.pose)
self.gripper_desired_pose.sin4.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse() *
self.otherHandle.pose)
## self.gripper_desired_pose.sin4.value = se3ToTuple (self.gripper.pose * self.handle.pose.inverse())
## self.gripper_desired_pose.sin5 -> plug to object real pose
## if self.otherGripper.joint not in sotrobot.dyn.signals():
## sotrobot.dyn.createOpPoint (self.otherGripper.joint, self.otherGripper.joint)
## plug(sotrobot.dyn.signal(self.otherGripper.joint), self.gripper_desired_pose.sin6)
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.topics = {
self.handle.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullLink,
"signalGetters": [
lambda: self._invert_planning_pose.sin,
]
},
"measurement_" + self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": "world",
"frame1": self.gripper.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin3,
]
},
## "measurement_" + self.otherHandle.fullLink: {
## "velocity": False,
## "type": "matrixHomo",
## "handler": "tf_listener",
## "frame0": "world",
## "frame1": self.otherHandle.fullLink,
## "signalGetters": [ self.gripper_desired_pose.sin5 ] },
}
else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose)
self.topics = {
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
}
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
from sot_talos_balance.create_entities_utils import *
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- 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
# --- 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(0)
robot.contactLF.keep()
robot.contactLF.task.setWithDerivative(True)
robot.contactLF.featureDes.velocity.value = [0.] * 6
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(0)
robot.contactRF.keep()
robot.contactRF.task.setWithDerivative(True)
robot.contactRF.featureDes.velocity.value = [0.] * 6
locals()['contactRF'] = robot.contactRF
def Pr2LeftHandTask(robot):
task = MetaTaskKine6d('left-wrist', robot.dynamic, 'left-wrist',
'left-wrist')
task.feature.frame('desired')
return task
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)
# 3. Create a solver
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver
solver = Solver(robot)
# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'arm_joint_5')
Pr2handMgrip = eye(4)
Pr2handMgrip[0:3, 3] = (0.337, 0, 0.275)
taskRH.opmodif = matrixToTuple(Pr2handMgrip)
taskRH.feature.frame('desired')
targetR = (0.65, 0.2, 0.9)
selec = '111'
gain = (4.9, 0.9, 0.01, 0.9)
gotoNd(taskRH, targetR, selec, gain)
# 5. Add a contact constraint with the robot and the floor
contact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')
contact.feature.frame('desired')
contact.feature.selec.value = '011100'
contact.gain.setConstant(10)
contact.keep()
robot = Pr2('PR2', device=RobotSimu('PR2'))
plug(robot.device.state, robot.dynamic.position)
# 2. Ros binding
# roscore must be running
from dynamic_graph.ros import Ros
ros = Ros(robot)
# Use kine solver (with inequalities)
solver = initialize(robot)
# 4. Define a position task for the right hand
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist')
Pr2handMgrip = eye(4)
Pr2handMgrip[0:3, 3] = (0.18, 0, 0)
taskRH.opmodif = matrixToTuple(Pr2handMgrip)
taskRH.feature.frame('desired')
targetR = (0.65, 0.2, 0.9)
selec = '111'
gain = (4.9, 0.9, 0.01, 0.9)
gotoNd(taskRH, targetR, selec, gain)
# 6. Push tasks in the solver
solver.push(taskRH.task)
# Main loop
dt = 3e-3
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.ros import RosPublish
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.tiago.diff_drive_controller import HolonomicProjection
from numpy import eye
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)
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()
def main(robot):
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- 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
# --- 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
# --- SOT
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.taskCom.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.device.control.recompute(0)
# --- ESTIMATION
robot.param_server = create_parameter_server(param_server_conf, dt)
# robot.imu_offset_compensation = create_imu_offset_compensation(robot, 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, conf)
robot.be_filters = create_be_filters(robot, dt)
# --- TRACERS
outputs = ['robotState']
robot.device_tracer = create_tracer(robot, robot.device, 'device_tracer', outputs)
outputs = ['q']
robot.estimator_tracer = create_tracer(robot, robot.base_estimator, 'estimator_tracer', outputs)
robot.device.after.addSignal('base_estimator.q')
# --- RUN SIMULATION
plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN)
sleep(1.0)
os.system("rosservice call \start_dynamic_graph")
sleep(2.0)
robot.comTrajGen.move(1, -0.025, 4.0)
sleep(5.0)
robot.comTrajGen.startSinusoid(1, 0.05, 8.0)
sleep(0.2)
robot.device_tracer.start()
robot.estimator_tracer.start()
sleep(3.0)
dump_tracer(robot.device_tracer)
dump_tracer(robot.estimator_tracer)
print('data dumped')
# --- DISPLAY
device_data = read_tracer_file('/tmp/dg_' + robot.device.name + '-robotState.dat')
estimator_data = read_tracer_file('/tmp/dg_' + robot.base_estimator.name + '-q.dat')
plot_select_traj(device_data, [10, 23, 15])
plot_select_traj(estimator_data, [10, 23, 15])
write_pdf_graph('/tmp/')
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()
q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q
robotDim = robot.dynamic.getDimension() # 38
for i in range(18, robotDim):
robot.taskUpperBody.feature.selectDof(i, 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)
plug(sot.control, robot.device.control)
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
dt = 1e-3
@loopInThread
def inc():
robot.device.increment(dt)
runner = inc()
[go, stop, next, n] = loopShortcuts(runner)
# Tasks
taskzero = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist')
taskzero.feature.frame('desired')
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(robot.dynamic.position, taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(
range(18, 25) + range(26, 27) + range(28, 31) + range(32, 40) +
range(41, 42) + range(43, 46) + range(47, 50))
taskContact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')
