Commit e09bc88c authored by andreadelprete's avatar andreadelprete
Browse files

[se3-traj-gen] Fix important bug

parent 0db3bdd5
......@@ -23,6 +23,7 @@ create_topic(robot.ros, robot.traj_gen.q, 'q_ref');
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_ref, 'zmp_ref');
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');
......@@ -52,18 +53,12 @@ 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)
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))))
robot.traj_sync.out.value = 1 #turnOn()
sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5)
......
......@@ -204,12 +204,10 @@ namespace dynamicgraph
else if(m_status[0]==TG_SPLINE)
{
const Eigen::Vector3d& t= (*m_splineTrajGen)(m_t);
s.segment<3>(0) = m_splineRotation.row(0);
s.segment<3>(4) = m_splineRotation.row(1);
s.segment<3>(8) = m_splineRotation.row(2);
s[3] = t[0];
s[7] = t[1];
s[11]= t[2];
s.head<3>() = t;
s.segment<3>(3) = m_splineRotation.row(0);
s.segment<3>(6) = m_splineRotation.row(1);
s.segment<3>(9) = m_splineRotation.row(2);
}
else
for(unsigned int i=0; i<m_np; i++)
......@@ -240,12 +238,10 @@ namespace dynamicgraph
if(!m_splineTrajGen->checkRange(m_t))
{
const Eigen::Vector3d& t= (*m_splineTrajGen)(m_splineTrajGen->tmax());
s.segment<3>(0) = m_splineRotation.row(0);
s.segment<3>(4) = m_splineRotation.row(1);
s.segment<3>(8) = m_splineRotation.row(2);
s[3] = t[0];
s[7] = t[1];
s[11]= t[2];
s.head<3>() = t;
s.segment<3>(3) = m_splineRotation.row(0);
s.segment<3>(6) = m_splineRotation.row(1);
s.segment<3>(9) = m_splineRotation.row(2);
for(unsigned int i=0; i<m_np; i++)
{
m_noTrajGen[i]->set_initial_point(s(i));
......@@ -258,12 +254,10 @@ namespace dynamicgraph
else
{
const Eigen::Vector3d& t= (*m_splineTrajGen)(m_t);
s.segment<3>(0) = m_splineRotation.row(0);
s.segment<3>(4) = m_splineRotation.row(1);
s.segment<3>(8) = m_splineRotation.row(2);
s[3] = t[0];
s[7] = t[1];
s[11]= t[2];
s.head<3>() = t;
s.segment<3>(3) = m_splineRotation.row(0);
s.segment<3>(6) = m_splineRotation.row(1);
s.segment<3>(9) = m_splineRotation.row(2);
}
}
else
......@@ -428,12 +422,10 @@ namespace dynamicgraph
const VectorXd& t= (*m_splineTrajGen)(0.0);
VectorXd xInit(12);
xInit.segment<3>(0) = m_splineRotation.row(0);
xInit.segment<3>(4) = m_splineRotation.row(1);
xInit.segment<3>(8) = m_splineRotation.row(2);
xInit[3] = t[0];
xInit[7] = t[1];
xInit[11]= t[2];
xInit.segment<3>(3) = m_splineRotation.row(0);
xInit.segment<3>(6) = m_splineRotation.row(1);
xInit.segment<3>(9) = m_splineRotation.row(2);
xInit.head<3>() = t;
for(unsigned int i=0; i<m_np; i++)
if(fabs(xInit[i] - m_currentTrajGen[i]->getPos()(0)) > 0.001)
......
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