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
class AdmittanceControl(object):
"""
The torque controller is then use to maintain a desired force.
It outputs a velocity command to be sent to entity Device.
"""
def __init__(self, name, estimated_theta_closed, desired_torque, period,
nums, denoms):
"""
- estimated_theta_closed: Use for the initial position control. It should correspond to a configuration in collision.
The closer to contact configuration, the least the overshoot.
- desired_torque: The torque to be applied on the object.
- period: The SoT integration time.
- nums, denoms: coefficient of the controller:
\sum_i denoms[i] * d^i theta / dt^i = \sum_j nums[j] d^j torque / dt^j
"""
self.name = name
self.est_theta_closed = estimated_theta_closed
self.desired_torque = desired_torque
self.dt = period
self._makeTorqueControl(nums, denoms)
self._makeIntegrationOfVelocity()
### Feed-forward - contact phase
def _makeTorqueControl(self, nums, denoms):
from agimus_sot.control.controllers import Controller
self.torque_controller = Controller(
self.name + "_torque", nums, denoms, self.dt,
[0. for _ in self.est_theta_closed])
self.torque_controller.addFeedback()
self.torque_controller.reference.value = self.desired_torque
### Internal method
def _makeIntegrationOfVelocity(self):
from dynamic_graph.sot.core import Add_of_vector
self.omega2theta = Add_of_vector(self.name + "_omega2theta")
self.omega2theta.setCoeff2(self.dt)
# self.omega2theta.sin1 <- current position
# self.omega2theta.sin2 <- desired velocity
plug(self.torque_controller.output, self.omega2theta.sin2)
### Setup event to tell when object is grasped and simulate torque feedback.
def setupFeedbackSimulation(self, mass, damping, spring, theta0):
from agimus_sot.control.controllers import Controller
from dynamic_graph.sot.core import Add_of_vector
from agimus_sot.sot import DelayVector
## theta -> phi = theta - theta0
self._sim_theta2phi = Add_of_vector(self.name + "_sim_theta2phi")
self._sim_theta2phi.setCoeff1(1)
self._sim_theta2phi.setCoeff2(-1)
self._sim_theta2phi.sin2.value = theta0
## phi -> torque
from dynamic_graph.sot.core.switch import SwitchVector
from dynamic_graph.sot.core.operator import CompareVector
# reverse = self.theta_open[0] > theta0[0]
reverse = self.desired_torque[0] < 0
self.sim_contact_condition = CompareVector(self.name +
"_sim_contact_condition")
self.sim_contact_condition.setTrueIfAny(False)
self._sim_torque = SwitchVector(self.name + "_sim_torque")
self._sim_torque.setSignalNumber(2)
plug(self.sim_contact_condition.sout, self._sim_torque.boolSelection)
# If phi < 0 (i.e. sim_contact_condition.sout == 1) -> non contact phase
# else contact phase
if reverse:
plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin2)
self.sim_contact_condition.sin1.value = [
0. for _ in self.est_theta_closed
]
else:
plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin1)
self.sim_contact_condition.sin2.value = [
0. for _ in self.est_theta_closed
]
# Non contact phase
# torque = 0, done above
# Contact phase
self.phi2torque = Controller(self.name + "_sim_phi2torque", (
spring,
damping,
mass,
), (1., ), self.dt, [0. for _ in self.est_theta_closed])
#TODO if M != 0: phi2torque.pushNumCoef(((M,),))
plug(self._sim_theta2phi.sout, self.phi2torque.reference)
# Condition
# if phi < 0 -> no contact -> torque = 0
self._sim_torque.sin1.value = [0. for _ in self.est_theta_closed]
# else -> contact -> phi2torque
plug(self.phi2torque.output, self._sim_torque.sin0)
plug(self._sim_torque.sout, self.currentTorqueIn)
def readPositionsFromRobot(self, robot, jointNames):
# TODO Compare current position to self.est_theta_closed and
# so as not to overshoot this position.
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._joint_selec = Selec_of_vector(self.name + "_joint_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
self._joint_selec.addSelec(jmodel.idx_v, jmodel.idx_v + jmodel.nv)
plug(robot.dynamic.position, self._joint_selec.sin)
self.setCurrentPositionIn(self._joint_selec.sout)
## - param torque_constants: Should take into account the motor torque constant and the gear ratio.
## - param first_order_filter: Add a first order filter to the current signal.
def readCurrentsFromRobot(self,
robot,
jointNames,
torque_constants,
first_order_filter=False):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._current_selec = Selec_of_vector(self.name + "_current_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._current_selec.addSelec(jmodel.idx_v - 6,
jmodel.idx_v - 6 + jmodel.nv)
from dynamic_graph.sot.core.operator import Multiply_of_vector
plug(robot.device.currents, self._current_selec.sin)
self._multiply_by_torque_constants = Multiply_of_vector(
self.name + "_multiply_by_torque_constants")
self._multiply_by_torque_constants.sin0.value = torque_constants
if first_order_filter:
from agimus_sot.control.controllers import Controller
self._first_order_current_filter = Controller(
self.name + "_first_order_current_filter", (5., ), (
5.,
1.,
), self.dt, [0. for _ in self.desired_torque])
plug(self._current_selec.sout,
self._first_order_current_filter.reference)
plug(self._first_order_current_filter.output,
self._multiply_by_torque_constants.sin1)
else:
plug(self._current_selec.sout,
self._multiply_by_torque_constants.sin1)
plug(self._multiply_by_torque_constants.sout, self.currentTorqueIn)
def readTorquesFromRobot(self, robot, jointNames):
# Input formattting
from dynamic_graph.sot.core.operator import Selec_of_vector
self._torque_selec = Selec_of_vector(self.name + "_torque_selec")
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
assert jid < len(model.joints)
jmodel = model.joints[jid]
# TODO there is no value for the 6 first DoF
assert jmodel.idx_v >= 6
self._torque_selec.addSelec(jmodel.idx_v - 6,
jmodel.idx_v - 6 + jmodel.nv)
plug(robot.device.ptorques, self._torque_selec.sin)
plug(self._torque_selec.sout, self.currentTorqueIn)
# TODO remove me
def addOutputTo(self, robot, jointNames, mix_of_vector, sot=None):
#TODO assert isinstance(mix_of_vector, Mix_of_vector)
i = mix_of_vector.getSignalNumber()
mix_of_vector.setSignalNumber(i + 1)
plug(self.outputVelocity, mix_of_vector.signal("sin" + str(i)))
model = robot.dynamic.model
for jn in jointNames:
jid = model.getJointId(jn)
jmodel = model.joints[jid]
mix_of_vector.addSelec(i, jmodel.idx_v, jmodel.nv)
def addTracerRealTime(self, robot):
from dynamic_graph.tracer_real_time import TracerRealTime
from agimus_sot.tools import filename_escape
self._tracer = TracerRealTime(self.name + "_tracer")
self._tracer.setBufferSize(10 * 1048576) # 10 Mo
self._tracer.open("/tmp", filename_escape(self.name), ".txt")
self._tracer.add(self.omega2theta.name + ".sin1", "_theta_current")
self._tracer.add(self.omega2theta.name + ".sin2", "_omega_desired")
self._tracer.add(self.omega2theta.name + ".sout", "_theta_desired")
self._tracer.add(self.torque_controller.referenceName,
"_reference_torque")
self._tracer.add(self.torque_controller.measurementName,
"_measured_torque")
if hasattr(self, "_current_selec"):
self._tracer.add(self._current_selec.name + ".sout",
"_measured_current")
robot.device.after.addSignal(self._tracer.name + ".triger")
return self._tracer
@property
def outputPosition(self):
return self.omega2theta.sout
@property
def referenceTorqueIn(self):
return self.torque_controller.reference
def setCurrentPositionIn(self, sig):
plug(sig, self.omega2theta.sin1)
if hasattr(self, "_sim_theta2phi"):
plug(sig, self._sim_theta2phi.sin1)
@property
def currentTorqueIn(self):
return self.torque_controller.measurement
@property
def torqueConstants(self):
return self._multiply_by_torque_constants.sin0
