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 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;
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
def main(dt=0.001, delay=0.01):
np.set_printoptions(precision=3, suppress=True)
COM_DES_1 = (0.012, 0.1, 0.81)
COM_DES_2 = (0.012, 0.0, 0.81)
dt = conf.dt
q0 = conf.q0_urdf
v0 = conf.v0
nv = v0.shape[0]
simulator = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt,
conf.model_path, conf.urdfFileName)
simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS
simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS
simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS
simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS
simulator.VIEWER_DT = conf.DT_VIEWER
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3
simulator.verb = 0
robot = simulator.r
device = create_device(conf.q0_sot)
from dynamic_graph.sot.core import Selec_of_vector
joint_vel = Selec_of_vector("joint_vel")
plug(device.velocity, joint_vel.sin)
joint_vel.selec(6, 36)
from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator
ff_locator = FreeFlyerLocator("ffLocator")
plug(device.state, ff_locator.base6d_encoders)
plug(joint_vel.sout, ff_locator.joint_velocities)
ff_locator.init(conf.urdfFileName)
# ff_locator = create_free_flyer_locator(device, conf.urdfFileName);
# traj_gen = create_trajectory_generator(device, dt);
traj_gen = JointTrajectoryGenerator("jtg")
plug(device.state, traj_gen.base6d_encoders)
traj_gen.init(dt)
# estimator = create_estimator(device, dt, delay, traj_gen);
# torque_ctrl = create_torque_controller(device, estimator);
# pos_ctrl = create_position_controller(device, estimator, dt, traj_gen);
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
plug(device.state, ctrl.q)
plug(traj_gen.q, ctrl.posture_ref_pos)
plug(traj_gen.dq, ctrl.posture_ref_vel)
plug(traj_gen.ddq, ctrl.posture_ref_acc)
# plug(estimator.contactWrenchRightSole, ctrl.wrench_right_foot);
# plug(estimator.contactWrenchLeftSole, ctrl.wrench_left_foot);
# plug(ctrl.tau_des, torque_ctrl.jointsTorquesDesired);
# plug(ctrl.tau_des, estimator.tauDes);
ctrl.com_ref_pos.value = to_tuple(robot.com(q0))
ctrl.com_ref_pos.value = COM_DES_1
ctrl.com_ref_vel.value = 3 * (0.0, )
ctrl.com_ref_acc.value = 3 * (0.0, )
ctrl.rotor_inertias.value = conf.ROTOR_INERTIAS
ctrl.gear_ratios.value = conf.GEAR_RATIOS
ctrl.contact_normal.value = (0.0, 0.0, 1.0)
ctrl.contact_points.value = conf.RIGHT_FOOT_CONTACT_POINTS
ctrl.f_min.value = conf.fMin
ctrl.mu.value = conf.mu[0]
ctrl.weight_contact_forces.value = (1e2, 1e2, 1e0, 1e3, 1e3, 1e3)
ctrl.kp_com.value = 3 * (conf.kp_com, )
ctrl.kd_com.value = 3 * (conf.kd_com, )
ctrl.kp_constraints.value = 6 * (conf.kp_constr, )
ctrl.kd_constraints.value = 6 * (conf.kd_constr, )
ctrl.kp_posture.value = 30 * (conf.kp_posture, )
ctrl.kd_posture.value = 30 * (conf.kd_posture, )
ctrl.kp_pos.value = 30 * (0 * conf.kp_pos, )
ctrl.kd_pos.value = 30 * (0 * conf.kd_pos, )
ctrl.w_com.value = conf.w_com
ctrl.w_forces.value = conf.w_forces
ctrl.w_posture.value = conf.w_posture
ctrl.w_base_orientation.value = conf.w_base_orientation
ctrl.w_torques.value = conf.w_torques
ctrl.init(dt, conf.urdfFileName)
ctrl.active_joints.value = conf.active_joints
# ctrl_manager = create_ctrl_manager(device, torque_ctrl, pos_ctrl, ctrl, estimator, dt);
# plug(device.velocity, torque_ctrl.jointsVelocities);
plug(device.velocity, ctrl.v)
plug(ctrl.dv_des, device.control)
t = 0.0
v = mat_zeros(nv)
dv = mat_zeros(nv)
x_rf = robot.framePosition(
robot.model.getFrameId('RLEG_JOINT5')).translation
x_lf = robot.framePosition(
robot.model.getFrameId('LLEG_JOINT5')).translation
simulator.viewer.addSphere('zmp_rf', 0.01, mat_zeros(3), mat_zeros(3),
(0.8, 0.3, 1.0, 1.0), 'OFF')
simulator.viewer.addSphere('zmp_lf', 0.01, mat_zeros(3), mat_zeros(3),
(0.8, 0.3, 1.0, 1.0), 'OFF')
simulator.viewer.addSphere('zmp', 0.01, mat_zeros(3), mat_zeros(3),
(0.8, 0.8, 0.3, 1.0), 'OFF')
for i in range(conf.MAX_TEST_DURATION):
# if(norm(dv[6:24]) > 1e-8):
# print "ERROR acceleration of blocked axes is not zero:", norm(dv[6:24]);
ff_locator.base6dFromFoot_encoders.recompute(i)
ff_locator.v.recompute(i)
device.increment(dt)
if (i == 1500):
ctrl.com_ref_pos.value = COM_DES_2
if (i % 30 == 0):
q = np.matrix(device.state.value).T
q_urdf = config_sot_to_urdf(q)
simulator.viewer.updateRobotConfig(q_urdf)
ctrl.f_des_right_foot.recompute(i)
ctrl.f_des_left_foot.recompute(i)
f_rf = np.matrix(ctrl.f_des_right_foot.value).T
f_lf = np.matrix(ctrl.f_des_left_foot.value).T
simulator.updateContactForcesInViewer(['rf', 'lf'], [x_rf, x_lf],
[f_rf, f_lf])
ctrl.zmp_des_right_foot.recompute(i)
ctrl.zmp_des_left_foot.recompute(i)
ctrl.zmp_des.recompute(i)
zmp_rf = np.matrix(ctrl.zmp_des_right_foot.value).T
zmp_lf = np.matrix(ctrl.zmp_des_left_foot.value).T
zmp = np.matrix(ctrl.zmp_des.value).T
simulator.viewer.updateObjectConfig(
'zmp_rf', (zmp_rf[0, 0], zmp_rf[1, 0], 0., 0., 0., 0., 1.))
simulator.viewer.updateObjectConfig(
'zmp_lf', (zmp_lf[0, 0], zmp_lf[1, 0], 0., 0., 0., 0., 1.))
simulator.viewer.updateObjectConfig(
'zmp', (zmp[0, 0], zmp[1, 0], 0., 0., 0., 0., 1.))
ctrl.com.recompute(i)
com = np.matrix(ctrl.com.value).T
com[2, 0] = 0.0
simulator.updateComPositionInViewer(com)
if (i % 100 == 0):
ctrl.com.recompute(i)
com = np.matrix(ctrl.com.value).T
v = np.matrix(device.velocity.value).T
dv = np.matrix(ctrl.dv_des.value).T
ctrl.zmp_des_right_foot_local.recompute(i)
ctrl.zmp_des_left_foot_local.recompute(i)
ctrl.zmp_des.recompute(i)
zmp_rf = np.matrix(ctrl.zmp_des_right_foot_local.value).T
zmp_lf = np.matrix(ctrl.zmp_des_left_foot_local.value).T
zmp = np.matrix(ctrl.zmp_des.value).T
print "t=%.3f dv=%.1f v=%.1f com=" % (t, norm(dv), norm(v)), com.T
print "zmp_lf", np.array([-f_lf[4, 0], f_lf[3, 0]
]) / f_lf[2, 0], zmp_lf.T
print "zmp_rf", np.array([-f_rf[4, 0], f_rf[3, 0]
]) / f_rf[2, 0], zmp_rf.T, '\n'
# print "Base pos (real)", device.state.value[:3];
# print "Base pos (esti)", ff_locator.base6dFromFoot_encoders.value[:3], '\n';
# print "Base velocity (real)", np.array(device.velocity.value[:6]);
# print "Base velocity (esti)", np.array(ff_locator.v.value[:6]), '\n';
if (i == 2):
ctrl_manager = ControlManager("ctrl_man")
ctrl_manager.resetProfiler()
t += dt
sleep(dt)
return (simulator, ctrl)
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)
# 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)
def create_balance_controller(robot,
conf,
motor_params,
dt,
robot_name='robot'):
from dynamic_graph.sot.torque_control.inverse_dynamics_balance_controller import InverseDynamicsBalanceController
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
try:
plug(robot.base_estimator.q, ctrl.q)
plug(robot.base_estimator.v, ctrl.v)
except:
plug(robot.ff_locator.base6dFromFoot_encoders, ctrl.q)
plug(robot.ff_locator.v, ctrl.v)
try:
from dynamic_graph.sot.core import Selec_of_vector
robot.ddq_des = Selec_of_vector('ddq_des')
plug(ctrl.dv_des, robot.ddq_des.sin)
robot.ddq_des.selec(6, NJ + 6)
except:
print "WARNING: Could not connect dv_des from BalanceController to ForceTorqueEstimator"
plug(ctrl.tau_des, robot.torque_ctrl.jointsTorquesDesired)
plug(ctrl.right_foot_pos, robot.rf_traj_gen.initial_value)
plug(robot.rf_traj_gen.x, ctrl.rf_ref_pos)
plug(robot.rf_traj_gen.dx, ctrl.rf_ref_vel)
plug(robot.rf_traj_gen.ddx, ctrl.rf_ref_acc)
plug(ctrl.left_foot_pos, robot.lf_traj_gen.initial_value)
plug(robot.lf_traj_gen.x, ctrl.lf_ref_pos)
plug(robot.lf_traj_gen.dx, ctrl.lf_ref_vel)
plug(robot.lf_traj_gen.ddx, ctrl.lf_ref_acc)
plug(robot.traj_gen.q, ctrl.posture_ref_pos)
plug(robot.traj_gen.dq, ctrl.posture_ref_vel)
plug(robot.traj_gen.ddq, ctrl.posture_ref_acc)
plug(robot.com_traj_gen.x, ctrl.com_ref_pos)
plug(robot.com_traj_gen.dx, ctrl.com_ref_vel)
plug(robot.com_traj_gen.ddx, ctrl.com_ref_acc)
# plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
# plug(robot.lf_force_traj_gen.x, ctrl.f_ref_left_foot);
# rather than giving to the controller the values of gear ratios and rotor inertias
# it is better to compute directly their product in python and pass the result
# to the C++ entity, because otherwise we get a loss of precision
# ctrl.rotor_inertias.value = conf.ROTOR_INERTIAS;
# ctrl.gear_ratios.value = conf.GEAR_RATIOS;
ctrl.rotor_inertias.value = tuple([
g * g * r
for (g,
r) in zip(motor_params.GEAR_RATIOS, motor_params.ROTOR_INERTIAS)
])
ctrl.gear_ratios.value = NJ * (1.0, )
ctrl.contact_normal.value = conf.FOOT_CONTACT_NORMAL
ctrl.contact_points.value = conf.RIGHT_FOOT_CONTACT_POINTS
ctrl.f_min.value = conf.fMin
ctrl.f_max_right_foot.value = conf.fMax
ctrl.f_max_left_foot.value = conf.fMax
ctrl.mu.value = conf.mu[0]
ctrl.weight_contact_forces.value = (1e2, 1e2, 1e0, 1e3, 1e3, 1e3)
ctrl.kp_com.value = 3 * (conf.kp_com, )
ctrl.kd_com.value = 3 * (conf.kd_com, )
ctrl.kp_constraints.value = 6 * (conf.kp_constr, )
ctrl.kd_constraints.value = 6 * (conf.kd_constr, )
ctrl.kp_feet.value = 6 * (conf.kp_feet, )
ctrl.kd_feet.value = 6 * (conf.kd_feet, )
ctrl.kp_posture.value = conf.kp_posture
ctrl.kd_posture.value = conf.kd_posture
ctrl.kp_pos.value = conf.kp_pos
ctrl.kd_pos.value = conf.kd_pos
ctrl.w_com.value = conf.w_com
ctrl.w_feet.value = conf.w_feet
ctrl.w_forces.value = conf.w_forces
ctrl.w_posture.value = conf.w_posture
ctrl.w_base_orientation.value = conf.w_base_orientation
ctrl.w_torques.value = conf.w_torques
ctrl.init(dt, robot_name)
return ctrl
def create_encoders(robot):
from dynamic_graph.sot.core import Selec_of_vector
encoders = Selec_of_vector('qn')
plug(robot.device.robotState, encoders.sin)
encoders.selec(6, NJ + 6)
return encoders
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self, name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(
matrixToTuple(
np.diag((1e-8, ) * 12 + (1e-4, ) * 3 + (1e-4, ) * 3 +
(1e-4, ) * 3 + (1e-4, ) * 3 + (1.e-2, ) * 6 +
(1e-15, ) * 2 + (1.e-8, ) * 3)))
self.setMeasurementNoiseCovariance(
matrixToTuple(np.diag((1e-3, ) * 3 + (1e-6, ) * 3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000))))
self.setKfv(matrixToTuple(np.diag((600, 600, 600))))
self.setKte(matrixToTuple(np.diag((600, 600, 600))))
self.setKtv(matrixToTuple(np.diag((60, 60, 60))))
#Estimator interface
self.interface = EstimatorInterface(name + "EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0., 0., 1.57))
self.interface.setRightHandSensorTransformation((0., 0., 1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug(self.robot.device.forceLLEG, self.interface.force_lf)
plug(self.robot.device.forceRLEG, self.interface.force_rf)
plug(self.robot.device.forceLARM, self.interface.force_lh)
plug(self.robot.device.forceRARM, self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value = 46 * (0., )
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState, self.robotState.sin)
self.robotState.selec(0, 36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo = self.createDynamic(self.robotState.sout,
'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo = Multiply_of_matrixHomo(name + "leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),
self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo = Multiply_of_matrixHomo(name + "rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),
self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value = self.robot.forceSensorInRightAnkle
# Odometry
self.odometry = Odometry(name + 'odometry')
plug(self.robot.frames['leftFootForceSensor'].position,
self.odometry.leftFootPositionRef)
plug(self.robot.frames['rightFootForceSensor'].position,
self.odometry.rightFootPositionRef)
plug(self.rightFootPosOdo.sout, self.odometry.rightFootPositionIn)
plug(self.leftFootPosOdo.sout, self.odometry.leftFootPositionIn)
plug(self.robot.device.forceLLEG, self.odometry.force_lf)
plug(self.robot.device.forceRLEG, self.odometry.force_rf)
self.odometry.setLeftFootPosition(
self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(
self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,
self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState, self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6, 36)
self.stateEncoders = Stack_of_vector(name + 'stateEncoders')
plug(self.odometry.freeFlyer, self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout, self.stateEncoders.sin2)
self.stateEncoders.selec1(0, 6)
self.stateEncoders.selec2(0, 30)
self.robot.dynamicEncoders = self.createDynamic(
self.stateEncoders.sout, '_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos = Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),
self.leftFootPos.sin1)
self.leftFootPos.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPos = Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),
self.rightFootPos.sin1)
self.rightFootPos.sin2.value = self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,
self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,
self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug(self.leftFootPos.sout, self.interface.position_lf)
plug(self.rightFootPos.sout, self.interface.position_rf)
plug(self.leftFootVelocity.sout, self.interface.velocity_lf)
plug(self.rightFootVelocity.sout, self.interface.velocity_rf)
plug(self.robot.dynamicEncoders.signal('right-wrist'),
self.interface.position_lh)
plug(self.robot.dynamicEncoders.signal('left-wrist'),
self.interface.position_rh)
# Compute contacts number
plug(self.interface.supportContactsNbr, self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel, self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config, self.config)
# Drift
self.drift = DriftFromMocap(name + 'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer, self.interface.accelerometer)
plug(self.robot.device.gyrometer, self.interface.gyrometer)
plug(self.drift.driftVector, self.interface.drift)
plug(self.interface.measurement, self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name + 'ChestJ_OpPoint',
'chest')
self.imuOpPoint = OpPointModifier(name + 'IMU_oppoint')
self.imuOpPoint.setTransformation(
matrixToTuple(
np.linalg.inv(np.matrix(
self.robot.dynamicEncoders.chest.value)) *
np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug(self.robot.dynamicEncoders.chest, self.imuOpPoint.positionIN)
plug(self.robot.dynamicEncoders.signal(name + 'ChestJ_OpPoint'),
self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer = Multiply_of_matrixHomo(name +
"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest, self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value = matrixToTuple(
self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name + 'InputPosition')
plug(self.PosAccelerometer.sout, self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name + 'InputVelocity')
plug(self.imuOpPoint.jacobian, self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity, self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector(name + 'InputPosVel')
plug(self.inputPos.sout, self.inputPosVel.sin1)
plug(self.inputVel.sout, self.inputPosVel.sin2)
self.inputPosVel.selec1(0, 6)
self.inputPosVel.selec2(0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor(name + 'EstimatorInput')
plug(self.inputPosVel.sout, self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn = self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor(name + 'ComVector')
plug(self.comIn, self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name + 'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,
self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout, self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia, self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,
self.interface.angMomentum)
plug(self.angMomDerivator.sout, self.interface.dangMomentum)
self.interface.dinertia.value = (0, 0, 0, 0, 0, 0)
plug(self.robot.dynamicEncoders.waist, self.interface.positionWaist)
plug(self.IMUVector.sout, self.interface.imuVector)
plug(self.interface.input, self.input)
self.robot.flextimator = self
def create_base_encoders(robot):
from dynamic_graph.sot.core import Selec_of_vector
base_encoders = Selec_of_vector('base_encoders')
plug(robot.device.robotState, base_encoders.sin)
base_encoders.selec(0, NJ + 6)
return base_encoders
# Create the sequence player
aPlayHppPath = PathSampler('aPlayHppPath')
aPlayHppPath.connect("ontake", 2809)
aPlayHppPath.setRootJointType("freeflyer")
aPlayHppPath.setPathID(1)
aPlayHppPath.setTimeStep(0.001)
for seg in hpp_to_keep:
aPlayHppPath.addParamSelection(seg[0], seg[1])
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import Selec_of_vector
from dynamic_graph import plug
# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getTargetValue = Selec_of_vector("current_target")
getPostureValue.selec(6, robotDim)
getTargetValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(aPlayHppPath.configuration, getTargetValue.sin)
plug(getTargetValue.sout, taskPosture.featureDes.errorIN)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(robot.dynamic.position, aPlayHppPath.position)
# 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)
