# --- SET INITIAL CONFIGURATION ------------------------------------------------
# TMP: overwrite halfSitting configuration to use SoT joint order
q = [0., 0., 1.018213, 0., 0., 0.] # Free flyer
q += [0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708] # legs
q += [0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708] # legs
q += [0.0, 0.006761] # Chest
q += [0.25847, 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005] # arms
q += [-0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005] # arms
q += [0., 0.] # Head
robot.halfSitting = q
# --- CREATE ENTITIES ----------------------------------------------------------
robot.ctrl_manager = create_ctrl_manager(conf.control_manager, conf.motor_params, dt)
robot.encoders = create_encoders(robot)
# --- Posture trajectory
robot.traj_gen = create_trajectory_generator(robot, dt)
robot.traj_gen.q.recompute(0)
# --- CoM trajectory
robot.com_traj_gen = create_com_traj_gen(robot, dt)
robot.com_traj_gen.x.recompute(0)
# --- Base orientation (SE3 on the waist) trajectory
robot.waist_traj_gen = create_waist_traj_gen("tg_waist_ref", robot, dt)
robot.waist_traj_gen.x.recompute(0)
# --- Simple inverse dynamic controller
robot.inv_dyn = create_simple_inverse_dyn_controller(robot, conf.balance_ctrl, dt)
robot.inv_dyn.setControlOutputType("torque")
def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
if (conf is None):
conf = get_default_conf()
dt = robot.timeStep
# TMP: overwrite halfSitting configuration to use SoT joint order
robot.halfSitting = (
# Free flyer
0.,
0.,
1.018213,
0.,
0.,
0.,
# legs
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708,
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708,
# Chest
0.0,
0.006761,
# arms
0.25847,
0.173046,
-0.0002,
-0.525366,
0.0,
-0.0,
0.1,
-0.005,
-0.25847,
-0.173046,
0.0002,
-0.525366,
0.0,
0.0,
0.1,
-0.005,
# Head
0.0,
0.0)
#robot.device.setControlInputType('noInteg');
robot.ctrl_manager = create_ctrl_manager(conf.control_manager,
conf.motor_params, dt)
robot.traj_gen = create_trajectory_generator(robot.device, dt)
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt)
robot.rf_force_traj_gen = create_force_traj_gen(
"rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt)
robot.lf_force_traj_gen = create_force_traj_gen(
"lf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt)
robot.traj_sync = create_trajectory_switch()
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf")
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf")
robot.rf_traj_gen.init(dt)
robot.lf_traj_gen.init(dt)
robot.encoders = create_encoders(robot)
robot.encoders_velocity = create_encoders_velocity(robot)
robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt)
#(robot.estimator_ft, robot.filters) = create_estimators(robot, conf.force_torque_estimator, conf.motor_params, dt);
robot.filters = create_filters(robot, conf.force_torque_estimator,
conf.motor_params, dt)
robot.imu_filter = create_imu_filter(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt,
conf.base_estimator)
connect_synchronous_trajectories(robot.traj_sync, [
robot.com_traj_gen, robot.rf_force_traj_gen, robot.lf_force_traj_gen,
robot.rf_traj_gen, robot.lf_traj_gen
])
#robot.rf_traj_gen, robot.lf_traj_gen])
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt)
robot.torque_ctrl = create_torque_controller(robot,
conf.joint_torque_controller,
conf.motor_params, dt)
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,
conf.motor_params, dt)
#robot.current_ctrl = create_current_controller(robot, conf.current_ctrl, conf.motor_params, dt);
connect_ctrl_manager(robot)
# create low-pass filter for computing joint velocities
robot.encoder_filter = create_chebi2_lp_filter_Wn_03_N_4(
'encoder_filter', dt, conf.motor_params.NJ)
plug(robot.encoders.sout, robot.encoder_filter.x)
#plug(robot.encoder_filter.dx, robot.current_ctrl.dq);
plug(robot.encoder_filter.dx, robot.torque_ctrl.jointsVelocities)
#plug(robot.encoder_filter.x_filtered, robot.base_estimator.joint_positions);
#plug(robot.encoder_filter.dx, robot.base_estimator.joint_velocities);
robot.ros = RosPublish('rosPublish')
robot.device.after.addDownsampledSignal('rosPublish.trigger', 1)
#robot.estimator_ft.dgyro.value = (0.0, 0.0, 0.0);
#robot.estimator_ft.gyro.value = (0.0, 0.0, 0.0);
# estimator.accelerometer.value = (0.0, 0.0, 9.81);
if (startSoT):
print "Gonna start SoT"
sleep(1.0)
start_sot()
if (go_half_sitting):
print "Gonna go to half sitting in 1 sec"
sleep(1.0)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 10.0)
return robot