Commit f44657a9 authored by Noëlie Ramuzat's avatar Noëlie Ramuzat
Browse files

[V2, ddp] Fix rebase duplication and se3 namespace

Fix some duplication of code during rebase of devel_ddp into devel.
Change the remaining se3 namespaces in pinocchio.
parent d49e0fa3
...@@ -300,7 +300,6 @@ namespace dynamicgraph ...@@ -300,7 +300,6 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT) ,CONSTRUCT_SIGNAL_OUT(com_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT) ,CONSTRUCT_SIGNAL_OUT(com_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT) ,CONSTRUCT_SIGNAL_OUT(base_orientation, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT) ,CONSTRUCT_SIGNAL_OUT(left_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT) ,CONSTRUCT_SIGNAL_OUT(right_foot_pos, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT) ,CONSTRUCT_SIGNAL_OUT(left_hand_pos, dg::Vector, m_tau_desSOUT)
...@@ -321,7 +320,6 @@ namespace dynamicgraph ...@@ -321,7 +320,6 @@ namespace dynamicgraph
,m_firstTime(true) ,m_firstTime(true)
,m_contactState(DOUBLE_SUPPORT) ,m_contactState(DOUBLE_SUPPORT)
,m_timeLast(0) ,m_timeLast(0)
,m_contactState(DOUBLE_SUPPORT)
,m_rightHandState(TASK_RIGHT_HAND_OFF) ,m_rightHandState(TASK_RIGHT_HAND_OFF)
,m_leftHandState(TASK_LEFT_HAND_OFF) ,m_leftHandState(TASK_LEFT_HAND_OFF)
,m_robot_util(RefVoidRobotUtil()) ,m_robot_util(RefVoidRobotUtil())
...@@ -1398,7 +1396,7 @@ namespace dynamicgraph ...@@ -1398,7 +1396,7 @@ namespace dynamicgraph
return s; return s;
} }
m_tau_desSOUT(iter); m_tau_desSOUT(iter);
se3::SE3 oMi; pinocchio::SE3 oMi;
s.resize(12); s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_lh, oMi); m_robot->framePosition(m_invDyn->data(), m_frame_id_lh, oMi);
tsid::math::SE3ToVector(oMi, s); tsid::math::SE3ToVector(oMi, s);
...@@ -1431,7 +1429,7 @@ namespace dynamicgraph ...@@ -1431,7 +1429,7 @@ namespace dynamicgraph
return s; return s;
} }
m_tau_desSOUT(iter); m_tau_desSOUT(iter);
se3::SE3 oMi; pinocchio::SE3 oMi;
s.resize(12); s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_rh, oMi); m_robot->framePosition(m_invDyn->data(), m_frame_id_rh, oMi);
tsid::math::SE3ToVector(oMi, s); tsid::math::SE3ToVector(oMi, s);
...@@ -1460,7 +1458,7 @@ namespace dynamicgraph ...@@ -1460,7 +1458,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal left_hand_vel before initialization!"); SEND_WARNING_STREAM_MSG("Cannot compute signal left_hand_vel before initialization!");
return s; return s;
} }
se3::Motion v; pinocchio::Motion v;
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lh, v); m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lh, v);
s = v.toVector(); s = v.toVector();
return s; return s;
...@@ -1487,7 +1485,7 @@ namespace dynamicgraph ...@@ -1487,7 +1485,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal right_hand_vel before initialization!"); SEND_WARNING_STREAM_MSG("Cannot compute signal right_hand_vel before initialization!");
return s; return s;
} }
se3::Motion v; pinocchio::Motion v;
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rh, v); m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rh, v);
s = v.toVector(); s = v.toVector();
return s; return s;
......
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