Commit a06f758f authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[nd-trajectory-generator] add synchronous trajectories with force and com

parent 8af4e1f4
......@@ -312,7 +312,7 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
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.l_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
......
......@@ -6,7 +6,7 @@
from dynamic_graph import plug
from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_switch, connect_synchronous_trajectories
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_switch, connect_synchronous_trajectories, create_force_traj_gen
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_generator, create_com_traj_gen, create_encoders
from dynamic_graph.sot.torque_control.create_entities_utils import create_imu_offset_compensation, create_estimators, create_imu_filter
from dynamic_graph.sot.torque_control.create_entities_utils import create_base_estimator, create_position_controller, create_torque_controller
......@@ -64,8 +64,8 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
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.LF_FORCE_DES, dt);
robot.lf_force_traj_gen = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, 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");
......@@ -81,8 +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_traj_gen, robot.lf_traj_gen])
robot.rf_force_traj_gen, robot.lf_force_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);
......
......@@ -36,7 +36,7 @@ def get_sim_conf():
conf.motor_params = motor_params;
return conf;
def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, start_sot=True):
def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, startSoT=True):
# BUILD THE STANDARD GRAPH
conf = get_sim_conf();
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf);
......@@ -76,7 +76,7 @@ def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=Fals
plug(robot.q.sout, robot.inv_dyn.q);
plug(robot.v.sout, robot.inv_dyn.v);
if(start_sot):
if(startSoT):
start_sot();
# RESET FORCE/TORQUE SENSOR OFFSET
sleep(10*robot.timeStep);
......
......@@ -37,8 +37,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("/local/rbudhira/git/sot/sot-torque-control/data/traj_com_y_0.08/com_spline.curve", 1)
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()
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)
......
......@@ -400,6 +400,12 @@ namespace dynamicgraph
SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
// check current configuration is not too far from initial configuration
bool needToMoveToInitConf = false;
if(timeToInitConf == -1)
{
m_splineReady = true;
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
return;
}
const VectorXd& xInit = (*m_splineTrajGen)(0.0);
assert(xInit.size() == m_n);
for(unsigned int i=0; i<m_n; i++)
......
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