Commit 38f7bfc7 authored by andreadelprete's avatar andreadelprete
Browse files

Minor changes

parent 46905628
......@@ -3,7 +3,6 @@ np.set_printoptions(precision=3, suppress=True)
robot.timeStep = 0.002;
ent = test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=True);
robot.ctrl_manager.setStreamPrintPeriod(0.5);
start_sot()
create_topic(robot.ros, robot.filters.estimator_kin.dx, 'dq_est');
create_topic(robot.ros, robot.dq.sout, 'dq');
......@@ -20,6 +19,8 @@ create_topic(robot.ros, robot.device.robotVelocity, 'v');
create_topic(robot.ros, robot.base_estimator.v, 'v_est');
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'zmp_des');
create_topic(robot.ros, robot.inv_dyn.zmp, 'zmp');
create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'f_ref_right_foot');
create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'f_ref_left_foot');
create_topic(robot.ros, robot.inv_dyn.f_des_right_foot, 'f_des_right_foot');
create_topic(robot.ros, robot.inv_dyn.f_des_left_foot, 'f_des_left_foot');
create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'zmp_left_foot');
......@@ -37,13 +38,13 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc');
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(0.0);
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.turnOn()
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)
#robot.traj_sync.turnOn()
robot.traj_sync.out.value = 1
create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'right_foot_force_des');
create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'left_foot_force_des');
sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5)
sleep(2.0*7)
......
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