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 __init__ (self,name,controllers,threshold_up,threshold_down):
"""
Use controller 0 until the condition signal value becomes greater than threshold.
Then use controller 1. Manually switch between controller using the latch.
OFF means controller 0 and ON means controller 1.
Currently support only two controllers.
- controllers: output signal of a controller
"""
self.reverse = (threshold_up < threshold_down)
# any(threshold_up < measured_torque) => torque control
self._condition_up = CompareVector (name + "_condition_up")
self._condition_up.setTrueIfAny(True)
self._condition_up.sin1.value = threshold_up
self._condition_up.sin2.value = threshold_up
# all(measured_torque < threshold_down) => position control
self._condition_down = CompareVector (name + "_condition_down")
self._condition_down.setTrueIfAny(False)
self._condition_down.sin1.value = threshold_down
self._condition_down.sin2.value = threshold_down
self._event_up = Event (name + "_event_up")
self._event_down = Event (name + "_event_down")
self._event_up .setOnlyUp(True);
self._event_down .setOnlyUp(True);
self._latch = Latch(name + "_latch")
self._latch.turnOff()
self._switch = SwitchVector (name + "_switch")
self._switch.setSignalNumber(len(controllers))
plug(self._condition_up .sout, self._event_up .condition)
plug(self._condition_down.sout, self._event_down.condition)
plug(self._latch.out , self._switch.boolSelection)
# This is necessary to initialize the event (the first recompute triggers the event...)
self._event_up.check.recompute(0)
self._event_down.check.recompute(0)
self._event_up .addSignal (name + "_latch.turnOnSout")
self._event_down.addSignal (name + "_latch.turnOffSout")
for n, sig in enumerate(controllers):
plug(sig, self.signalIn(n))
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
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
## omega -> theta
# Done in _makeControllerSwich
# A delay is necessary to avoid loops in SoT
delayTheta = DelayVector(self.name + "_sim_theta_delay")
delayTheta.setMemory(tuple([0. for _ in self.est_theta_closed]))
plug(self.omega2theta.sout, delayTheta.sin)
self.setCurrentPositionIn(delayTheta.previous)
## theta -> phi = theta - theta0
self.theta2phi = Add_of_vector(self.name + "_sim_theta2phi")
self.theta2phi.setCoeff1(1)
self.theta2phi.setCoeff2(-1)
plug(delayTheta.current, self.theta2phi.sin1)
self.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_switch = SwitchVector(self.name + "_sim_torque")
self.sim_switch.setSignalNumber(2)
plug(self.sim_contact_condition.sout, self.sim_switch.boolSelection)
# Non contact phase
if reverse:
plug(self.theta2phi.sout, self.sim_contact_condition.sin2)
self.sim_contact_condition.sin1.value = [
0. for _ in self.est_theta_closed
]
else:
plug(self.theta2phi.sout, self.sim_contact_condition.sin1)
self.sim_contact_condition.sin2.value = [
0. for _ in self.est_theta_closed
]
# 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.theta2phi.sout, self.phi2torque.reference)
# Condition
# if phi < 0 -> no contact -> torque = 0
self.sim_switch.sin1.value = [0. for _ in self.est_theta_closed]
# else -> contact -> phi2torque
plug(self.phi2torque.output, self.sim_switch.sin0)
delay = DelayVector(self.name + "_sim_torque_delay")
delay.setMemory(tuple([0. for _ in self.est_theta_closed]))
# plug (self.phi2torque.output, delay.sin)
plug(self.sim_switch.sout, delay.sin)
# self.setCurrentConditionIn (delay.current)
plug(delay.previous, self.currentTorqueIn)