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