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)
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)
target = (0.5, -0.2, 1.0)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
sot.push(taskRH.task.name)
robot.device.control.recompute(0)
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
targetRH = (0.5, -0.2, 1.0)
targetLH = (0.5, 0.2, 1.0)
# addRobotViewer(robot, small=False)
# robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH, targetRH, '111', (4.9, 0.9, 0.01, 0.9))
gotoNd(taskLH, targetLH, '111', (4.9, 0.9, 0.01, 0.9))
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
sot.push(taskLH.task.name)
sot.push(taskRH.task.name)
toe = TOE("test_entity_toe")
id4 = ((1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.0),
(0.0, 0.0, 0.0, 1.0))
epsilon = 0.1
ref_offset = 0.1
tau_offset = np.asarray((ref_offset, ) * njoints).squeeze()
toe.init(urdfPath, id4, epsilon, OperationalPointsMap['waist'],
OperationalPointsMap['chest'])
plug(robot.device.state, toe.base6d_encoders)
toe.gyroscope.value = (0., 0., 0.)
taskTwofeet.signal('controlGain').value = 0.
# --- CONTACT CONSTRAINT
legs = Constraint('legs')
legs.addJacobian('dyn.Jlleg')
# --- SOT
sot = SOT('sot')
sot.signal('damping').value = 1e-6
sot.addConstraint('legs')
sot.setNumberDofs(dimension)
# plug dyn.inertia sot.weight
# sot.push taskTwofeet
# sot.push taskLegs
sot.push('task')
sot.push('taskCom')
# sot.push taskChest
# sot.gradient taskJl
featureComDes.signal('errorIN').value = (0.004,-0.09,.6)
zeroCom = VectorConstant('zeroCom')
zeroCom.set(dimension*(0.,))
dyn.createOpPoint('Waist', 'waist')
OpenHRP = RobotSimu('OpenHRP')
OpenHRP.set((0.,0.,0.,0.,0.,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.0000,0.,-0.,-0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.))
taskTwofeet.signal('controlGain').value = 0.
# --- CONTACT CONSTRAINT
legs = Constraint('legs')
legs.addJacobian('dyn.Jlleg')
# --- SOT
sot = SOT('sot')
sot.signal('damping').value = 1e-6
sot.addConstraint('legs')
sot.setNumberDofs(dimension)
# plug dyn.inertia sot.weight
# sot.push taskTwofeet
# sot.push taskLegs
sot.push('task')
# sot.push taskCom
# sot.push taskChest
# sot.gradient taskJl
featureComDes.signal('errorIN').value = (0.004, -0.09, .6)
zeroCom = VectorConstant('zeroCom')
zeroCom.set(dimension * (0., ))
dyn.createOpPoint('Waist', 'waist')
OpenHRP = RobotSimu('OpenHRP')
OpenHRP.set((0., 0., 0., 0., 0., 0., 0., 0., -1.04720, 2.09440, -1.04720, 0.,
0., 0., -1.04720, 2.09440, -1.04720, 0., 0.0000, 0., -0., -0.,
0.17453, -0.17453, -0.17453, -0.87266, 0., -0., 0.1, 0., 0.17453,
plug(seqplay.posture, taskPosture.featureDes.errorIN)
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, seqplay.currentPosture)
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
# START SEQUENCE PLAYER
seqplay.start()
taskPosture.featureDes.errorIN.recompute(0)
# PUSH TO SOLVER
sot.push(taskPosture.name)
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
def runner(n):
for i in range(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
runner(3000)
taskTwofeet.signal('controlGain').value = 0.
# --- CONTACT CONSTRAINT
legs = Constraint('legs')
legs.addJacobian('dyn.Jlleg')
# --- SOT
sot = SOT('sot')
sot.signal('damping').value = 1e-6
sot.addConstraint('legs')
sot.setNumberDofs(dimension)
# plug dyn.inertia sot.weight
# sot.push taskTwofeet
# sot.push taskLegs
sot.push('task')
# sot.push taskCom
# sot.push taskChest
# sot.gradient taskJl
featureComDes.signal('errorIN').value = (0.004,-0.09,.6)
zeroCom = VectorConstant('zeroCom')
zeroCom.set(dimension*(0.,))
dyn.createOpPoint('Waist', 'waist')
OpenHRP = RobotSimu('OpenHRP')
OpenHRP.set((0.,0.,0.,0.,0.,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.0000,0.,-0.,-0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.))
# define contactLF and contactRF
for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']],
['RF', robot.OperationalPointsMap['right-ankle']]]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact' + name] = contact
target = (0.5, -0.2, 1.0)
# addRobotViewer(robot, small=False)
# robot.viewer.updateElementConfig('zmp',target+(0,0,0))
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
sot.push(contactRF.task.name) # noqa TODO
sot.push(contactLF.task.name) # noqa TODO
sot.push(taskCom.task.name)
sot.push(taskRH.task.name)
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
def runner(n):
for i in range(n):
robot.device.increment(dt)
pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))
