def publishState(self, subsampling=100):
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", "/sot_hpp/state")
plug(self.sotrobot.device.state, self.ros_publish_state.state)
self.sotrobot.device.after.addDownsampledSignal(
"ros_publish_state.trigger", 100)
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")
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)
def __init__(self, name, robot):
self.name = name
# Setup entity that triggers the event.
self.switch = SwitchBoolean(name + "_switch")
self.event = Event(name + "_event")
plug(self.switch.sout, self.event.condition)
robot.device.after.addSignal(name + "_event.check")
self.ros_publish = RosPublish(name + '_ros_publish')
# self.ros_publish.add ('boolean', name, '/agimus/sot/event/' + name)
# self.ros_publish.signal(name).value = int(True)
self.ros_publish.add('int', name, '/agimus/sot/event/' + name)
self.event.addSignal(name + "_ros_publish.trigger")
self.switch_string = {}
def init_appli(robot):
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
locals()['contactRF'] = contactRF
from dynamic_graph import plug
from dynamic_graph.sot.core.sot import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
from dynamic_graph.ros import RosPublish
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
from dynamic_graph import plug
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
robot.device.control.recompute(0)
def test_balance_ctrl_talos_gazebo(robot,
use_real_vel=True,
use_real_base_state=False,
startSoT=True):
# BUILD THE STANDARD GRAPH
conf = get_sim_conf()
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf)
# force current measurements to zero
robot.ctrl_manager.i_measured.value = NJ * (0.0, )
#robot.current_ctrl.i_measured.value = NJ*(0.0,);
robot.filters.current_filter.x.value = NJ * (0.0, )
# BYPASS TORQUE CONTROLLER
plug(robot.inv_dyn.tau_des, robot.ctrl_manager.ctrl_torque)
# CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36)
robot.q = Selec_of_vector("q")
plug(robot.device.robotState, robot.q.sin)
robot.q.selec(0, NJ + 6)
plug(robot.q.sout, robot.pos_ctrl.base6d_encoders)
plug(robot.q.sout, robot.traj_gen.base6d_encoders)
#plug(robot.q.sout, robot.estimator_ft.base6d_encoders);
robot.ros = RosPublish('rosPublish')
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
# BYPASS JOINT VELOCITY ESTIMATOR
if (use_real_vel):
robot.dq = Selec_of_vector("dq")
#plug(robot.device.robotVelocity, robot.dq.sin); # to check robotVelocity empty
plug(robot.device.velocity, robot.dq.sin)
robot.dq.selec(6, NJ + 6)
plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities)
# generate seg fault
plug(robot.dq.sout, robot.base_estimator.joint_velocities)
plug(robot.device.gyrometer, robot.base_estimator.gyroscope)
# BYPASS BASE ESTIMATOR
robot.v = Selec_of_vector("v")
#plug(robot.device.robotVelocity, robot.dq.sin); # to check robotVelocity empty
plug(robot.device.velocity, robot.dq.sin)
robot.v.selec(0, NJ + 6)
if (use_real_base_state):
plug(robot.q.sout, robot.inv_dyn.q)
plug(robot.v.sout, robot.inv_dyn.v)
if (startSoT):
start_sot()
# RESET FORCE/TORQUE SENSOR OFFSET
# sleep(10*robot.timeStep);
#robot.estimator_ft.setFTsensorOffsets(24*(0.0,));
return robot
def setupEvents(self):
from dynamic_graph_hpp.sot import Event, CompareDouble
from dynamic_graph.sot.core.operator import Norm_of_vector
from dynamic_graph.ros import RosPublish
self.norm = Norm_of_vector("control_norm")
plug(self.sotrobot.device.control, self.norm.sin)
self.norm_comparision = CompareDouble("control_norm_comparison")
plug(self.norm.sout, self.norm_comparision.sin1)
self.norm_comparision.sin2.value = 1e-2
self.norm_event = Event("control_norm_event")
plug(self.norm_comparision.sout, self.norm_event.condition)
# self.sotrobot.device.after.addSignal (self.norm_event.check.name)
self.sotrobot.device.after.addSignal("control_norm_event.check")
self.ros_publish = RosPublish('ros_publish_control_norm')
self.ros_publish.add('double', 'event_control_norm',
'/sot_hpp/control_norm_changed')
plug(self.norm.sout, self.ros_publish.event_control_norm)
# plug (self.norm_event.trigger, self.ros_publish.trigger)
self.norm_event.addSignal("ros_publish_control_norm.trigger")
def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False):
# BUILD THE STANDARD GRAPH
conf = get_sim_conf();
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf);
# force current measurements to zero
robot.ctrl_manager.currents.value = NJ*(0.0,);
# BYPASS TORQUE CONTROLLER
plug(robot.inv_dyn.tau_des, robot.ctrl_manager.ctrl_torque);
# CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36)
robot.q = Selec_of_vector("q");
plug(robot.device.robotState, robot.q.sin);
robot.q.selec(0, NJ+6);
plug(robot.q.sout, robot.pos_ctrl.base6d_encoders);
plug(robot.q.sout, robot.traj_gen.base6d_encoders);
plug(robot.q.sout, robot.estimator_ft.base6d_encoders);
plug(robot.q.sout, robot.ctrl_manager.base6d_encoders);
plug(robot.q.sout, robot.torque_ctrl.base6d_encoders);
robot.ros = RosPublish('rosPublish');
robot.device.after.addDownsampledSignal('rosPublish.trigger',1);
# BYPASS JOINT VELOCITY ESTIMATOR
if(use_real_vel):
robot.dq = Selec_of_vector("dq");
plug(robot.device.robotVelocity, robot.dq.sin);
robot.dq.selec(6, NJ+6);
plug(robot.dq.sout, robot.pos_ctrl.jointsVelocities);
plug(robot.dq.sout, robot.base_estimator.joint_velocities);
# BYPASS BASE ESTIMATOR
robot.v = Selec_of_vector("v");
plug(robot.device.robotVelocity, robot.v.sin);
robot.v.selec(0, NJ+6);
if(use_real_base_state):
plug(robot.q.sout, robot.inv_dyn.q);
plug(robot.v.sout, robot.inv_dyn.v);
# start_sot();
#
# # RESET FORCE/TORQUE SENSOR OFFSET
# sleep(10*robot.timeStep);
# robot.estimator_ft.setFTsensorOffsets(24*(0.0,));
return robot;
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
projection = HolonomicProjection("projection")
projection.setSize(robot.dynamic.getDimension())
projection.setLeftWheel(6)
projection.setRightWheel(7)
# The wheel separation could be obtained with pinocchio.
# See pmb2_description/urdf/base.urdf.xacro
projection.setWheelRadius(0.0985)
projection.setWheelSeparation(0.4044)
plug(robot.dynamic.mobilebase, projection.basePose)
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(projection.projection, sot.proj0)
plug(sot.control, robot.device.control)
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
robot.device.control.recompute(0)
def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
if (conf is None):
conf = get_default_conf()
dt = robot.timeStep
# TMP: overwrite halfSitting configuration to use SoT joint order
robot.halfSitting = (
# Free flyer
0.,
0.,
0.648702,
0.,
0.,
0.,
# Legs
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0.,
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0.,
# Chest and head
0.,
0.,
0.,
0.,
# Arms
0.261799,
-0.17453,
0.,
-0.523599,
0.,
0.,
0.1,
0.261799,
0.17453,
0.,
-0.523599,
0.,
0.,
0.1)
robot.device.setControlInputType('noInteg')
robot.ctrl_manager = create_ctrl_manager(conf.control_manager,
conf.motor_params, dt)
robot.traj_gen = create_trajectory_generator(robot.device, dt)
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt)
robot.rf_force_traj_gen = create_force_traj_gen(
"rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt)
robot.lf_force_traj_gen = create_force_traj_gen(
"lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt)
robot.traj_sync = create_trajectory_switch()
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf")
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf")
robot.rf_traj_gen.init(dt)
robot.lf_traj_gen.init(dt)
robot.encoders = create_encoders(robot)
robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt)
(robot.estimator_ft,
robot.filters) = create_estimators(robot, conf.force_torque_estimator,
conf.motor_params, dt)
robot.imu_filter = create_imu_filter(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt,
conf.base_estimator)
connect_synchronous_trajectories(robot.traj_sync, [
robot.com_traj_gen, robot.rf_force_traj_gen, robot.lf_force_traj_gen,
robot.rf_traj_gen, robot.lf_traj_gen
])
#robot.rf_traj_gen, robot.lf_traj_gen])
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt)
robot.torque_ctrl = create_torque_controller(robot,
conf.joint_torque_controller,
conf.motor_params, dt)
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,
conf.motor_params, dt)
robot.adm_ctrl = create_admittance_ctrl(robot, conf.adm_ctrl, dt)
robot.current_ctrl = create_current_controller(robot, conf.current_ctrl,
conf.motor_params, dt)
connect_ctrl_manager(robot)
# create low-pass filter for computing joint velocities
robot.encoder_filter = create_chebi2_lp_filter_Wn_03_N_4(
'encoder_filter', dt, conf.motor_params.NJ)
plug(robot.encoders.sout, robot.encoder_filter.x)
plug(robot.encoder_filter.dx, robot.current_ctrl.dq)
plug(robot.encoder_filter.dx, robot.torque_ctrl.jointsVelocities)
#plug(robot.encoder_filter.x_filtered, robot.base_estimator.joint_positions);
#plug(robot.encoder_filter.dx, robot.base_estimator.joint_velocities);
robot.ros = RosPublish('rosPublish')
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
robot.estimator_ft.dgyro.value = (0.0, 0.0, 0.0)
robot.estimator_ft.gyro.value = (0.0, 0.0, 0.0)
# estimator.accelerometer.value = (0.0, 0.0, 9.81);
if (startSoT):
print "Gonna start SoT"
sleep(1.0)
start_sot()
if (go_half_sitting):
print "Gonna go to half sitting in 1 sec"
sleep(1.0)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
return robot
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
"""
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
def setupEvents(self):
from dynamic_graph_hpp.sot import Event, CompareDouble
from dynamic_graph.sot.core.operator import Norm_of_vector
from dynamic_graph.ros import RosPublish
self.norm = Norm_of_vector("control_norm")
plug(self.sotrobot.device.control, self.norm.sin)
self.norm_comparision = CompareDouble("control_norm_comparison")
plug(self.norm.sout, self.norm_comparision.sin1)
self.norm_comparision.sin2.value = 1e-2
self.norm_event = Event("control_norm_event")
plug(self.norm_comparision.sout, self.norm_event.condition)
# self.sotrobot.device.after.addSignal (self.norm_event.check.name)
self.sotrobot.device.after.addSignal("control_norm_event.check")
self.ros_publish = RosPublish('ros_publish_control_norm')
self.ros_publish.add('double', 'event_control_norm',
'/sot_hpp/control_norm_changed')
plug(self.norm.sout, self.ros_publish.event_control_norm)
# plug (self.norm_event.trigger, self.ros_publish.trigger)
self.norm_event.addSignal("ros_publish_control_norm.trigger")
def makeInitialSot(self):
# Create the initial sot (keep)
sot = SOT('sot_keep')
sot.setSize(self.sotrobot.dynamic.getDimension())
self.keep_posture = Posture("posture_keep", self.sotrobot)
self.keep_posture.tp.setWithDerivative(False)
# TODO : I do agree that this is a dirty hack.
# The COM of the robot in the HPP frame is at those coordinates (approx.).
# But the keep_posture task is « internally » (there is no actuator able to directly move the COM,
# but the controller in the task is computing controls anyway, and integrating them)
# moving the computed COM to its reference value which is (0, 0, 0).
# So, when we send the goal coordinates of the feet from HPP to the SoT, there is an offset of 0,74m
# between the goal and the current position of the feet. This was the cause of the strange feet
# movements at the beginning of the demo.
# Maybe we can get the COM position and orientation from HPP at the beginning of the trajectory
# to initialize self.sotrobot.dynamic.position.value
# self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:])
# The above TODO must be fixed in users script by providing the
# right initial pose using robot.device.set (configuration) before starting
# dynamic graph.
self.keep_posture._signalPositionRef(
).value = self.sotrobot.dynamic.position.value
self.keep_posture.pushTo(sot)
self.sots[""] = sot
def topics(self):
c = self.hpTasks + self.lpTasks
for g in self.grasps.values():
c += g
return c.topics
def plugTopics(self, rosexport):
self.rosexport = rosexport
topics = self.topics()
for n, t in topics.items():
if t.has_key('handler'):
topic = _handlers[t['handler']](n, t)
else:
topic = t["topic"]
rosexport.add(t["type"], n, topic)
for s in t['signalGetters']:
plug(rosexport.signal(n), s())
print(topic, "plugged to", n, ', ', len(t['signalGetters']),
'times')
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):
exec("tmp = " + self.rosexport.list())
for s in tmp:
self.rosexport.clearQueue(s)
def readQueue(self, read):
if read < 0:
print("ReadQueue argument should be >= 0")
return
t = self.sotrobot.device.control.time
self.rosexport.readQueue(t + read)
def stopReadingQueue(self):
self.rosexport.readQueue(-1)
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 == "":
# TODO : Explanation and linked TODO in the function makeInitialSot
self.keep_posture._signalPositionRef(
).value = self.sotrobot.dynamic.position.value
sot = self.sots[transitionName]
control = self._getControlSignal(sot)
# Start reading queues
self.readQueue(10)
plug(control, self.sotrobot.device.control)
print("Current sot:", transitionName, "\n", sot.display())
self.currentSot = transitionName
def runPreAction(self, transitionName):
if self.preActions.has_key(transitionName):
sot = self.preActions[transitionName]
print("Running pre action", transitionName, "\n", sot.display())
t = self.sotrobot.device.control.time
# This is not safe since it would be run concurrently with the
# real time thread.
# sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print("No pre action", transitionName)
def runPostAction(self, targetStateName):
if self.postActions.has_key(self.currentSot):
d = self.postActions[self.currentSot]
if d.has_key(targetStateName):
sot = d[targetStateName]
print("Running post action", self.currentSot, targetStateName,
"\n", sot.display())
t = self.sotrobot.device.control.time
# This is not safe since it would be run concurrently with the
# real time thread.
# sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print("No post action", self.currentSot, targetStateName)
def getJointList(self, prefix=""):
return [prefix + n for n in self.sotrobot.dynamic.model.names[1:]]
def _getControlSignal(self, sot):
if self.SoTtimers.has_key(sot.name):
return self.SoTtimers[sot.name].sout
else:
return sot.control
def publishState(self, subsampling=100):
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", "/sot_hpp/state")
plug(self.sotrobot.device.state, self.ros_publish_state.state)
self.sotrobot.device.after.addDownsampledSignal(
"ros_publish_state.trigger", 100)
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.ros import RosPublish
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_hpp/state")
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
v = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
robot.device.control.value = v
def create_rospublish(robot, name):
from dynamic_graph.ros import RosPublish
rospub = RosPublish(name)
robot.device.after.addSignal(rospub.name + '.trigger')
return rospub
class Events:
def __init__(self, name, robot):
self.name = name
# Setup entity that triggers the event.
self.switch = SwitchBoolean(name + "_switch")
self.event = Event(name + "_event")
plug(self.switch.sout, self.event.condition)
robot.device.after.addSignal(name + "_event.check")
self.ros_publish = RosPublish(name + '_ros_publish')
# self.ros_publish.add ('boolean', name, '/agimus/sot/event/' + name)
# self.ros_publish.signal(name).value = int(True)
self.ros_publish.add('int', name, '/agimus/sot/event/' + name)
self.event.addSignal(name + "_ros_publish.trigger")
self.switch_string = {}
def getSignalNumber(self):
return self.switch.getSignalNumber()
def setSignalNumber(self, n):
self.switch.setSignalNumber(n)
def setSelectedSignal(self, n):
self.switch.selection.value = n
#self.idSignal.value = n
## Creates entities to check whether the norm is
## superior to the threshold \c thr
##
## Use controlNormSignal to get the created signal.
def setupNormOfControl(self, control, thr):
self.norm, self.norm_comparision = norm_inferior_to(
self.name + "_control", control, thr)
## Creates entities to check whether the norm is
## superior to the threshold \c thr
##
## Use timeSignal to get the created signal.
def setupTime(self):
self.time = Time(self.name + "_time")
plug(self.time.now, self.ros_publish.signal(self.name))
def setFutureTime(self, time):
self.time.setTime(time)
def conditionSignal(self, i):
return self.switch.signal("sin" + str(i))
def setConditionString(self, i, name):
self.switch_string[i] = name
def getConditionString(self, i):
return self.switch_string.get(i, None)
@property
def controlNormSignal(self):
return self.norm_comparision.sout
@property
def timeEllapsedSignal(self):
return self.time.after
@property
def remainsTimeSignal(self):
return self.time.before
@property
def idSignal(self):
return self.ros_publish.signal(self.name)
def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
if(conf is None):
conf = get_default_conf();
dt = robot.timeStep;
# TMP: overwrite halfSitting configuration to use SoT joint order
robot.halfSitting = (
# Free flyer
0., 0., 1.018213, 0., 0. , 0.,
# legs
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708,
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708,
# Chest
0.0 , 0.006761,
# arms
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005,
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005,
# Head
0.0,0.0);
#robot.device.setControlInputType('noInteg');
robot.ctrl_manager = create_ctrl_manager(conf.control_manager, conf.motor_params, dt);
robot.traj_gen = create_trajectory_generator(robot.device, dt);
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt);
robot.rf_force_traj_gen = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt);
robot.lf_force_traj_gen = create_force_traj_gen("lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt);
robot.traj_sync = create_trajectory_switch();
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf");
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf");
robot.rf_traj_gen.init(dt);
robot.lf_traj_gen.init(dt);
robot.encoders = create_encoders(robot);
robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt);
#(robot.estimator_ft, robot.filters) = create_estimators(robot, conf.force_torque_estimator, conf.motor_params, dt);
robot.filters = create_filters(robot, conf.force_torque_estimator, conf.motor_params, dt);
robot.imu_filter = create_imu_filter(robot, dt);
robot.base_estimator = create_base_estimator(robot, dt, conf.base_estimator);
connect_synchronous_trajectories(robot.traj_sync,
[robot.com_traj_gen,
robot.rf_force_traj_gen, robot.lf_force_traj_gen,
robot.rf_traj_gen, robot.lf_traj_gen])
#robot.rf_traj_gen, robot.lf_traj_gen])
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt);
robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt);
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt);
#robot.current_ctrl = create_current_controller(robot, conf.current_ctrl, conf.motor_params, dt);
connect_ctrl_manager(robot);
# create low-pass filter for computing joint velocities
robot.encoder_filter = create_chebi2_lp_filter_Wn_03_N_4('encoder_filter', dt, conf.motor_params.NJ)
plug(robot.encoders.sout, robot.encoder_filter.x)
#plug(robot.encoder_filter.dx, robot.current_ctrl.dq);
plug(robot.encoder_filter.dx, robot.torque_ctrl.jointsVelocities);
#plug(robot.encoder_filter.x_filtered, robot.base_estimator.joint_positions);
#plug(robot.encoder_filter.dx, robot.base_estimator.joint_velocities);
robot.ros = RosPublish('rosPublish');
robot.device.after.addDownsampledSignal('rosPublish.trigger',1);
#robot.estimator_ft.dgyro.value = (0.0, 0.0, 0.0);
#robot.estimator_ft.gyro.value = (0.0, 0.0, 0.0);
# estimator.accelerometer.value = (0.0, 0.0, 9.81);
if(startSoT):
print "Gonna start SoT";
sleep(1.0);
start_sot();
if(go_half_sitting):
print "Gonna go to half sitting in 1 sec";
sleep(1.0);
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0);
return robot;
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)
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.ros import RosPublish
robot.ros = RosPublish('rosPublish')
robot.ros.add('vector', 'robotState_ros', 'robotState')
plug(robot.device.robotState, robot.ros.signal('robotState_ros'))
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
def create_ros_topics(robot):
from dynamic_graph.ros import RosPublish
ros = RosPublish('rosPublish')
try:
create_topic(ros, robot.device.robotState, 'robotState')
create_topic(ros, robot.device.gyrometer, 'gyrometer')
create_topic(ros, robot.device.accelerometer, 'accelerometer')
create_topic(ros, robot.device.forceRLEG, 'forceRLEG')
create_topic(ros, robot.device.forceLLEG, 'forceLLEG')
create_topic(ros, robot.device.currents, 'currents')
# create_topic(ros, robot.device.forceRARM, 'forceRARM');
# create_topic(ros, robot.device.forceLARM, 'forceLARM');
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
except:
pass
try:
create_topic(ros, robot.filters.estimator_kin.dx, 'jointsVelocities')
# create_topic(ros, robot.estimator.jointsTorquesFromInertiaModel, 'jointsTorquesFromInertiaModel');
# create_topic(ros, robot.estimator.jointsTorquesFromMotorModel, 'jointsTorquesFromMotorModel');
# create_topic(ros, robot.estimator.currentFiltered, 'currentFiltered');
except:
pass
try:
create_topic(ros, robot.torque_ctrl.u, 'i_des_torque_ctrl')
except:
pass
try:
create_topic(ros, robot.traj_gen.q, 'q_ref')
# create_topic(ros, robot.traj_gen.dq, 'dq_ref');
# create_topic(ros, robot.traj_gen.ddq, 'ddq_ref');
except:
pass
try:
create_topic(ros, robot.ctrl_manager.pwmDes, 'i_des')
create_topic(ros, robot.ctrl_manager.pwmDesSafe, 'i_des_safe')
# create_topic(ros, robot.ctrl_manager.signOfControlFiltered, 'signOfControlFiltered');
# create_topic(ros, robot.ctrl_manager.signOfControl, 'signOfControl');
except:
pass
try:
create_topic(ros, robot.inv_dyn.tau_des, 'tau_des')
except:
pass
try:
create_topic(ros, robot.ff_locator.base6dFromFoot_encoders,
'base6dFromFoot_encoders')
except:
pass
try:
create_topic(ros, robot.floatingBase.soutPos, 'floatingBase_pos')
except:
pass
return ros