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 makeGrasps (self, transitions):
"""
@param transition: a list of dictionaries containing the following keys:
- "id", "name": the id and name of the transition in hpp.
- "manifold": a tuple of (gripper_index, handle_index) defining the submanifold into which the transition takes place.
- "grasp" (optional) : (gripper_index, handle_index) corresponding to the added/removed grasp.
- "forward" (required if "grasp") : True if added, False if removed.
- "step" (required if "grasp") : integer [ 0, 5 ].
"""
self.grasps = dict()
self.sots = dict()
self.postActions = dict()
self.preActions = dict()
self.transitions = transitions
print "Transitions:\n"
print transitions
print "\n"
for t in transitions:
# Create SOT solver
# sot = SOT ('sot_' + str(t['id']) + '-' + t['name'])
sot = SOT ('sot_' + str(t['id']))
sot.setSize(self.sotrobot.dynamic.getDimension())
# Push high priority tasks (Equilibrium for instance)
self.hpTasks.pushTo(sot)
# Create (or get) the tasks
M = self._manifold(t["manifold"])
if t.has_key("grasp"):
grasp = self._manifold(t["manifold"] + (t["grasp"],))
forward = bool(t["forward"])
step = int(t["step"])
assert step >= 0 and step <= 5, "'step' must be an integer within [0, 5]"
# TODO Events should be set up here for each step.
# For instance, adding some events based on force feedback to say
# when the object is grasped.
if forward:
if step == 1:
M.pushTo(sot)
elif step == 0 or step == 2:
grasp.pushTo(sot)
else:
grasp.pushTo(sot)
else:
if step == 1:
M.pushTo(sot)
elif step == 0 or step == 2:
grasp.pushTo(sot)
else:
grasp.pushTo(sot)
else:
M.pushTo(sot)
# Put low priority tasks
self.lpTasks.pushTo(sot)
self.sots[t['id']] = sot
def makeLoopTransition(self, state):
n = self._loopTransitionName(state.grasps)
sot = SOT('sot_' + n)
sot.setSize(self.sotrobot.dynamic.getDimension())
self.hpTasks.pushTo(sot)
state.manifold.pushTo(sot)
self.lpTasks.pushTo(sot)
self.sots[n] = sot
def _newSoT(self, name):
sot = SOT(name)
sot.setSize(self.sotrobot.dynamic.getDimension())
sot.damping.value = 0.001
if self.addTimerToSotControl:
from .tools import insertTimerOnOutput
self.timers[name] = insertTimerOnOutput(sot.control, "vector")
self.SoTtracer.add(self.timers[name].name + ".timer",
str(len(self.timerIds)) + ".timer")
self.timerIds.append(name)
return sot
def __init__(self, name, dimension, damping=None, timer=None):
from dynamic_graph.sot.core import SOT
sot = SOT(name)
sot.setSize(dimension)
if damping is not None: sot.damping.value = damping
self.sot = sot
self.tasks = []
if timer:
from .tools import insertTimerOnOutput
self.timer = insertTimerOnOutput(sot.control, "vector")
else:
self.timer = None
class Solver:
robot = None
sot = None
def __init__(self, robot):
self.robot = robot
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setNumberDofs(self.robot.dimension)
if robot.simu:
plug(self.sot.signal('control'), robot.simu.signal('control'))
plug(self.robot.simu.state,
self.robot.dynamic.position)
def __init__(self, name, dimension, damping=None, timer=False):
from dynamic_graph.entity import VerbosityLevel
from dynamic_graph.sot.core import SOT
sot = SOT(name)
sot.setSize(dimension)
sot.setMaxControlIncrementSquaredNorm(10.)
sot.setLoggerVerbosityLevel(VerbosityLevel.VERBOSITY_ALL)
if damping is not None: sot.damping.value = damping
self.sot = sot
self.tasks = []
if timer:
from .tools import insertTimerOnOutput
self.timer = insertTimerOnOutput(sot.control, "vector")
else:
self.timer = None
def main(robot):
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
robot.traj_gen = create_config_trajectory_generator(dt, robot)
JOINT = 31
QJOINT = JOINT + 6
# --- Joint
robot.taskJoint = MetaTaskKineConfig(robot.dynamic, [QJOINT])
# robot.dynamic.com.recompute(0)
robot.taskJoint.featureDes.errorIN.value = N_CONFIG * [0.0]
robot.taskJoint.task.controlGain.value = 100
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskJoint.task.name)
robot.device.control.recompute(0)
plug(robot.traj_gen.x, robot.taskJoint.featureDes.errorIN)
sleep(1.0)
os.system('rosservice call /start_dynamic_graph')
sleep(1.0)
robot.traj_gen.move(QJOINT, -1.0, 1.0)
sleep(1.1)
robot.traj_gen.startSinusoid(QJOINT, 1.0, 8.0)
sleep(10.0)
robot.traj_gen.stop(QJOINT)
class Solver:
def __init__(self, robot):
self.robot = robot
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
# Create the solver.
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
# Important: always use 'jointLimitator.control'
# and NOT 'sot.control'!
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push (self, task):
"""
Proxy method to push a task in the sot
"""
self.sot.push (task.name)
def remove (self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.remove (task.name)
def __str__ (self):
return self.sot.display ()
def main(robot, conf=None):
if (conf is None):
conf = get_default_conf()
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
NJ = robot.dimension - 7
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 10
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
# --- SOT
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.device.control.recompute(0)
# --- ENTITIES
robot.param_server = create_parameter_server(conf.param_server, dt)
robot.example = create_example()
def __init__(self, robot):
self.robot = robot
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
plug(self.robot.dynamic.position, self.jointLimitator.joint)
plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
# Create the solver.
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
# Important: always use 'jointLimitator.control'
# and NOT 'sot.control'!
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
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.sot.core 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(robot.taskUpperBody.name)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
robot.device.control.recompute(0)
print("sot_youbot")
print("Prologue loaded.")
from dynamic_graph import plug
from dynamic_graph.sot.youbot.robot import youbot
from dynamic_graph.sot.core import SOT,FeaturePosition, Task
#from dynamic_graph.sot.youbot.youbot_device import YoubotDevice
from dynamic_graph.entity import PyEntityFactoryClass
Device = PyEntityFactoryClass('YoubotDevice')
robot = youbot(name = 'youBot', device = Device('youBot_device'))
dimension = robot.dynamic.getDimension()
plug(robot.device.state, robot.dynamic.position)
#solver#
solver = SOT ('solver')
solver.setSize (robot.dynamic.getDimension())
plug (solver.control, robot.device.control)
__all__ = ["robot"]
def newSot(name):
sot_keep = SOT (name)
sot_keep.setSize(robot.dynamic.getDimension())
sot_keep.damping.value = 0.001
return name
model.acceleration.value = dimension * (0., )
# Create taks for the base
model.createOpPoint("base", "waist")
# Create task for the wrist
model.createOpPoint("wrist", "wrist_3_joint")
feature_wrist = FeaturePosition('position_wrist', model.wrist, model.Jwrist,
I4)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1.
task_wrist.add(feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint("ee", "ee_fixed_joint")
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_wrist.name)
plug(solver.control, device.control)
device.increment(0.01)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(device, tracer, device.name, "state")
addTrace(device, tracer, feature_wrist._feature.name, "position")
addTrace(device, tracer, feature_wrist._reference.name, "position")
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz"
)
aSimpleSeqPlay.setTimeToStart(10.0)
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import Selec_of_vector
from dynamic_graph import plug
plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)
# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture)
# Set the gain of the posture task
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
# Create the solver
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
taskPosture.featureDes.errorIN.recompute(0)
# Push the posture task in the solver
task_wrist = Task ('wrist_task')
task_wrist.controlGain.value = 1
task_wrist.add (feature_wrist.name)
#Create tracer
tracer = TracerRealTime ('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/','dg_','.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace (robot.device, tracer, robot.device.name, "state")
addTrace (robot.device, tracer, feature_wrist._feature.name, "position")
addTrace (robot.device, tracer, feature_wrist._reference.name, "position")
# solver
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_waist.name)
solver.push (task_wrist.name)
solver.push (task_elbow.name)
solver.push (task_lift.name)
solver.push (task_wrist1.name)
solver.push (task_wrist2.name)
plug (solver.control, robot.device.control)
#robot.device.increment (0.01)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def makeTransition(self, stateFrom, stateTo, ig):
sf = stateFrom
st = stateTo
names = self._transitionNames(sf, st, ig)
print "names: {}".format(names)
iobj = self.objectFromHandle[st.grasps[ig]]
obj = self.objects[iobj]
noPlace = self._isObjectGrasped(sf.grasps, iobj)
print "iobj, obj, noPlace: {}, {}, {}".format(iobj, obj, noPlace)
# The different cases:
pregrasp = True
intersec = not noPlace
preplace = not noPlace
# Start here
nWaypoints = pregrasp + intersec + preplace
nTransitions = 1 + nWaypoints
# Link waypoints
transitions = names[:]
assert nWaypoints > 0
M = 1 + pregrasp
sots = []
for i in range(nTransitions):
ns = ("{0}_{1}{2}".format(names[0], i, i + 1),
"{0}_{2}{1}".format(names[1], i, i + 1))
for n in ns:
s = SOT('sot_' + n)
s.setSize(self.sotrobot.dynamic.getDimension())
self.hpTasks.pushTo(s)
#if pregrasp and i == 1:
# Add pregrasp task
#self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s)
if i < M: sf.manifold.pushTo(s)
else: st.manifold.pushTo(s)
self.lpTasks.pushTo(s)
self.sots[n] = s
sots.append(n)
key = sots[2 * (M - 1)]
states = (st.name, names[0] + "_intersec")
sot = SOT("postAction_" + key)
sot.setSize(self.sotrobot.dynamic.getDimension())
self.hpTasks.pushTo(sot)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
st.manifold.pushTo(sot)
self.lpTasks.pushTo(sot)
# print sot
# TODO one post action is missing
if not self.postActions.has_key(key):
self.postActions[key] = dict()
for n in states:
self.postActions[key][n] = sot
key = sots[2 * (M - 1) + 1]
states = (st.name, names[0] + "_intersec")
sot = SOT("preAction_" + key)
sot.setSize(self.sotrobot.dynamic.getDimension())
self.hpTasks.pushTo(sot)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
sf.manifold.pushTo(sot)
self.lpTasks.pushTo(sot)
# print sot
self.preActions[key] = sot
self.preActions[sots[2 * (M - 1)]] = sot
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper(URDFPATH, URDFDIR, se3.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=DISPLAY)
if DISPLAY:
pinocchioRobot.display(fromSotToPinocchio(halfSitting))
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data,
halfSitting, OperationalPointsMap)
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
# DEFINE POSTURE TASK
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity, hstack, zeros
task_name = "posture_task"
taskPosture = Task(task_name)
taskPosture.dyn = robot.dynamic
taskPosture.feature = FeatureGeneric('feature_' + task_name)
taskPosture.featureDes = FeatureGeneric('feature_des_' + task_name)
taskPosture.gain = GainAdaptive("gain_" + task_name)
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model,
pinocchioRobot.data, initialConfig, OperationalPointsMap)
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control,robot.device.control)
# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye
taskRH = MetaTaskKine6d('rh',robot.dynamic,'rh',robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4); handMgrip[0:3,3] = (0.1,0,0)
#task_waist_metakine.feature.selec.value = '111111'#RzRyRxTzTyTx
task_waist_metakine.gain.setConstant(1)
task_waist_metakine.featureDes.position.value = goal_waist
solver = SolverKine('sot_solver')
solver.setSize (robot.dynamic.getDimension())
robot.device.resize (robot.dynamic.getDimension())
solver.push (task_waist_metakine.task.name)
plug (solver.control,robot.device.control)
'''
# solver
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_waist.name)
solver.push (task_wrist.name)
plug (solver.control, robot.device.control)
#robot.device.increment (0.01)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
runner.once()
def main(robot):
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 10
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
# --- SOT
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.device.control.recompute(0)
# --- ESTIMATION
robot.param_server = create_parameter_server(param_server_conf, dt)
# robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt)
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt, conf)
robot.be_filters = create_be_filters(robot, dt)
# --- TRACERS
outputs = ['robotState']
robot.device_tracer = create_tracer(robot, robot.device, 'device_tracer', outputs)
outputs = ['q']
robot.estimator_tracer = create_tracer(robot, robot.base_estimator, 'estimator_tracer', outputs)
robot.device.after.addSignal('base_estimator.q')
# --- RUN SIMULATION
plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN)
sleep(1.0)
os.system("rosservice call \start_dynamic_graph")
sleep(2.0)
robot.comTrajGen.move(1, -0.025, 4.0)
sleep(5.0)
robot.comTrajGen.startSinusoid(1, 0.05, 8.0)
sleep(0.2)
robot.device_tracer.start()
robot.estimator_tracer.start()
sleep(3.0)
dump_tracer(robot.device_tracer)
dump_tracer(robot.estimator_tracer)
print('data dumped')
# --- DISPLAY
device_data = read_tracer_file('/tmp/dg_' + robot.device.name + '-robotState.dat')
estimator_data = read_tracer_file('/tmp/dg_' + robot.base_estimator.name + '-q.dat')
plot_select_traj(device_data, [10, 23, 15])
plot_select_traj(estimator_data, [10, 23, 15])
write_pdf_graph('/tmp/')
def main(robot):
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 10
robot.estimator = create_dummy_dcm_estimator(robot)
# --- Admittance controller
Kp = [0.0, 0.0, 0.0]
robot.com_admittance_control = create_com_admittance_controller(
Kp, dt, robot)
# --- CONTACTS
# define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
addTrace(robot.tracer, robot.dynamic, 'zmp')
addTrace(robot.tracer, robot.estimator, 'dcm')
# SIMULATION
plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
sleep(1.0)
os.system("rosservice call \start_dynamic_graph")
sleep(2.0)
plug(robot.estimator.dcm, robot.com_admittance_control.zmpDes)
robot.com_admittance_control.setState(robot.dynamic.com.value,
[0.0, 0.0, 0.0])
robot.com_admittance_control.Kp.value = [10.0, 10.0, 0.0]
robot.tracer.start()
sleep(5.0)
dump_tracer(robot.tracer)
# --- DISPLAY
zmp_data = np.loadtxt('/tmp/dg_' + robot.dynamic.name + '-zmp.dat')
zmpDes_data = np.loadtxt('/tmp/dg_' + robot.estimator.name + '-dcm.dat')
plt.figure()
plt.plot(zmp_data[:, 1], 'b-')
plt.plot(zmpDes_data[:, 1], 'b--')
plt.plot(zmp_data[:, 2], 'r-')
plt.plot(zmpDes_data[:, 2], 'r--')
plt.title('ZMP real vs desired')
plt.legend(['Real x', 'Desired x', 'Real y', 'Desired y'])
plt.figure()
plt.plot(zmp_data[:, 1] - zmpDes_data[:, 1], 'b-')
plt.plot(zmp_data[:, 2] - zmpDes_data[:, 2], 'r-')
plt.title('ZMP error')
plt.legend(['Error on x', 'Error on y'])
plt.show()
plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 100.
robot.taskCom.task.setWithDerivative(True)
robot.taskCom.feature.selec.value = '011'
# --- Waist
robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT',
robot.OperationalPointsMap['waist'])
robot.keepWaist.feature.frame('desired')
robot.keepWaist.gain.setConstant(300)
plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
robot.keepWaist.feature.selec.value = '111000'
locals()['keepWaist'] = robot.keepWaist
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
# --- Plug SOT control to device through control manager
plug(robot.sot.control, robot.cm.ctrl_sot_input)
plug(robot.cm.u_safe, robot.device.control)
robot.sot.push(robot.taskUpperBody.name)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskComH.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.keepWaist.task.name)
# robot.sot.push(robot.taskPos.name)
# robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
robot.dynamic.Jwrist_3_joint, I4)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1
task_wrist.add(feature_wrist.name)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(robot.device, tracer, robot.device.name, "state")
addTrace(robot.device, tracer, feature_wrist._feature.name, "position")
addTrace(robot.device, tracer, feature_wrist._reference.name, "position")
# solver
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_waist.name)
solver.push(task_wrist.name)
solver.push(task_elbow.name)
solver.push(task_lift.name)
solver.push(task_wrist1.name)
solver.push(task_wrist2.name)
plug(solver.control, robot.device.control)
#robot.device.increment (0.01)
dt = 0.01
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts