class MetaTaskJoint(object):
def __init__(self, dyn, joint, name=None):
if name is None:
name = "joint" + str(joint)
self.dyn = dyn
self.name = name
self.joint = joint
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
self.selec = Selec_of_vector("selec" + name)
self.selec.selec(joint, joint + 1)
plug(dyn.position, self.selec.sin)
plug(self.selec.sout, self.feature.errorIN)
robotDim = len(dyn.position.value)
Id = identity(robotDim)
J = Id[joint:joint + 1]
self.feature.jacobianIN.value = matrixToTuple(J)
self.feature.setReference(self.featureDes.name)
def plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
@property
def ref(self):
return self.featureDes.errorIN.value
@ref.setter
def ref(self, v):
self.featureDes.errorIN.value = v
def create_floatingBase(robot):
from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force import FromLocalToGLobalFrame
floatingBase = FromLocalToGLobalFrame(robot.flex_est, "FloatingBase")
plug(robot.ff_locator.freeflyer_aa, floatingBase.sinPos)
from dynamic_graph.sot.core import Selec_of_vector
base_vel_no_flex = Selec_of_vector('base_vel_no_flex')
plug(robot.ff_locator.v, base_vel_no_flex.sin)
base_vel_no_flex.selec(0, 6)
plug(base_vel_no_flex.sout, floatingBase.sinVel)
return floatingBase
jacobian_posture = hstack([first_6, other_dof])
taskPosture.feature.jacobianIN.value = matrixToTuple(jacobian_posture)
taskPosture.feature.setReference(taskPosture.featureDes.name)
taskPosture.add(taskPosture.feature.name)
# Create the sequence player
aSimpleSeqPlay = SimpleSeqPlay('aSimpleSeqPlay')
aSimpleSeqPlay.load(
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz")
aSimpleSeqPlay.setTimeToStart(10.0)
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)
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
taskPosture.featureDes.errorIN.recompute(0)
def create_joint_vel_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointVel')
plug(robot.device.robotVelocity, encoders.sin)
encoders.selec(conf.controlled_joint + 6, conf.controlled_joint + 7)
return encoders
def create_joint_pos_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointPos')
plug(robot.device.robotState, encoders.sin)
encoders.selec(conf.controlled_joint + 6, conf.controlled_joint + 7)
return encoders
def create_encoders_velocity(robot):
encoders = Selec_of_vector('dqn')
plug(robot.device.robotVelocity, encoders.sin)
encoders.selec(6, NJ + 6)
return encoders
def create_encoders(robot):
encoders = Selec_of_vector('qn')
plug(robot.device.robotState, encoders.sin)
encoders.selec(6, NJ + 6)
return encoders
def create_torque_des_selector2(robot, conf):
encoders = Selec_of_vector('selecDdpTorqueDes2')
plug(robot.torque_ctrl.u, encoders.sin)
encoders.selec(31, 32)
return encoders
def create_tau_des_selector(robot, conf):
encoders = Selec_of_vector('selecDdpTauDes')
plug(robot.inv_dyn.tau_des, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_motor_pos_selector(robot, conf):
encoders = Selec_of_vector('selecDdpMotorPos')
plug(robot.device.motor_angles, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_pos_des_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointPosDes')
plug(robot.traj_gen.q, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_joint_torque_selector(robot, conf):
encoders = Selec_of_vector('selecDdpJointTorque')
plug(robot.device.ptorque, encoders.sin)
encoders.selec(conf.controlled_joint, conf.controlled_joint + 1)
return encoders
def create_base_encoders(robot):
base_encoders = Selec_of_vector('base_encoders')
plug(robot.device.robotState, base_encoders.sin)
base_encoders.selec(0, NJ + 6)
return base_encoders
