def create_balance_controller(dt, q, conf, robot_name='robot'):
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
ctrl.q.value = tuple(q)
ctrl.v.value = (NJ + 6) * (0.0, )
ctrl.wrench_right_foot.value = 6 * (0.0, )
ctrl.wrench_left_foot.value = 6 * (0.0, )
ctrl.posture_ref_pos.value = tuple(q[6:])
ctrl.posture_ref_vel.value = NJ * (0.0, )
ctrl.posture_ref_acc.value = NJ * (0.0, )
ctrl.com_ref_pos.value = (0., 0., 0.8)
ctrl.com_ref_vel.value = 3 * (0.0, )
ctrl.com_ref_acc.value = 3 * (0.0, )
# ctrl.rotor_inertias.value = np.array(conf.ROTOR_INERTIAS);
# ctrl.gear_ratios.value = conf.GEAR_RATIOS;
ctrl.rotor_inertias.value = tuple(
[g * g * r for (g, r) in zip(conf.GEAR_RATIOS, conf.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.active_joints.value = NJ * (1, )
ctrl.init(dt, robot_name)
return ctrl
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 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 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 create_balance_controller(robot, conf, 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);
plug(robot.estimator_ft.contactWrenchRightSole, ctrl.wrench_right_foot);
plug(robot.estimator_ft.contactWrenchLeftSole, ctrl.wrench_left_foot);
plug(ctrl.tau_des, robot.torque_ctrl.jointsTorquesDesired);
plug(ctrl.tau_des, robot.estimator_ft.tauDes);
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);
# 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(conf.GEAR_RATIOS, conf.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_balance_controller(device,
floatingBase,
estimator,
torque_ctrl,
traj_gen,
com_traj_gen,
urdfFileName,
dt=0.001,
ff_locator=None):
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
if (floatingBase != None):
from dynamic_graph.sot.core import Stack_of_vector
base6d_encoders = Stack_of_vector('base6d_encoders')
plug(floatingBase.soutPos, base6d_encoders.sin1)
base6d_encoders.selec1(0, 6)
plug(device.robotState, base6d_encoders.sin2)
base6d_encoders.selec2(6, 36)
plug(base6d_encoders.sout, ctrl.q)
v = Stack_of_vector('v')
plug(floatingBase.soutVel, v.sin1)
v.selec1(0, 6)
plug(estimator.jointsVelocities, v.sin2)
v.selec2(6, 36)
plug(v.sout, ctrl.v)
else:
plug(ff_locator.base6dFromFoot_encoders, ctrl.q)
plug(ff_locator.v, ctrl.v)
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)
import balance_ctrl_conf as conf
plug(com_traj_gen.x, ctrl.com_ref_pos)
plug(com_traj_gen.dx, ctrl.com_ref_vel)
plug(com_traj_gen.ddx, ctrl.com_ref_acc)
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 * (conf.kp_pos, )
ctrl.kd_pos.value = 30 * (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, urdfFileName)
return ctrl