Commit bf7c3951 authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by andreadelprete
Browse files

[se3-trajectory-generator] add traj-gen for left foot and synchronize

parent e2b20d98
......@@ -81,7 +81,8 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
connect_synchronous_trajectories(robot.traj_sync,
[robot.com_traj_gen,
robot.rf_force_traj_gen, robot.lf_force_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);
......
......@@ -42,20 +42,28 @@ create_topic(robot.ros, robot.base_estimator.w_lf_filtered, 'w_lf_filtered', dat
create_topic(robot.ros, robot.base_estimator.w_rf_filtered, 'w_rf_filtered', data_type='double');
#create_topic(robot.ros, robot.torque_ctrl.dq_motor, 'dq_motor');
#create_topic(robot.ros, robot.torque_ctrl.jointsAccelerationsDesired, 'ddq_des');
create_topic(robot.ros, robot.rf_traj_gen.x, 'rf_ref_traj_gen');
create_topic(robot.ros, robot.lf_traj_gen.x, 'lf_ref_traj_gen');
#robot.torque_ctrl.velLeakingRate.value = 30*(0.,)
#robot.torque_ctrl.reset_ddq_integral();
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(1.0);
SOT_TORQUE_INSTALL_DIR = '/home/adelpret/devel/openrobots/src/sot-torque-control'
robot.com_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/com_spline.curve", 1)
robot.lf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_force.curve", 1)
robot.rf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/rf_force.curve", 1)
SOT_TORQUE_INSTALL_DIR = '/local/rbudhira/git/sot/sot-torque-control'
robot.com_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/com_spline.curve", -1)
robot.lf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_force.curve", -1)
robot.rf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/rf_force.curve", -1)
robot.traj_sync.out.value = 1
robot.lf_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_spline2.curve", -1, tuple(map(tuple, np.identity(3))))
robot.traj_sync.turnOn()
robot.com_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/com_spline.curve", 1)
robot.lf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_force.curve", 1)
robot.rf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/rf_force.curve", 1)
robot.lf_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_spline2.curve", 1, tuple(map(tuple, np.identity(3))))
sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment