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
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
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 ()
"/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.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
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)
taskRH.opmodif = matrixToTuple(handMgrip)
def newSot(name):
sot_keep = SOT (name)
sot_keep.setSize(robot.dynamic.getDimension())
sot_keep.damping.value = 0.001
return name
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
