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') ...@@ -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.com_traj_gen.ddx, ctrl.com_ref_acc);
plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot); 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 # rather than giving to the controller the values of gear ratios and rotor inertias
......
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
from dynamic_graph import plug from dynamic_graph import plug
from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator 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_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_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 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): ...@@ -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.traj_gen = create_trajectory_generator(robot.device, dt);
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, 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.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("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.traj_sync = create_trajectory_switch();
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf"); robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf");
...@@ -81,8 +81,8 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None): ...@@ -81,8 +81,8 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
connect_synchronous_trajectories(robot.traj_sync, connect_synchronous_trajectories(robot.traj_sync,
[robot.com_traj_gen, [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); 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.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt);
......
...@@ -36,7 +36,7 @@ def get_sim_conf(): ...@@ -36,7 +36,7 @@ def get_sim_conf():
conf.motor_params = motor_params; conf.motor_params = motor_params;
return conf; 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 # BUILD THE STANDARD GRAPH
conf = get_sim_conf(); conf = get_sim_conf();
robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=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 ...@@ -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.q.sout, robot.inv_dyn.q);
plug(robot.v.sout, robot.inv_dyn.v); plug(robot.v.sout, robot.inv_dyn.v);
if(start_sot): if(startSoT):
start_sot(); start_sot();
# RESET FORCE/TORQUE SENSOR OFFSET # RESET FORCE/TORQUE SENSOR OFFSET
sleep(10*robot.timeStep); sleep(10*robot.timeStep);
......
...@@ -37,8 +37,13 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc'); ...@@ -37,8 +37,13 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc');
ent.ctrl_manager.setCtrlMode('all','torque') ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(0.0); 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() 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) sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5) robot.com_traj_gen.move(1, 0.05, 1.5)
sleep(2.0*7) sleep(2.0*7)
......
...@@ -400,6 +400,12 @@ namespace dynamicgraph ...@@ -400,6 +400,12 @@ namespace dynamicgraph
SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO); SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
// check current configuration is not too far from initial configuration // check current configuration is not too far from initial configuration
bool needToMoveToInitConf = false; 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); const VectorXd& xInit = (*m_splineTrajGen)(0.0);
assert(xInit.size() == m_n); assert(xInit.size() == m_n);
for(unsigned int i=0; i<m_n; i++) 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