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 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 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 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);
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
class HRP2ModelBaseFlexEstimatorIMUForceEncoders(DGIMUModelBaseFlexEstimation):
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
# Create a dynamic ######################################
def createCenterOfMassFeatureAndTask(self,
dynamicTmp,
featureName,
featureDesName,
taskName,
selec='111',
ingain=1.):
dynamicTmp.com.recompute(0)
dynamicTmp.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(dynamicTmp.com, featureCom.errorIN)
plug(dynamicTmp.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = dynamicTmp.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain' + taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
def createOperationalPointFeatureAndTask(self,
dynamicTmp,
operationalPointName,
featureName,
taskName,
ingain=.2):
jacobianName = 'J{0}'.format(operationalPointName)
dynamicTmp.signal(operationalPointName).recompute(0)
dynamicTmp.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
dynamicTmp.signal(operationalPointName),
dynamicTmp.signal(jacobianName),
dynamicTmp.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createDynamic(self, state, name):
# Create dynamic
self.dynamicTmp = self.robot.loadModelFromJrlDynamics(
self.robot.name + name, self.robot.modelDir, self.robot.modelName,
self.robot.specificitiesPath, self.robot.jointRankPath,
DynamicHrp2_14)
self.dynamicTmp.dimension = self.dynamicTmp.getDimension()
if self.dynamicTmp.dimension != len(self.robot.halfSitting):
raise RuntimeError(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.format(len(self.halfSitting), self.dynamicTmp.dimension))
# Pluging position
plug(state, self.dynamicTmp.position)
self.derivative = True
# Pluging velocity
self.robot.enableVelocityDerivator = self.derivative
if self.robot.enableVelocityDerivator:
self.dynamicTmp.velocityDerivator = Derivator_of_Vector(
'velocityDerivator')
self.dynamicTmp.velocityDerivator.dt.value = self.robot.timeStep
plug(state, self.dynamicTmp.velocityDerivator.sin)
plug(self.dynamicTmp.velocityDerivator.sout,
self.dynamicTmp.velocity)
else:
self.dynamicTmp.velocity.value = self.dynamicTmp.dimension * (0., )
# Pluging acceleration
self.robot.enableAccelerationDerivator = self.derivative
if self.robot.enableAccelerationDerivator:
self.dynamicTmp.accelerationDerivator = Derivator_of_Vector(
'accelerationDerivator')
self.dynamicTmp.accelerationDerivator.dt.value = self.robot.timeStep
plug(self.dynamicTmp.velocityDerivator.sout,
self.dynamicTmp.accelerationDerivator.sin)
plug(self.dynamicTmp.accelerationDerivator.sout,
self.dynamicTmp.acceleration)
else:
self.dynamicTmp.acceleration.value = self.dynamicTmp.dimension * (
0., )
# # --- center of mass ------------
# (self.featureCom, self.featureComDes, self.taskCom, self.gainCom) = \
# self.createCenterOfMassFeatureAndTask\
# (self.dynamicTmp, '{0}_feature_com'.format(self.robot.name),
# '{0}_feature_ref_com'.format(self.robot.name),
# '{0}_task_com'.format(self.robot.name))
# --- operational points tasks -----
self.robot.features = dict()
self.robot.tasks = dict()
self.robot.gains = dict()
for op in self.robot.OperationalPoints:
opName = op + name
self.dynamicTmp.createOpPoint(op, op)
(self.robot.features[opName], self.robot.tasks[opName], self.robot.gains[opName]) = \
self.createOperationalPointFeatureAndTask(self.dynamicTmp, op,
'{0}_feature_{1}'.format(self.robot.name, opName),
'{0}_task_{1}'.format(self.robot.name, opName))
# define a member for each operational point
w = op.split('-')
memberName = w[0]
for i in w[1:]:
memberName += i.capitalize()
setattr(self, memberName, self.robot.features[opName])
# self.robot.tasks ['com'] = self.taskCom
# self.robot.features ['com'] = self.featureCom
# self.robot.gains['com'] = self.gainCom
self.robot.features['waist' + name].selec.value = '011100'
return self.dynamicTmp
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
class HRP2ModelBaseFlexEstimatorIMUForceEncoders(DGIMUModelBaseFlexEstimation):
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
# Create a dynamic ######################################
def createCenterOfMassFeatureAndTask(self,
dynamicTmp,
featureName, featureDesName,
taskName,
selec = '111',
ingain = 1.):
dynamicTmp.com.recompute(0)
dynamicTmp.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(dynamicTmp.com, featureCom.errorIN)
plug(dynamicTmp.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = dynamicTmp.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain'+taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
def createOperationalPointFeatureAndTask(self,
dynamicTmp,
operationalPointName,
featureName,
taskName,
ingain = .2):
jacobianName = 'J{0}'.format(operationalPointName)
dynamicTmp.signal(operationalPointName).recompute(0)
dynamicTmp.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
dynamicTmp.signal(operationalPointName),
dynamicTmp.signal(jacobianName),
dynamicTmp.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain'+taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createDynamic(self,state,name) :
# Create dynamic
self.dynamicTmp = self.robot.loadModelFromJrlDynamics(
self.robot.name + name,
self.robot.modelDir,
self.robot.modelName,
self.robot.specificitiesPath,
self.robot.jointRankPath,
DynamicHrp2_14)
self.dynamicTmp.dimension = self.dynamicTmp.getDimension()
if self.dynamicTmp.dimension != len(self.robot.halfSitting):
raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dynamicTmp.dimension))
# Pluging position
plug(state, self.dynamicTmp.position)
self.derivative=True
# Pluging velocity
self.robot.enableVelocityDerivator = self.derivative
if self.robot.enableVelocityDerivator:
self.dynamicTmp.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.dynamicTmp.velocityDerivator.dt.value = self.robot.timeStep
plug(state, self.dynamicTmp.velocityDerivator.sin)
plug(self.dynamicTmp.velocityDerivator.sout, self.dynamicTmp.velocity)
else:
self.dynamicTmp.velocity.value = self.dynamicTmp.dimension*(0.,)
# Pluging acceleration
self.robot.enableAccelerationDerivator = self.derivative
if self.robot.enableAccelerationDerivator:
self.dynamicTmp.accelerationDerivator = Derivator_of_Vector('accelerationDerivator')
self.dynamicTmp.accelerationDerivator.dt.value = self.robot.timeStep
plug(self.dynamicTmp.velocityDerivator.sout, self.dynamicTmp.accelerationDerivator.sin)
plug(self.dynamicTmp.accelerationDerivator.sout, self.dynamicTmp.acceleration)
else:
self.dynamicTmp.acceleration.value = self.dynamicTmp.dimension*(0.,)
# # --- center of mass ------------
# (self.featureCom, self.featureComDes, self.taskCom, self.gainCom) = \
# self.createCenterOfMassFeatureAndTask\
# (self.dynamicTmp, '{0}_feature_com'.format(self.robot.name),
# '{0}_feature_ref_com'.format(self.robot.name),
# '{0}_task_com'.format(self.robot.name))
# --- operational points tasks -----
self.robot.features = dict()
self.robot.tasks = dict()
self.robot.gains = dict()
for op in self.robot.OperationalPoints:
opName= op + name
self.dynamicTmp.createOpPoint(op, op)
(self.robot.features[opName], self.robot.tasks[opName], self.robot.gains[opName]) = \
self.createOperationalPointFeatureAndTask(self.dynamicTmp, op,
'{0}_feature_{1}'.format(self.robot.name, opName),
'{0}_task_{1}'.format(self.robot.name, opName))
# define a member for each operational point
w = op.split('-')
memberName = w[0]
for i in w[1:]:
memberName += i.capitalize()
setattr(self, memberName, self.robot.features[opName])
# self.robot.tasks ['com'] = self.taskCom
# self.robot.features ['com'] = self.featureCom
# self.robot.gains['com'] = self.gainCom
self.robot.features['waist'+name].selec.value = '011100'
return self.dynamicTmp
plug(est2.flexibility,integr.sin)
#appli.robot.addTrace( est2.name,'flexThetaU' )
#appli.robot.addTrace( est2.name,'flexOmega' )
#appli.robot.addTrace( deriv.name,'sout')
#appli.robot.addTrace( integr.name,'sout')
zmp = ZmpFromForces('zmp')
plug (robot.device.forceRLEG , zmp.force_0)
plug (robot.device.forceLLEG, zmp.force_1)
plug (robot.frames['rightFootForceSensor'].position , zmp.sensorPosition_0)
plug (robot.frames['leftFootForceSensor'].position, zmp.sensorPosition_1)
appli.robot.addTrace( zmp.name, 'zmp')
zmpnoisy = ZmpFromForces('zmpnoisy')
firstContactForceEst = Selec_of_vector('firstContactForcenoise')
secondContactForceEst = Selec_of_vector('secondContactForcenoise')
firstContactForceEst.selec(0,6)
secondContactForceEst.selec(6,12)
plug ( perturbator.sout, firstContactForceEst.sin)
plug ( perturbator.sout, secondContactForceEst.sin)
plug (firstContactForceEst.sout , zmpnoisy.force_0)
plug (secondContactForceEst.sout, zmpnoisy.force_1)
plug (robot.frames['rightFootForceSensor'].position , zmpnoisy.sensorPosition_1)
plug (robot.frames['leftFootForceSensor'].position, zmpnoisy.sensorPosition_0)
appli.robot.addTrace( zmpnoisy.name, 'zmp')
zmpestimated0 = ZmpFromForces('zmpestimated')
firstContactForceEst = Selec_of_vector('firstContactForceEst')
secondContactForceEst = Selec_of_vector('secondContactForceEst')
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
