class Supervisor(object):
"""
Steps: P = placement, G = grasp, p = pre-P, g = pre-G
0. P <-> GP
1. P <-> gP
2. gP <-> GP
3. GP <-> G
4. GP <-> Gp
5. Gp <-> G
"""
##
# \param lpTasks list of low priority tasks. If None, a Posture task will be used.
# \param hpTasks list of high priority tasks (like balance)
def __init__(self, sotrobot, lpTasks=None, hpTasks=None):
self.sotrobot = sotrobot
self.hpTasks = hpTasks if hpTasks is not None else _hpTasks(sotrobot)
self.lpTasks = lpTasks if lpTasks is not None else _lpTasks(sotrobot)
self.currentSot = None
from dynamic_graph.sot.core.switch import SwitchVector
self.sot_switch = SwitchVector("sot_supervisor_switch")
plug(self.sot_switch.sout, self.sotrobot.device.control)
from agimus_sot.events import Events
self.done_events = Events("done", sotrobot)
self.error_events = Events("error", sotrobot)
self.done_events.setupNormOfControl(sotrobot.device.control, 1e-2)
self.done_events.setupTime(
) # For signal self. done_events.timeEllapsedSignal
self.error_events.setupTime(
) # For signal self.error_events.timeEllapsedSignal
def makeInitialSot(self):
# Create the initial sot (keep)
from .solver import Solver
sot = Solver('sot_keep', self.sotrobot.dynamic.getDimension())
self.keep_posture = Posture("posture_keep", self.sotrobot)
self.keep_posture._task.setWithDerivative(False)
self.keep_posture._signalPositionRef(
).value = self.sotrobot.dynamic.position.value
self.keep_posture.pushTo(sot)
sot.doneSignal = self.done_events.controlNormSignal
sot.errorSignal = False
self.addSolver("", sot)
## Set the robot base pose in the world.
# \param basePose a list: [x,y,z,r,p,y] or [x,y,z,qx,qy,qz,qw]
# \return success True in case of success
def setBasePose(self, basePose):
if len(basePose) == 7:
# Currently, this case never happens
from dynamic_graph.sot.tools.quaternion import Quaternion
from numpy.linalg import norm
q = Quaternion(basePose[6], basePose[3], basePose[4], basePose[5])
if abs(norm(q.array) - 1.) > 1e-2:
return False, "Quaternion is not normalized"
basePose = basePose[:3] + q.toRPY().tolist()
if self.currentSot == "" or len(basePose) != 6:
# We are using the SOT to keep the current posture.
# The 6 first DoF are not used by the task so we can change them safely.
self.sotrobot.device.set(
tuple(basePose + list(self.sotrobot.device.state.value[6:])))
self.keep_posture._signalPositionRef(
).value = self.sotrobot.device.state.value
return True
else:
return False
## \name SoT managements
## \{
def addPreAction(self, name, preActionSolver):
self.preActions[name] = preActionSolver
self._addSignalToSotSwitch(preActionSolver)
def addSolver(self, name, solver):
self.sots[name] = solver
self._addSignalToSotSwitch(solver)
def duplicateSolver(self, existingSolver, newSolver):
self.sots[newSolver] = self.sots[existingSolver]
def addPostActions(self, name, postActionSolvers):
self.postActions[name] = postActionSolvers
for targetState, pa_sot in postActionSolvers.iteritems():
self._addSignalToSotSwitch(pa_sot)
## This is for internal purpose
def _addSignalToSotSwitch(self, solver):
n = self.sot_switch.getSignalNumber()
self.sot_switch.setSignalNumber(n + 1)
self.sots_indexes[solver.name] = n
plug(solver.control, self.sot_switch.signal("sin" + str(n)))
def _plug(e, events, n, name):
assert events.getSignalNumber() == n, "Wrong number of events."
events.setSignalNumber(n + 1)
events.setConditionString(n, name)
if isinstance(e, (bool, int)):
events.conditionSignal(n).value = int(e)
else:
plug(e, events.conditionSignal(n))
_plug(solver.doneSignal, self.done_events, n, solver.name)
_plug(solver.errorSignal, self.error_events, n, solver.name)
def _selectSolver(self, solver):
n = self.sots_indexes[solver.name]
self.sot_switch.selection.value = n
self.done_events.setSelectedSignal(n)
self.error_events.setSelectedSignal(n)
## \}
def topics(self):
c = self.hpTasks + self.lpTasks
for g in self.grasps.values():
c += g
for p in self.placements.values():
c += p
return c.topics
def plugTopicsToRos(self):
from dynamic_graph.ros.ros_queued_subscribe import RosQueuedSubscribe
self.rosSubscribe = RosQueuedSubscribe('ros_queued_subscribe')
from dynamic_graph.ros.ros_tf_listener import RosTfListener
self.rosTf = RosTfListener('ros_tf_listener')
topics = self.topics()
for name, topic_info in topics.items():
topic_handler = _handlers[topic_info.get("handler", "default")]
topic_handler(name, topic_info, self.rosSubscribe, self.rosTf)
def printQueueSize(self):
exec("tmp = " + self.rosSubscribe.list())
for l in tmp:
print(l, self.rosSubscribe.queueSize(l))
## Check consistency between two SoTs.
#
# This is not used anymore because it must be synchronized with the real-time thread.
# \todo Re-enable consistency check between two SoTs.
def isSotConsistentWithCurrent(self, transitionName, thr=1e-3):
if self.currentSot is None or transitionName == self.currentSot:
return True
csot = self.sots[self.currentSot]
nsot = self.sots[transitionName]
t = self.sotrobot.device.control.time
# This is not safe since it would be run concurrently with the
# real time thread.
csot.control.recompute(t)
nsot.control.recompute(t)
from numpy import array, linalg
error = array(nsot.control.value) - array(csot.control.value)
n = linalg.norm(error)
if n > thr:
print("Control not consistent:", linalg.norm(error), '\n', error)
return False
return True
def clearQueues(self):
self.rosSubscribe.readQueue(-1)
exec("tmp = " + self.rosSubscribe.list())
for s in tmp:
print('{} queue size: {}'.format(s,
self.rosSubscribe.queueSize(s)))
self.rosSubscribe.clearQueue(s)
## Wait for the queue to be of a given size.
# \param minQueueSize (integer) waits to the queue size of rosSubscribe
# to be greater or equal to \c minQueueSize
# \param timeout time in seconds after which to return a failure.
# \return True on success, False on timeout.
def waitForQueue(self, minQueueSize, timeout):
ts = self.sotrobot.device.getTimeStep()
to = int(timeout / self.sotrobot.device.getTimeStep())
from time import sleep
start_it = self.sotrobot.device.control.time
exec("queues = " + self.rosSubscribe.list())
for queue in queues:
while self.rosSubscribe.queueSize(queue) < minQueueSize:
if self.sotrobot.device.control.time > start_it + to:
return False, "Queue {} has received {} points.".format(
queue, self.rosSubscribe.queueSize(queue))
sleep(ts)
return True, ""
## Start reading values received by the RosQueuedSubscribe entity.
# \param delay (integer) how many periods to wait before reading.
# It allows to give some delay to network connection.
# \param minQueueSize (integer) waits to the queue size of rosSubscribe
# to be greater or equal to \p minQueueSize
# \param duration expected duration (in seconds) of the queue.
# \param timeout time in seconds after which to return a failure.
# \return success, time boolean, SoT time at which reading starts (invalid if success is False)
#
# \warning If \p minQueueSize is greater than the number of values to
# be received by rosSubscribe, this function does an infinite loop.
def readQueue(self, delay, minQueueSize, duration, timeout):
from time import sleep
print("Current solver {0}".format(self.currentSot))
if delay < 0:
print("Delay argument should be >= 0")
return False, -1
minSizeReached, msg = self.waitForQueue(minQueueSize, timeout)
if not minSizeReached:
return False, -1
durationStep = int(duration / self.sotrobot.device.getTimeStep())
t = self.sotrobot.device.control.time + delay
self.rosSubscribe.readQueue(t)
self.done_events.setFutureTime(t + durationStep)
self.error_events.setFutureTime(t + durationStep)
return True, t
def stopReadingQueue(self):
self.rosSubscribe.readQueue(-1)
# \return success, time boolean, SoT time at which reading starts (invalid if success is False)
def plugSot(self, transitionName, check=False):
if check and not self.isSotConsistentWithCurrent(transitionName):
# raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
print("Sot {0} not consistent with sot {1}".format(
self.currentSot, transitionName))
if transitionName == "":
self.keep_posture._signalPositionRef(
).value = self.sotrobot.dynamic.position.value
solver = self.sots[transitionName]
# No done events should be triggered before call
# to readQueue. We expect it to happen with 1e6 milli-seconds
# from now...
devicetime = self.sotrobot.device.control.time
self.done_events.setFutureTime(devicetime + 100000)
self._selectSolver(solver)
print("{0}: Current solver {1}\n{2}".format(devicetime, transitionName,
solver.sot.display()))
self.currentSot = transitionName
if hasattr(self, 'ros_publish_state'):
self.ros_publish_state.transition_name.value = transitionName
return True, devicetime
# \return success, time boolean, SoT time at which reading starts (invalid if success is False)
def runPreAction(self, transitionName):
if self.preActions.has_key(transitionName):
solver = self.preActions[transitionName]
t = self.sotrobot.device.control.time + 2
self.done_events.setFutureTime(t)
self._selectSolver(solver)
print("{0}: Running pre action {1}\n{2}".format(
t, transitionName, solver.sot.display()))
return True, t - 2
print("No pre action", transitionName)
return False, -1
## Execute a post-action
# \return success, time boolean, SoT time at which reading starts (invalid if success is False)
def runPostAction(self, targetStateName):
if self.postActions.has_key(self.currentSot):
d = self.postActions[self.currentSot]
if d.has_key(targetStateName):
solver = d[targetStateName]
devicetime = self.sotrobot.device.control.time
self.done_events.setFutureTime(devicetime + 2)
self._selectSolver(solver)
print("{0}: Running post action {1} --> {2}\n{3}".format(
devicetime, self.currentSot, targetStateName,
solver.sot.display()))
return True, devicetime
print("No post action {0} --> {1}".format(self.currentSot,
targetStateName))
return False, -1
def getJointList(self, prefix=""):
return [prefix + n for n in self.sotrobot.dynamic.model.names[1:]]
def publishState(self, subsampling=40):
if hasattr(self, "ros_publish_state"):
return
from dynamic_graph.ros import RosPublish
self.ros_publish_state = RosPublish("ros_publish_state")
self.ros_publish_state.add("vector", "state", "/agimus/sot/state")
self.ros_publish_state.add("vector", "reference_state",
"/agimus/sot/reference_state")
self.ros_publish_state.add("string", "transition_name",
"/agimus/sot/transition_name")
self.ros_publish_state.transition_name.value = ""
plug(self.sotrobot.device.state, self.ros_publish_state.state)
plug(self.rosSubscribe.posture, self.ros_publish_state.reference_state)
self.sotrobot.device.after.addDownsampledSignal(
"ros_publish_state.trigger", subsampling)
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
class ControllerSwitch:
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))
def setMeasurement (self,sig):
if self.reverse:
plug(sig, self._condition_up .sin1)
plug(sig, self._condition_down.sin2)
else:
plug(sig, self._condition_up .sin2)
plug(sig, self._condition_down.sin1)
@property
def thresholdUp (self):
if self.reverse:
return self._condition_up .sin2
else:
return self._condition_up .sin1
@property
def thresholdDown (self):
if self.reverse:
return self._condition_down.sin1
else:
return self._condition_down.sin2
@property
def signalOut (self): return self._switch.sout
def signalIn (self, n): return self._switch.signal("sin" + str(n))
@property
def conditionUp (self): return self._condition_up
@property
def conditionDown (self): return self._condition_down
@property
def eventUp (self): return self._event_up
@property
def eventDown (self): return self._event_down
@property
def latch (self): return self._latch
@property
def switch (self): return self._switch
class QuadrupedComControl(object):
def __init__(self,
robot,
ViconClientEntity=None,
client_name="vicon_client",
vicon_ip='10.32.3.16:801',
EntityName="quad_com_ctrl",
base_position=None,
base_velocity=None):
"""
Args:
base_position: (Optional, Vec7d signal) Base position of the robot.
base_velocity: (Optional, Vec6d signal) Base velocity of the robot.
"""
self.robot = robot
self.EntityName = EntityName
self.init_robot_properties()
self.client_name = client_name
self.host_name_quadruped = vicon_ip
if ViconClientEntity:
self.vicon_client = ViconClientEntity('vicon_' + self.client_name)
self.vicon_client.connect_to_vicon(self.host_name_quadruped)
self.vicon_client.displaySignals()
self.vicon_client.add_object_to_track("{}/{}".format(
self.robot_vicon_name, self.robot_vicon_name))
try:
## comment out if running on real robot
self.vicon_client.robot_wrapper(robot, self.robot_vicon_name)
except:
print("not in simulation")
self.robot.add_trace(self.vicon_client.name,
self.robot_vicon_name + "_position")
self.robot.add_trace(self.vicon_client.name,
self.robot_vicon_name + "_velocity_body")
self.robot.add_trace(self.vicon_client.name,
self.robot_vicon_name + "_velocity_world")
self.vicon_base_position = self.vicon_client.signal(
self.robot_vicon_name + "_position")
self.vicon_base_velocity = self.vicon_client.signal(
self.robot_vicon_name + "_velocity_body")
elif base_position and base_velocity:
self.vicon_base_position = base_position
self.vicon_base_velocity = base_velocity
else:
raise ValueError(
'Need to provide either ViconClientEntity or (base_positon and base_velocity)'
)
self.com_imp_ctrl = ComImpedanceControl(EntityName)
self._biased_base_position = None
self._biased_base_velocity = None
self.vicon_offset = constVector([
0.,
0.,
0.,
])
def init_robot_properties(self):
self.robot_vicon_name = "quadruped"
self.robot_mass = [2.17784, 2.17784, 2.17784]
self.robot_base_inertia = [0.00578574, 0.01938108, 0.02476124]
def compute_torques(self, Kp, des_pos, Kd, des_vel, des_fff):
self.base_pos_xyz = add_vec_vec(
selec_vector(self.vicon_base_position, 0, 3, "base_pos"),
self.vicon_offset)
self.base_vel_xyz = selec_vector(self.vicon_base_velocity, 0, 3,
"base_vel")
plug(self.base_pos_xyz, self.com_imp_ctrl.position)
plug(self.base_vel_xyz, self.com_imp_ctrl.velocity)
# plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel)
plug(Kp, self.com_imp_ctrl.Kp)
plug(Kd, self.com_imp_ctrl.Kd)
plug(des_pos, self.com_imp_ctrl.des_pos)
plug(des_vel, self.com_imp_ctrl.des_vel)
plug(des_fff, self.com_imp_ctrl.des_fff)
### mass in all direction (double to vec returns zero)
## TODO : Check if there is dynamicgraph::double
self.com_imp_ctrl.mass.value = self.robot_mass
self.control_switch_pos = SwitchVector("control_switch_pos")
self.control_switch_pos.setSignalNumber(
2) # we want to switch between 2 signals
plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0)
plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1)
self.control_switch_pos.selection.value = 0
self.control_switch_vel = SwitchVector("control_switch_vel")
self.control_switch_vel.setSignalNumber(
2) # we want to switch between 2 signals
plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0)
plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1)
plug(self.control_switch_pos.selection,
self.control_switch_vel.selection)
self.biased_base_pos_xyz = subtract_vec_vec(
self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos")
self.biased_base_vel_xyz = subtract_vec_vec(
self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel")
plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos)
plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel)
self.torques = self.com_imp_ctrl.tau
return self.torques
def compute_ang_control_torques(self, Kp_ang, des_ori, Kd_ang, des_ang_vel,
des_fft):
"""
### Computes torques required to control the orientation of base
"""
self.base_orientation = selec_vector(self.vicon_base_position, 3, 7,
"base_orientation")
self.base_ang_vel_xyz = selec_vector(self.vicon_base_velocity, 3, 6,
"selec_ang_dxyz")
plug(Kp_ang, self.com_imp_ctrl.Kp_ang)
plug(Kd_ang, self.com_imp_ctrl.Kd_ang)
self.com_imp_ctrl.inertia.value = [0.00578574, 0.01938108, 0.02476124]
plug(self.base_orientation, self.com_imp_ctrl.ori)
plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel)
plug(des_ori, self.com_imp_ctrl.des_ori)
plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel)
plug(des_fft, self.com_imp_ctrl.des_fft)
return self.com_imp_ctrl.angtau
def set_bias(self):
self.control_switch_pos.selection.value = 1
def get_biased_base_position(self):
"""
Return the robot position taking the bias offset into account.
Returns:
Signal<dg::vector> of size 7 (0:3 translation, 3:7 quaternion orientation)
"""
if self._biased_base_position is None:
self._biased_base_position = stack_two_vectors(
self.biased_base_pos_xyz, self.base_orientation, 3, 4,
self.EntityName + '_biased_base_pos')
return self._biased_base_position
def get_biased_base_velocity(self):
"""
Return the robot velocity taking the bias offset into account.
Returns:
Signal<dg::vector> of size 6 (0:3 translation, 3:6 rpy orientation)
"""
if self._biased_base_velocity is None:
self._biased_base_velocity = stack_two_vectors(
self.biased_base_vel_xyz, self.base_ang_vel_xyz, 3, 3,
self.EntityName + '_biased_base_vel')
return self._biased_base_velocity
def set_abs_end_eff_pos(self, abs_end_eff_pos_sig):
plug(abs_end_eff_pos_sig, self.com_imp_ctrl.abs_end_eff_pos)
def threshold_cnt_sensor(self):
plug(self.robot.device.contact_sensors, self.com_imp_ctrl.cnt_sensor)
return self.com_imp_ctrl.thr_cnt_sensor
def convert_cnt_value_to_3d(self, cnt_sensor, start_index, end_index,
entityName):
## Converts each element of contact sensor to 3d to allow vec_vec multiplication
selec_cnt_value = selec_vector(cnt_sensor, start_index, end_index,
entityName)
cnt_value_2d = stack_two_vectors(selec_cnt_value, selec_cnt_value, 1,
1)
cnt_value_3d = stack_two_vectors(cnt_value_2d, selec_cnt_value, 2, 1)
return cnt_value_3d
def return_lqr_tau(self, des_pos, des_vel, des_ori, des_ang_vel, des_fff,
des_fft, des_lqr):
"""
### for lqr based controller. reads values obtained from the planner
"""
self.base_pos_xyz = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_position"), 0,
3, "base_pos")
self.base_vel_xyz = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"),
0, 3, "base_vel")
plug(self.base_pos_xyz, self.com_imp_ctrl.position)
plug(self.base_vel_xyz, self.com_imp_ctrl.velocity)
self.control_switch_pos = SwitchVector("control_switch_pos")
self.control_switch_pos.setSignalNumber(
2) # we want to switch between 2 signals
plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0)
plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1)
self.control_switch_pos.selection.value = 0
self.control_switch_vel = SwitchVector("control_switch_vel")
self.control_switch_vel.setSignalNumber(
2) # we want to switch between 2 signals
plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0)
plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1)
plug(self.control_switch_pos.selection,
self.control_switch_vel.selection)
self.base_orientation = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_position"), 3,
7, "base_orientation")
self.biased_base_pos_xyz = subtract_vec_vec(
self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos")
self.biased_base_vel_xyz = subtract_vec_vec(
self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel")
self.base_ang_vel_xyz = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"),
3, 6, "selec_ang_dxyz")
#### LQR equation:
###should be removed
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp_ang)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd_ang)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.inertia)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.mass)
###
plug(des_lqr, self.com_imp_ctrl.lqr_gain)
plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos)
plug(des_pos, self.com_imp_ctrl.des_pos)
plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel)
plug(des_vel, self.com_imp_ctrl.des_vel)
plug(des_fff, self.com_imp_ctrl.des_fff)
plug(self.base_orientation, self.com_imp_ctrl.ori)
plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel)
plug(des_ori, self.com_imp_ctrl.des_ori)
plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel)
plug(des_fft, self.com_imp_ctrl.des_fft)
lqr_com_force = self.com_imp_ctrl.lqrtau
return lqr_com_force
def return_end_eff_lqr_tau(self, des_pos, des_vel, des_ori, des_ang_vel,
des_fff, des_lqr):
"""
### for lqr based controller at the end effector. reads values obtained from the planner
"""
self.base_pos_xyz = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_position"), 0,
3, "base_pos")
self.base_vel_xyz = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"),
0, 3, "base_vel")
plug(self.base_pos_xyz, self.com_imp_ctrl.position)
plug(self.base_vel_xyz, self.com_imp_ctrl.velocity)
self.control_switch_pos = SwitchVector("control_switch_pos")
self.control_switch_pos.setSignalNumber(
2) # we want to switch between 2 signals
plug(zero_vec(3, "zero_pos"), self.control_switch_pos.sin0)
plug(self.com_imp_ctrl.set_pos_bias, self.control_switch_pos.sin1)
self.control_switch_pos.selection.value = 0
self.control_switch_vel = SwitchVector("control_switch_vel")
self.control_switch_vel.setSignalNumber(
2) # we want to switch between 2 signals
plug(zero_vec(3, "zero_vel"), self.control_switch_vel.sin0)
plug(self.com_imp_ctrl.set_vel_bias, self.control_switch_vel.sin1)
plug(self.control_switch_pos.selection,
self.control_switch_vel.selection)
self.base_orientation = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_position"), 3,
7, "base_orientation")
self.biased_base_pos_xyz = subtract_vec_vec(
self.base_pos_xyz, self.control_switch_pos.sout, "biased_pos")
self.biased_base_vel_xyz = subtract_vec_vec(
self.base_vel_xyz, self.control_switch_vel.sout, "biased_vel")
self.base_ang_vel_xyz = selec_vector(
self.vicon_client.signal(self.robot_vicon_name + "_velocity_body"),
3, 6, "selec_ang_dxyz")
#### LQR equation:
###should be removed
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kp_ang)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.Kd_ang)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.inertia)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.mass)
plug(constVector([0.0, 0.0, 0.0], "Kp_ang"), self.com_imp_ctrl.des_fft)
###
plug(des_lqr, self.com_imp_ctrl.lqr_gain)
plug(self.biased_base_pos_xyz, self.com_imp_ctrl.biased_pos)
plug(des_pos, self.com_imp_ctrl.des_pos)
plug(self.biased_base_vel_xyz, self.com_imp_ctrl.biased_vel)
plug(des_vel, self.com_imp_ctrl.des_vel)
plug(des_fff, self.com_imp_ctrl.des_fff)
plug(self.base_orientation, self.com_imp_ctrl.ori)
plug(self.base_ang_vel_xyz, self.com_imp_ctrl.angvel)
plug(des_ori, self.com_imp_ctrl.des_ori)
plug(des_ang_vel, self.com_imp_ctrl.des_ang_vel)
lqr_end_eff_force = self.com_imp_ctrl.end_eff_lqr_tau
torques_fl = selec_vector(lqr_end_eff_force, 0, 3,
self.EntityName + "torques_fl")
torques_fr = selec_vector(lqr_end_eff_force, 3, 6,
self.EntityName + "torques_fr")
torques_hl = selec_vector(lqr_end_eff_force, 6, 9,
self.EntityName + "torques_hl")
torques_hr = selec_vector(lqr_end_eff_force, 9, 12,
self.EntityName + "torques_hr")
torques_fl_6d = stack_two_vectors(torques_fl,
zero_vec(3, "stack_fl_tau"), 3, 3)
torques_fr_6d = stack_two_vectors(torques_fr,
zero_vec(3, "stack_fr_tau"), 3, 3)
torques_hl_6d = stack_two_vectors(torques_hl,
zero_vec(3, "stack_hl_tau"), 3, 3)
torques_hr_6d = stack_two_vectors(torques_hr,
zero_vec(3, "stack_hr_tau"), 3, 3)
torques_fl_fr = stack_two_vectors(torques_fl_6d, torques_fr_6d, 6, 6)
torques_hl_hr = stack_two_vectors(torques_hl_6d, torques_hr_6d, 6, 6)
lqr_end_eff_force_24d = stack_two_vectors(torques_fl_fr, torques_hl_hr,
12, 12)
lqr_end_eff_force_24d = mul_double_vec(1.0, lqr_end_eff_force_24d,
"lqr_end_eff_force")
return lqr_end_eff_force_24d
def return_com_torques(self,
com_tau,
ang_tau,
des_abs_vel,
hess,
g0,
ce,
ci,
ci0,
reg,
cnt_plan=None):
### This divides forces using the wbc controller
plug(com_tau, self.com_imp_ctrl.lctrl)
plug(ang_tau, self.com_imp_ctrl.actrl)
plug(des_abs_vel, self.com_imp_ctrl.abs_end_eff_vel)
plug(hess, self.com_imp_ctrl.hess)
plug(g0, self.com_imp_ctrl.g0)
plug(ce, self.com_imp_ctrl.ce)
plug(ci, self.com_imp_ctrl.ci)
plug(ci0, self.com_imp_ctrl.ci0)
plug(reg, self.com_imp_ctrl.reg)
if cnt_plan == None:
thr_cnt_sensor = self.threshold_cnt_sensor()
plug(thr_cnt_sensor, self.com_imp_ctrl.thr_cnt_value)
else:
plug(cnt_plan, self.com_imp_ctrl.cnt_sensor)
plug(cnt_plan, self.com_imp_ctrl.thr_cnt_value)
self.wb_ctrl = self.com_imp_ctrl.wbctrl
## Thresholding with contact sensor to make forces event based
# fl_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 0, 1, "fl_cnt_3d")
# fr_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 1, 2, "fr_cnt_3d")
# hl_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 2, 3, "hl_cnt_3d")
# hr_cnt_value = self.convert_cnt_value_to_3d(thr_cnt_sensor, 3, 4, "hr_cnt_3d")
torques_fl = selec_vector(self.wb_ctrl, 0, 3,
self.EntityName + "torques_fl")
# torques_fl = mul_vec_vec(fl_cnt_value, torques_fl, self.EntityName + "fused_torques_fl")
torques_fr = selec_vector(self.wb_ctrl, 3, 6,
self.EntityName + "torques_fr")
# torques_fr = mul_vec_vec(fr_cnt_value, torques_fr, self.EntityName + "fused_torques_fr")
torques_hl = selec_vector(self.wb_ctrl, 6, 9,
self.EntityName + "torques_hl")
# torques_hl = mul_vec_vec(hl_cnt_value, torques_hl, self.EntityName + "fused_torques_hl")
torques_hr = selec_vector(self.wb_ctrl, 9, 12,
self.EntityName + "torques_hr")
# torques_hr = mul_vec_vec(hr_cnt_value, torques_hr, self.EntityName + "fused_torques_hr")
torques_fl_6d = stack_two_vectors(torques_fl,
zero_vec(3, "stack_fl_tau"), 3, 3)
torques_fr_6d = stack_two_vectors(torques_fr,
zero_vec(3, "stack_fr_tau"), 3, 3)
torques_hl_6d = stack_two_vectors(torques_hl,
zero_vec(3, "stack_hl_tau"), 3, 3)
torques_hr_6d = stack_two_vectors(torques_hr,
zero_vec(3, "stack_hr_tau"), 3, 3)
torques_fl_fr = stack_two_vectors(torques_fl_6d, torques_fr_6d, 6, 6)
torques_hl_hr = stack_two_vectors(torques_hl_6d, torques_hr_6d, 6, 6)
wbc_torques = stack_two_vectors(torques_fl_fr, torques_hl_hr, 12, 12)
## hack to allow tracking of torques
wbc_torques = mul_double_vec(1.0, wbc_torques, "com_torques")
return wbc_torques
def record_data(self):
self.get_biased_base_position()
self.get_biased_base_velocity()
self.robot.add_trace(self.com_imp_ctrl.name, "des_pos")
self.robot.add_trace(self.com_imp_ctrl.name, "des_vel")
self.robot.add_trace(self.EntityName + '_biased_base_pos', 'sout')
self.robot.add_trace(self.EntityName + '_biased_base_vel', 'sout')
self.robot.add_trace(self.EntityName, "tau")
# self.robot.add_ros_and_trace(self.EntityName, "tau")
# #
self.robot.add_trace(self.EntityName, "angtau")
# self.robot.add_ros_and_trace(self.EntityName, "angtau")
# #
#
self.robot.add_trace(self.EntityName, "wbctrl")
# self.robot.add_ros_and_trace(self.EntityName, "wbctrl")
# self.robot.add_trace(self.EntityName, "lqrtau")
# self.robot.add_ros_and_trace(self.EntityName, "lqrtau")
# self.robot.add_trace(self.EntityName, "thr_cnt_sensor")
# self.robot.add_ros_and_trace(self.EntityName, "thr_cnt_sensor")
#
self.robot.add_trace("com_torques", "sout")
# self.robot.add_ros_and_trace("com_torques", "sout")
# self.robot.add_trace(self.EntityName, "end_eff_lqr_tau")
# self.robot.add_ros_and_trace(self.EntityName, "end_eff_lqr_tau")
# self.robot.add_trace("lqr_end_eff_force", "sout")
# self.robot.add_ros_and_trace("lqr_end_eff_force", "sout")
self.robot.add_trace("biased_pos", "sout")
# self.robot.add_ros_and_trace("biased_pos", "sout")
#
self.robot.add_trace("biased_vel", "sout")