def initPostureTask(robot):
# --- TASK POSTURE --------------------------------------------------
# set a default position for the joints.
robot.features['featurePosition'] = FeaturePosture('featurePosition')
plug(robot.device.state, robot.features['featurePosition'].state)
robotDim = len(robot.dynamic.velocity.value)
robot.features['featurePosition'].posture.value = robot.halfSitting
# Remove the dofs of the feet.
postureTaskDofs = [True] * (len(robot.dynamic.position.value) - 6)
jla = robot.dynamic.signal('J' +
robot.OperationalPointsMap['left-ankle']).value
postureTaskDofs = removeDofUsed(jla, postureTaskDofs)
jra = robot.dynamic.signal('J' +
robot.OperationalPointsMap['right-ankle']).value
postureTaskDofs = removeDofUsed(jra, postureTaskDofs)
for dof, isEnabled in enumerate(postureTaskDofs):
robot.features['featurePosition'].selectDof(dof + 6, isEnabled)
robot.tasks['robot_task_position'] = Task('robot_task_position')
robot.tasks['robot_task_position'].add('featurePosition')
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1, 0.1, 125e3)
gainPosition.gain.value = 5
plug(robot.tasks['robot_task_position'].error, gainPosition.error)
plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
def __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.gain = yamlData.get('gain', 1.)
self.name = yamlData['name']
self.reference = yamlData['reference']
self.task = Task('joint'+str(id(yamlData)))
self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
plug(motion.robot.device.state, self.feature.state)
jointId = self.nameToId[self.name]
posture = np.array(motion.robot.halfSitting)
posture[jointId] = self.reference
self.feature.setPosture(tuple(posture.tolist()))
for i in xrange(len(posture) - 6):
if i + 6 == jointId:
self.feature.selectDof(i + 6, True)
else:
self.feature.selectDof(i + 6, False)
self.task.add(self.feature.name)
self.task.controlGain.value = self.gain
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
(jointId,))
class MotionJoint(Motion):
yaml_tag = u'joint'
gain = None
name = None
reference = None
# FIXME: not generic enough and joints are missing.
nameToId = {
'left-claw': 35,
'right-claw': 28
}
def __init__(self, motion, yamlData, defaultDirectories):
checkDict('interval', yamlData)
Motion.__init__(self, motion, yamlData)
self.gain = yamlData.get('gain', 1.)
self.name = yamlData['name']
self.reference = yamlData['reference']
self.task = Task('joint'+str(id(yamlData)))
self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
plug(motion.robot.device.state, self.feature.state)
jointId = self.nameToId[self.name]
posture = np.array(motion.robot.halfSitting)
posture[jointId] = self.reference
self.feature.setPosture(tuple(posture.tolist()))
for i in xrange(len(posture) - 6):
if i + 6 == jointId:
self.feature.selectDof(i + 6, True)
else:
self.feature.selectDof(i + 6, False)
self.task.add(self.feature.name)
self.task.controlGain.value = self.gain
# Push the task into supervisor.
motion.supervisor.addTask(self.task.name,
self.interval[0], self.interval[1],
self.priority,
(jointId,))
def __str__(self):
return "joint motion ({0})".format(self.name)
def setupTrace(self, trace):
pass
def __init__(self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EEPosture, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
pinmodel = sotrobot.dynamic.model
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(
list(position))
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
# Define the reference and the selected DoF
rank = 0
ranks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
ranks += range(idx_v, idx_v + nv)
q[idx_v:idx_v + nv] = position[rank:rank + nv]
rank += nv
assert rank == len(position)
self.tp.feature.posture.value = q
for i in ranks:
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
def __init__ (self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity, hstack, zeros
super(EEPosture, self).__init__()
self.gripper = gripper
# Get joint position in posture
pinmodel = sotrobot.dynamic.model
idJ = pinmodel.getJointId(gripper.sotjoint)
assert idJ < pinmodel.njoints
joint = sotrobot.dynamic.model.joints[idJ]
assert joint.nq == len(position)
idx_q = joint.idx_q + 1
idx_v = joint.idx_v + 1
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position)
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture ('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
q[idx_v:idx_v + joint.nv] = position
self.tp.feature.posture.value = q
self.tp.gain = GainAdaptive("gain_"+n)
robotDim = sotrobot.dynamic.getDimension()
# for i in range (6, robotDim):
# self.tp.feature.selectDof (i, False)
# print idx_q, idx_v
for i in range(joint.nv):
self.tp.feature.selectDof (idx_v + i, True)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
def __init__(self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity
n = Posture.sep + name
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
q = list(sotrobot.dynamic.position.value)
self.tp.feature.state.value = q
self.tp.feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
for i in range(6, robotDim):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.state)
self.tp.setWithDerivative(True)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [self.tp]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": [self._signalPositionRef]
},
"vel_" + name: {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": [self._signalVelocityRef]
},
}
def initPostureTask(robot):
# --- TASK POSTURE --------------------------------------------------
# set a default position for the joints.
robot.features['featurePosition'] = FeaturePosture('featurePosition')
plug(robot.device.state, robot.features['featurePosition'].state)
robotDim = len(robot.dynamic.velocity.value)
robot.features['featurePosition'].posture.value = robot.halfSitting
if robot.device.name == 'HRP2LAAS' or \
robot.device.name == 'HRP2JRL':
postureTaskDofs = [ False,False,False,False,False,False, \
False,False,False,False,False,False, \
True,True,True,True, \
True,True,True,True,True,True,True, \
True,True,True,True,True,True,True ]
elif robot.device.name == 'HRP4LIRMM':
# Right Leg, Left leg, chest, right arm, left arm
postureTaskDofs = [False] * 6 + [False] * 6 + [True] * 4 + [
True
] * 9 + [True] * 9
elif robot.device.name == 'ROMEO':
# chest, left/right arms, left/right legs
postureTaskDofs = [True] * 5 + [True] * 7 + [True] * 7 + [
False
] * 7 + [False] * 7
else:
print "/!\\ walking.py: The robot " + robot.device.name + " is unknown."
print " Default posture task froze all the dofs"
postureTaskDofs = [True] * (robot.dimension - 6)
for dof, isEnabled in enumerate(postureTaskDofs):
robot.features['featurePosition'].selectDof(dof + 6, isEnabled)
robot.tasks['robot_task_position'] = Task('robot_task_position')
robot.tasks['robot_task_position'].add('featurePosition')
# featurePosition.selec.value = toFlags((6,24))
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1, 0.1, 125e3)
gainPosition.gain.value = 5
plug(robot.tasks['robot_task_position'].error, gainPosition.error)
plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
def __init__(self, sotrobot, gripper, name_suffix):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EndEffector, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
self.robot = sotrobot
pinmodel = sotrobot.dynamic.model
self.name = Posture.sep.join(["ee", gripper.name, name_suffix])
self.tp = Task('task' + self.name)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + self.name)
plug(sotrobot.dynamic.position, self.tp.feature.state)
# Select the dofs
self.tp.feature.posture.value = sotrobot.dynamic.position.value
# Define the reference and the selected DoF
self.jointRanks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
self.jointRanks.append((idx_v, nv))
for idx_v, nv in self.jointRanks:
for i in range(idx_v, idx_v + nv):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + self.name)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
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')
robot.cm.addEmergencyStopSIN('distribute')
# -------------------------- 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
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
def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
print("Constructor of LegsFollower Graph")
self.robot = robot
self.solver = solver
self.legsFollower = LegsFollower('legs-follower')
self.statelength = len(robot.device.state.value)
# Initialize the posture task.
print("Posture Task")
self.postureTaskDofs = postureTaskDofs
if not self.postureTaskDofs:
self.postureTaskDofs = []
for i in xrange(len(robot.halfSitting) - 6):
# Disable legs dofs.
if i < 12: #FIXME: not generic enough
self.postureTaskDofs.append((i + 6, False))
else:
self.postureTaskDofs.append((i + 6, True))
# This part is taken from feet_follower_graph
self.postureTask = Task(self.robot.name + '_posture')
self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
plug(self.robot.device.state, self.postureFeature.state)
posture = list(self.robot.halfSitting)
self.postureFeature.setPosture(tuple(posture))
for (dof, isEnabled) in self.postureTaskDofs:
self.postureFeature.selectDof(dof, isEnabled)
self.postureTask.add(self.postureFeature.name)
self.postureTask.controlGain.value = 2.
# Initialize the waist follower task.
print("Waist Task")
self.robot.features['waist'].selec.value = '111111'
plug(self.legsFollower.waist, self.robot.features['waist'].reference)
self.robot.tasks['waist'].controlGain.value = 1.
# Initialize the legs follower task.
print("Legs Task")
self.legsTask = Task(self.robot.name + '_legs')
self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
legsFeatureDesName = self.robot.name + '_legsFeatureDes'
self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
self.legsError = LegsError('LegsError')
plug(self.robot.device.state, self.legsError.state)
# self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value
plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
self.legsFeature.jacobianIN.value = self.legsJacobian()
self.legsFeature.setReference(legsFeatureDesName)
plug(self.legsError.error, self.legsFeature.errorIN)
self.legsTask.add(self.legsFeature.name)
self.legsTask.controlGain.value = 5.
#CoM task
print("Com Task")
print (0., 0., self.robot.dynamic.com.value[2])
self.robot.comTask.controlGain.value = 50.
self.robot.featureComDes.errorIN.value = (0., 0., self.robot.dynamic.com.value[2])
self.robot.featureCom.selec.value = '111'
plug(self.legsFollower.com, self.robot.featureComDes.errorIN)
# Plug the legs follower zmp output signals.
plug(self.legsFollower.zmp, self.robot.device.zmp)
solver.sot.remove(self.robot.comTask.name)
print("Push in solver.")
solver.sot.push(self.legsTask.name)
solver.sot.push(self.postureTask.name)
solver.sot.push(self.robot.tasks['waist'].name)
solver.sot.push(self.robot.comTask.name)
solver.sot.remove(self.robot.tasks['left-ankle'].name)
solver.sot.remove(self.robot.tasks['right-ankle'].name)
print solver.sot.display()
print("Tasks added in solver.\n")
print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
" - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")
class LegsFollowerGraph(object):
legsFollower = None
postureTask = None
postureFeature = None
postureFeatureDes = None
postureError = None
legsTask = None
legsFeature = None
legsFeatureDes = None
legsError = None
waistTask = None
waistFeature = None
waistFeatureDes = None
waistError = None
trace = None
def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
print("Constructor of LegsFollower Graph")
self.robot = robot
self.solver = solver
self.legsFollower = LegsFollower('legs-follower')
self.statelength = len(robot.device.state.value)
# Initialize the posture task.
print("Posture Task")
self.postureTaskDofs = postureTaskDofs
if not self.postureTaskDofs:
self.postureTaskDofs = []
for i in xrange(len(robot.halfSitting) - 6):
# Disable legs dofs.
if i < 12: #FIXME: not generic enough
self.postureTaskDofs.append((i + 6, False))
else:
self.postureTaskDofs.append((i + 6, True))
# This part is taken from feet_follower_graph
self.postureTask = Task(self.robot.name + '_posture')
self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
plug(self.robot.device.state, self.postureFeature.state)
posture = list(self.robot.halfSitting)
self.postureFeature.setPosture(tuple(posture))
for (dof, isEnabled) in self.postureTaskDofs:
self.postureFeature.selectDof(dof, isEnabled)
self.postureTask.add(self.postureFeature.name)
self.postureTask.controlGain.value = 2.
# Initialize the waist follower task.
print("Waist Task")
self.robot.features['waist'].selec.value = '111111'
plug(self.legsFollower.waist, self.robot.features['waist'].reference)
self.robot.tasks['waist'].controlGain.value = 1.
# Initialize the legs follower task.
print("Legs Task")
self.legsTask = Task(self.robot.name + '_legs')
self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
legsFeatureDesName = self.robot.name + '_legsFeatureDes'
self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
self.legsError = LegsError('LegsError')
plug(self.robot.device.state, self.legsError.state)
# self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value
plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
self.legsFeature.jacobianIN.value = self.legsJacobian()
self.legsFeature.setReference(legsFeatureDesName)
plug(self.legsError.error, self.legsFeature.errorIN)
self.legsTask.add(self.legsFeature.name)
self.legsTask.controlGain.value = 5.
#CoM task
print("Com Task")
print (0., 0., self.robot.dynamic.com.value[2])
self.robot.comTask.controlGain.value = 50.
self.robot.featureComDes.errorIN.value = (0., 0., self.robot.dynamic.com.value[2])
self.robot.featureCom.selec.value = '111'
plug(self.legsFollower.com, self.robot.featureComDes.errorIN)
# Plug the legs follower zmp output signals.
plug(self.legsFollower.zmp, self.robot.device.zmp)
solver.sot.remove(self.robot.comTask.name)
print("Push in solver.")
solver.sot.push(self.legsTask.name)
solver.sot.push(self.postureTask.name)
solver.sot.push(self.robot.tasks['waist'].name)
solver.sot.push(self.robot.comTask.name)
solver.sot.remove(self.robot.tasks['left-ankle'].name)
solver.sot.remove(self.robot.tasks['right-ankle'].name)
print solver.sot.display()
print("Tasks added in solver.\n")
print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
" - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")
def legsJacobian(self):
j = []
for i in xrange(12):
j.append(oneVector(6+i,self.statelength))
return tuple(j)
def waistJacobian(self):
j = []
for i in xrange(6):
j.append(oneVector(i,self.statelength))
return tuple(j)
def postureJacobian(self):
j = []
for i in xrange(self.statelength):
if i >= 6 + 2 * 6:
j.append(oneVector(i))
if i == 3 or i == 4:
j.append(zeroVector())
return tuple(j)
def computeDesiredValue(self):
e = self.robot.halfSitting
#e = halfSitting
e_ = [e[3], e[4]]
offset = 6 + 2 * 6
for i in xrange(len(e) - offset):
e_.append(e[offset + i])
return tuple(e_)
def canStart(self):
securityThreshold = 1e-3
return (self.postureTask.error.value <=
(securityThreshold,) * len(self.postureTask.error.value))
def setupTrace(self):
self.trace = TracerRealTime('trace')
self.trace.setBufferSize(2**20)
self.trace.open('/tmp/','legs_follower_','.dat')
self.trace.add('legs-follower.com', 'com')
self.trace.add('legs-follower.zmp', 'zmp')
self.trace.add('legs-follower.ldof', 'ldof')
self.trace.add('legs-follower.waist', 'waist')
self.trace.add(self.robot.device.name + '.state', 'state')
self.trace.add(self.legsTask.name + '.error', 'errorLegs')
self.trace.add(self.robot.comTask.name + '.error', 'errorCom')
#self.trace.add('legs-follower.outputStart','start')
#self.trace.add('legs-follower.outputYaw','yaw')
self.trace.add('corba.planner_steps','steps')
self.trace.add('corba.planner_outputGoal','goal')
self.trace.add('corba.waist','waistMocap')
self.trace.add('corba.left-foot','footMocap')
self.trace.add('corba.table','tableMocap')
self.trace.add('corba.bar','barMocap')
self.trace.add('corba.chair','chairMocap')
self.trace.add('corba.helmet','helmetMocap')
self.trace.add('corba.planner_outputObs','obstacles')
self.trace.add(self.robot.dynamic.name + '.left-ankle',
self.robot.dynamic.name + '-left-ankle')
self.trace.add(self.robot.dynamic.name + '.right-ankle',
self.robot.dynamic.name + '-right-ankle')
# Recompute trace.triger at each iteration to enable tracing.
self.robot.device.after.addSignal('legs-follower.zmp')
self.robot.device.after.addSignal('legs-follower.outputStart')
self.robot.device.after.addSignal('legs-follower.outputYaw')
self.robot.device.after.addSignal('corba.planner_steps')
self.robot.device.after.addSignal('corba.planner_outputGoal')
self.robot.device.after.addSignal('corba.waist')
self.robot.device.after.addSignal('corba.left-foot')
self.robot.device.after.addSignal('corba.table')
self.robot.device.after.addSignal('corba.bar')
self.robot.device.after.addSignal('corba.chair')
self.robot.device.after.addSignal('corba.helmet')
self.robot.device.after.addSignal('corba.planner_outputObs')
self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle')
self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle')
self.robot.device.after.addSignal('trace.triger')
return
def plugPlanner(self):
print("Plug planner.")
plug(corba.planner_radQ, self.legsFollower.inputRef)
plug(self.legsFollower.outputStart, corba.planner_inputStart)
plug(corba.waistTimestamp, corba.planner_timestamp)
plug(corba.table, corba.planner_table)
plug(corba.chair, corba.planner_chair)
plug(corba.bar, corba.planner_bar)
plug(corba.waist, corba.planner_waist)
plug(corba.helmet, corba.planner_inputGoal)
plug(corba.torus1, corba.planner_torus1)
plug(corba.torus2, corba.planner_torus2)
plug(corba.signal('left-foot'), corba.planner_foot)
plug(corba.signal('left-footTimestamp'), corba.planner_footTimestamp)
return
def plugPlannerWithoutMocap(self):
print("Plug planner without mocap.")
plug(corba.planner_radQ, self.legsFollower.inputRef)
plug(self.legsFollower.outputStart, corba.planner_inputStart)
return
def plugViewer(self):
print("Plug viewer.")
plug(self.legsFollower.ldof, corba.viewer_Ldof)
plug(self.legsFollower.outputStart, corba.viewer_Start)
plug(self.legsFollower.com, corba.viewer_Com)
plug(self.legsFollower.outputYaw, corba.viewer_Yaw)
plug(corba.planner_steps, corba.viewer_Steps)
plug(corba.planner_outputGoal, corba.viewer_Goal)
plug(corba.table, corba.viewer_Table)
plug(corba.chair, corba.viewer_Chair)
plug(corba.bar, corba.viewer_Bar)
plug(corba.torus1, corba.viewer_Torus1)
plug(corba.torus2, corba.viewer_Torus2)
plug(corba.waist, corba.viewer_Waist)
plug(corba.planner_outputLabyrinth, corba.viewer_Labyrinth)
plug(corba.planner_outputObs, corba.viewer_Obs)
return
def plug(self):
self.plugPlanner()
self.plugViewer()
return
def readMocap(self):
print "Table : "
print corba.table.value
print "Waist : "
if len(corba.waist.value)<3:
corba.waist.value = (0,0,0)
print corba.waist.value
print "Helmet : "
print corba.helmet.value
return;
def start(self):
if not self.canStart():
print("Robot has not yet converged to the initial position,"
" please wait and try again.")
return
print("Start.")
self.postureTask.controlGain.value = 180.
#self.waistTask.controlGain.value = 90.
self.legsTask.controlGain.value = 180.
self.robot.comTask.controlGain.value = 180.
self.robot.tasks['waist'].controlGain.value = 45.
self.setupTrace()
self.trace.start()
self.legsFollower.start()
return
def stop(self):
self.legsFollower.stop()
self.trace.dump()
return
controller.init(timeStep, 1)
controller.setPosition([robot.device.state.value[LeftRollJoint + 6]])
robot.leftRollAnkleController = controller
robot.leftAnkleRollTask = MetaTaskKineJoint(robot.dynamic, LeftRollJoint)
robot.leftAnkleRollTask.task.controlGain.value = 0
robot.leftAnkleRollTask.task.setWithDerivative(True)
plug(robot.leftRollAnkleController.qRef,
robot.leftAnkleRollTask.featureDes.errorIN)
plug(robot.leftRollAnkleController.dqRef,
robot.leftAnkleRollTask.featureDes.errordotIN)
# -------------------------- SOT CONTROL --------------------------
# --- Posture
robot.taskPosture = Task('taskPosture')
robot.taskPosture.feature = FeaturePosture('featurePosture')
q = list(robot.dynamic.position.value)
robot.taskPosture.feature.state.value = q
robot.taskPosture.feature.posture.value = q
for i in range(18, 38):
robot.taskPosture.feature.selectDof(i, True)
robot.taskPosture.controlGain.value = 100.0
robot.taskPosture.add(robot.taskPosture.feature.name)
plug(robot.dynamic.position, robot.taskPosture.feature.state)
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT, FeatureGeneric, FeaturePosture, GainAdaptive, Task
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.tiago.diff_drive_controller import HolonomicProjection
n = "posture"
tp = Task('task' + n)
tp.dyn = robot.dynamic
tp.feature = FeaturePosture('feature_' + n)
q = list(robot.device.robotState.value)
tp.feature.state.value = q
tp.feature.posture.value = q
robotDim = robot.dynamic.getDimension()
assert robotDim == len(q)
with_wheels = robotDim == 19
for i in range(8 if with_wheels else 6, robotDim):
tp.feature.selectDof(i, True)
tp.gain = GainAdaptive("gain_" + n)
tp.add(tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(robot.dynamic.position, tp.feature.state)
def __init__ (self, robot, solver):
self.samplingPeriod = .005
self.robot = robot
self.solver = solver
self.ros = Ros (robot)
self.ros.rosExport.add ('matrixHomoStamped', 'ballInCamera',
'/wide/blobs/rose/transform')
self.ros.rosExport.add ('matrixHomoStamped', 'ballInWaist',
'/wide/blobs/rose/transform_base_link')
self.rightGripperId = 28
# Right hand task
self.featureRightHand = \
FeaturePosition ('featureRightHand',
robot.frames ['rightHand'].position,
robot.frames ['rightHand'].jacobian)
self.featureRightHand.selec.value = '000111'
# interpolation
self.prodSE3 = Multiply_of_matrixHomo ("prod SE3")
self.interpolation = CylindricalCubicInterpolationSE3 ('interpolation')
self.interpolation.setSamplingPeriod (self.samplingPeriod)
plug (self.robot.waist.position, self.prodSE3.sin1)
plug (self.interpolation.reference,
self.featureRightHand.signal('reference'))
plug (self.ros.rosExport.ballInWaist, self.prodSE3.sin2)
plug (self.prodSE3.sout, self.interpolation.goal)
plug (self.featureRightHand.position, self.interpolation.init)
plug (self.robot.waist.position, self.interpolation.localFrame)
self.taskRightHand = TaskPD ('taskRightHand')
self.taskRightHand.add (self.featureRightHand.name)
#plug (interpolation.errorDot, taskRightHand.errorDot)
self.taskRightHand.controlGain.value = 180.0
self.solver.push (self.taskRightHand)
# Waist task
self.robot.waist.selec.value = '011000'
self.robot.waist.value = I4
self.solver.push (self.robot.tasks ['waist'])
# Ball tracking
refBallInCamera = FeatureVisualPoint ('featureBallInCameraRef')
refBallInCamera.xy.value = (0., 0.)
self.pinholeProjection = VispPointProjection('pinholeProjection')
plug (self.ros.rosExport.ballInCamera, self.pinholeProjection.cMo)
plug (self.ros.rosTime.time, self.pinholeProjection.time)
plug (self.ros.rosExport.ballInCameraTimestamp,
self.pinholeProjection.cMoTimestamp)
self.ballInCamera = FeatureVisualPoint ('featureBallInCamera')
plug (self.pinholeProjection.xy, self.ballInCamera.xy)
self.ballInCamera.Z.value = 1.
self.ballInCamera.setReference (refBallInCamera.name)
self.ballInCamera.selec.value = '11'
plug (self.robot.frames ['cameraTopRight'].jacobian,
self.ballInCamera.Jq)
self.taskBallTracking = TaskPD ('taskBallTracking')
self.taskBallTracking.add (self.ballInCamera.name)
self.taskBallTracking.controlGain.value = 1.0
# Posture task
self.posture = list (self.robot.halfSitting [::])
self.featurePosture = FeaturePosture ('featurePosture')
plug (self.robot.device.state, self.featurePosture.state)
self.featurePosture.setPosture (tuple (self.posture))
for dof in range (6, self.robot.dimension):
self.featurePosture.selectDof (dof, False)
self.postureTask = TaskPD ('postureTask')
self.postureTask.add (self.featurePosture.name)
self.postureTask.controlGain.value = 1.
self.solver.push (self.postureTask)
class Motion (object):
reachTime = 3.
stillTime = .5
def __init__ (self, robot, solver):
self.samplingPeriod = .005
self.robot = robot
self.solver = solver
self.ros = Ros (robot)
self.ros.rosExport.add ('matrixHomoStamped', 'ballInCamera',
'/wide/blobs/rose/transform')
self.ros.rosExport.add ('matrixHomoStamped', 'ballInWaist',
'/wide/blobs/rose/transform_base_link')
self.rightGripperId = 28
# Right hand task
self.featureRightHand = \
FeaturePosition ('featureRightHand',
robot.frames ['rightHand'].position,
robot.frames ['rightHand'].jacobian)
self.featureRightHand.selec.value = '000111'
# interpolation
self.prodSE3 = Multiply_of_matrixHomo ("prod SE3")
self.interpolation = CylindricalCubicInterpolationSE3 ('interpolation')
self.interpolation.setSamplingPeriod (self.samplingPeriod)
plug (self.robot.waist.position, self.prodSE3.sin1)
plug (self.interpolation.reference,
self.featureRightHand.signal('reference'))
plug (self.ros.rosExport.ballInWaist, self.prodSE3.sin2)
plug (self.prodSE3.sout, self.interpolation.goal)
plug (self.featureRightHand.position, self.interpolation.init)
plug (self.robot.waist.position, self.interpolation.localFrame)
self.taskRightHand = TaskPD ('taskRightHand')
self.taskRightHand.add (self.featureRightHand.name)
#plug (interpolation.errorDot, taskRightHand.errorDot)
self.taskRightHand.controlGain.value = 180.0
self.solver.push (self.taskRightHand)
# Waist task
self.robot.waist.selec.value = '011000'
self.robot.waist.value = I4
self.solver.push (self.robot.tasks ['waist'])
# Ball tracking
refBallInCamera = FeatureVisualPoint ('featureBallInCameraRef')
refBallInCamera.xy.value = (0., 0.)
self.pinholeProjection = VispPointProjection('pinholeProjection')
plug (self.ros.rosExport.ballInCamera, self.pinholeProjection.cMo)
plug (self.ros.rosTime.time, self.pinholeProjection.time)
plug (self.ros.rosExport.ballInCameraTimestamp,
self.pinholeProjection.cMoTimestamp)
self.ballInCamera = FeatureVisualPoint ('featureBallInCamera')
plug (self.pinholeProjection.xy, self.ballInCamera.xy)
self.ballInCamera.Z.value = 1.
self.ballInCamera.setReference (refBallInCamera.name)
self.ballInCamera.selec.value = '11'
plug (self.robot.frames ['cameraTopRight'].jacobian,
self.ballInCamera.Jq)
self.taskBallTracking = TaskPD ('taskBallTracking')
self.taskBallTracking.add (self.ballInCamera.name)
self.taskBallTracking.controlGain.value = 1.0
# Posture task
self.posture = list (self.robot.halfSitting [::])
self.featurePosture = FeaturePosture ('featurePosture')
plug (self.robot.device.state, self.featurePosture.state)
self.featurePosture.setPosture (tuple (self.posture))
for dof in range (6, self.robot.dimension):
self.featurePosture.selectDof (dof, False)
self.postureTask = TaskPD ('postureTask')
self.postureTask.add (self.featurePosture.name)
self.postureTask.controlGain.value = 1.
self.solver.push (self.postureTask)
def trackBall (self):
self.solver.push (self.taskBallTracking)
def openHand (self):
# open hand
self.featurePosture.selectDof (self.rightGripperId, True)
self.posture [self.rightGripperId] = 0.7
self.featurePosture.setPosture (tuple (self.posture))
def reach (self):
# move hand
self.handInitPos = self.robot.frames ['rightHand'].position.value
self.interpolation.start (self.reachTime)
def stopTracking (self):
self.solver.remove (self.taskBallTracking)
def closeHand (self):
self.posture [self.rightGripperId] = 0.3
self.featurePosture.setPosture (tuple (self.posture))
self.postureTask.controlGain.value = 4.
def moveBack (self):
# move hand back
self.postureTask.controlGain.value = 0.
for dof in range (6, len (self.posture)):
self.featurePosture.selectDof (dof, True)
self.interpolation.goal.value = self.handInitPos
self.postureTask.controlGain.value = 3.
self.interpolation.start (2.)
def stopMotion (self):
self.solver.remove (self.taskBallTracking)
plug (self.ros.rosExport.ballInWaist, self.interpolation.goal)
def catchBall (self):
#self.trackBall ()
self.openHand ()
self.reach ()
time.sleep (self.reachTime)
self.closeHand ()
time.sleep (1.)
self.moveBack ()
self.stopMotion ()
def play(self, totalTime):
nbSteps = int(totalTime/timeStep) + 1
path = []
for i in xrange(nbSteps):
print (i)
if i == 100:
self.start ()
t = timeStep*i
self.robot.device.increment(timeStep)
config = self.robot.device.state.value
path.append(config)
if hasRobotViewer:
clt.updateElementConfig(rvName, toViewerConfig(config))
ballPos = []
for i in range (3):
ballPos.append(self.ros.rosExport.ballInWaist.value[i][3])
ballPos.extend (3*[0])
clt.updateElementConfig("rose_ball", ballPos)