Commit 458d4031 authored by andreadelprete's avatar andreadelprete
Browse files

[common] Fix bug in conversion of velocities from SoT to urdf format (base...

[common] Fix bug in conversion of velocities from SoT to urdf format (base velocity is expressed in local frame in Pinocchio). Add output signals for foot vel/acc.
parent e09bc88c
......@@ -243,9 +243,11 @@ namespace dynamicgraph {
bool joints_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool velocity_urdf_to_sot(Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_urdf_to_sot(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_sot_to_urdf(Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
bool velocity_sot_to_urdf(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
bool config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
......
......@@ -169,6 +169,12 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
/// This signal copies active_joints only if it changes from a all false or to an all false value
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
......
......@@ -2,12 +2,7 @@ from dynamic_graph.sot.torque_control.tests.test_balance_ctrl_openhrp import *
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()
#wait 1s
robot.estimator_ft.setFTsensorOffsets((0.,)*24)
robot.ctrl_manager.setStreamPrintPeriod(0.5)
create_topic(robot.ros, robot.filters.estimator_kin.dx, 'dq_est');
create_topic(robot.ros, robot.dq.sout, 'dq');
......@@ -36,15 +31,20 @@ create_topic(robot.ros, robot.device.forceRLEG, 'forceRLEG');
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'wrenchLeftSole');
create_topic(robot.ros, robot.device.gyrometer, 'gyro');
create_topic(robot.ros, robot.device.accelerometer, 'acc');
create_topic(robot.ros, robot.base_estimator.w_lf, 'w_lf', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_rf, 'w_rf', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_lf_filtered, 'w_lf_filtered', data_type='double');
create_topic(robot.ros, robot.base_estimator.w_rf_filtered, 'w_rf_filtered', data_type='double');
#create_topic(robot.ros, robot.torque_ctrl.dq_motor, 'dq_motor');
#create_topic(robot.ros, robot.torque_ctrl.jointsAccelerationsDesired, 'ddq_des');
create_topic(robot.ros, robot.rf_traj_gen.x, 'rf_ref_traj_gen');
create_topic(robot.ros, robot.lf_traj_gen.x, 'lf_ref_traj_gen');
create_topic(robot.ros, robot.rf_traj_gen.x, 'rf_pose_ref');
create_topic(robot.ros, robot.lf_traj_gen.x, 'lf_pose_ref');
create_topic(robot.ros, robot.lf_traj_gen.dx, 'lf_vel_ref');
create_topic(robot.ros, robot.lf_traj_gen.ddx, 'lf_acc_ref');
create_topic(robot.ros, robot.inv_dyn.left_foot_pos, 'lf_pose');
create_topic(robot.ros, robot.inv_dyn.left_foot_vel, 'lf_vel');
create_topic(robot.ros, robot.inv_dyn.left_foot_acc, 'lf_acc');
create_topic(robot.ros, robot.inv_dyn.left_foot_acc_des, 'lf_acc_des');
#robot.torque_ctrl.velLeakingRate.value = 30*(0.,)
#robot.torque_ctrl.reset_ddq_integral();
......
......@@ -17,7 +17,7 @@
#include <sot/torque_control/common.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
//#include <pinocchio/algorithm/kinematics.hpp>
namespace dynamicgraph
{
......@@ -293,8 +293,10 @@ namespace dynamicgraph
}
bool RobotUtil::
velocity_urdf_to_sot(Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot)
velocity_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::ConstRefVector v_urdf,
Eigen::RefVector v_sot)
{
assert(q_urdf.size()==m_nbJoints+7);
assert(v_urdf.size()==m_nbJoints+6);
assert(v_sot.size()==m_nbJoints+6);
......@@ -303,16 +305,21 @@ namespace dynamicgraph
SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
return false;
}
v_sot.head<6>() = v_urdf.head<6>();
const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
Eigen::Matrix3d oRb = q.toRotationMatrix();
v_sot.head<3>() = oRb*v_urdf.head<3>();
v_sot.segment<3>(3) = oRb*v_urdf.segment<3>(3);
// v_sot.head<6>() = v_urdf.head<6>();
joints_urdf_to_sot(v_urdf.tail(m_nbJoints),
v_sot.tail(m_nbJoints));
return true;
}
bool RobotUtil::
velocity_sot_to_urdf(Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf)
velocity_sot_to_urdf(Eigen::ConstRefVector q_urdf, Eigen::ConstRefVector v_sot,
Eigen::RefVector v_urdf)
{
assert(q_urdf.size()==m_nbJoints+7);
assert(v_urdf.size()==m_nbJoints+6);
assert(v_sot.size()==m_nbJoints+6);
......@@ -321,7 +328,12 @@ namespace dynamicgraph
SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
return false;
}
v_urdf.head<6>() = v_sot.head<6>();
// compute rotation from world to base frame
const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
Eigen::Matrix3d oRb = q.toRotationMatrix();
v_urdf.head<3>() = oRb.transpose()*v_sot.head<3>();
v_urdf.segment<3>(3) = oRb.transpose()*v_sot.segment<3>(3);
// v_urdf.head<6>() = v_sot.head<6>();
joints_sot_to_urdf(v_sot.tail(m_nbJoints),
v_urdf.tail(m_nbJoints));
return true;
......
......@@ -226,6 +226,7 @@ void DeviceTorqueCtrl::setState( const dynamicgraph::Vector& q )
tsid::math::SE3ToVector(H_rf, s.pos);
m_contactRF->setReference(s);
SEND_MSG("Setting right foot reference to "+toString(H_rf), MSG_TYPE_DEBUG);
setVelocity(m_v_sot);
}
void DeviceTorqueCtrl::setVelocity( const dynamicgraph::Vector& v )
......@@ -238,7 +239,7 @@ void DeviceTorqueCtrl::setVelocity( const dynamicgraph::Vector& v )
}
Device::setVelocity(v);
m_v_sot = v;
m_robot_util->velocity_sot_to_urdf(m_v_sot, m_v);
m_robot_util->velocity_sot_to_urdf(m_q, m_v_sot, m_v);
}
void DeviceTorqueCtrl::setControlInputType(const std::string& cit)
......@@ -340,8 +341,8 @@ void DeviceTorqueCtrl::integrate( const double & dt )
m_v += dt*m_dv;
m_robot_util->config_urdf_to_sot(m_q, m_q_sot);
m_robot_util->velocity_urdf_to_sot(m_v, m_v_sot);
m_robot_util->velocity_urdf_to_sot(m_dv, m_dv_sot);
m_robot_util->velocity_urdf_to_sot(m_q, m_v, m_v_sot);
m_robot_util->velocity_urdf_to_sot(m_q, m_dv, m_dv_sot);
state_ = m_q_sot;
velocity_ = m_v_sot;
......
......@@ -140,6 +140,12 @@ namespace dynamicgraph
<< m_base_orientationSOUT \
<< m_right_foot_posSOUT \
<< m_left_foot_posSOUT \
<< m_right_foot_velSOUT \
<< m_left_foot_velSOUT \
<< m_right_foot_accSOUT \
<< m_left_foot_accSOUT \
<< m_right_foot_acc_desSOUT \
<< m_left_foot_acc_desSOUT \
<< m_dv_desSOUT \
<< m_MSOUT
......@@ -243,6 +249,12 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(base_orientation, 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(left_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_vel, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(left_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_OUT(right_foot_acc_des, dg::Vector, m_tau_desSOUT)
,CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, m_active_jointsSIN)
,m_initSucceeded(false)
,m_enabled(false)
......@@ -288,7 +300,12 @@ namespace dynamicgraph
if(m_contactState == DOUBLE_SUPPORT)
{
SEND_MSG("Remove right foot contact in "+toString(transitionTime)+" s", MSG_TYPE_INFO);
m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime);
bool res = m_invDyn->removeRigidContact(m_contactRF->name(), transitionTime);
if(!res)
{
const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
SEND_MSG("Error while remove right foot contact."+tsid::solvers::HQPDataToString(hqpData, false), MSG_TYPE_ERROR);
}
const double & w_feet = m_w_feetSIN.accessCopy();
m_invDyn->addMotionTask(*m_taskRF, w_feet, 1);
if(transitionTime>m_dt)
......@@ -306,7 +323,12 @@ namespace dynamicgraph
if(m_contactState == DOUBLE_SUPPORT)
{
SEND_MSG("Remove left foot contact in "+toString(transitionTime)+" s", MSG_TYPE_INFO);
m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime);
bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime);
if(!res)
{
const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
SEND_MSG("Error while remove right foot contact."+tsid::solvers::HQPDataToString(hqpData, false), MSG_TYPE_ERROR);
}
const double & w_feet = m_w_feetSIN.accessCopy();
m_invDyn->addMotionTask(*m_taskLF, w_feet, 1);
if(transitionTime>m_dt)
......@@ -640,7 +662,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_PREPARE_INV_DYN);
m_robot_util->config_sot_to_urdf(q_sot, m_q_urdf);
m_robot_util->velocity_sot_to_urdf(v_sot, m_v_urdf);
m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_sot, m_v_urdf);
m_sampleCom.pos = x_com_ref;
m_sampleCom.vel = dx_com_ref;
......@@ -745,7 +767,7 @@ namespace dynamicgraph
getStatistics().store("com ff ratio", ddx_com_ref.norm()/m_taskCom->getConstraint().vector().norm());
m_dv_urdf = m_invDyn->getAccelerations(sol);
m_robot_util->velocity_urdf_to_sot(m_dv_urdf, m_dv_sot);
m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot);
Eigen::Matrix<double,12,1> tmp;
if(m_invDyn->getContactForces(m_contactRF->name(), sol, tmp))
m_f_RF = m_contactRF->getForceGeneratorMatrix() * tmp;
......@@ -1140,9 +1162,9 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_pos before initialization!");
return s;
}
// m_tau_desSOUT(iter);
m_tau_desSOUT(iter);
se3::SE3 oMi;
s.resize(12);
s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi);
tsid::math::SE3ToVector(oMi, s);
return s;
......@@ -1155,14 +1177,100 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_pos before initialization!");
return s;
}
// m_tau_desSOUT(iter);
m_tau_desSOUT(iter);
se3::SE3 oMi;
s.resize(12);
s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi);
tsid::math::SE3ToVector(oMi, s);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_vel, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_vel before initialization!");
return s;
}
se3::Motion v;
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lf, v);
s = v.toVector();
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_vel, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_vel before initialization!");
return s;
}
se3::Motion v;
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rf, v);
s = v.toVector();
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_acc, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_acc before initialization!");
return s;
}
m_tau_desSOUT(iter);
if(m_contactState == RIGHT_SUPPORT)
s = m_taskLF->getAcceleration(m_dv_urdf);
else
s = m_contactLF->getMotionTask().getAcceleration(m_dv_urdf);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_acc before initialization!");
return s;
}
m_tau_desSOUT(iter);
if(m_contactState == LEFT_SUPPORT)
s = m_taskRF->getAcceleration(m_dv_urdf);
else
s = m_contactRF->getMotionTask().getAcceleration(m_dv_urdf);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(left_foot_acc_des, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_acc_des before initialization!");
return s;
}
m_tau_desSOUT(iter);
if(m_contactState == RIGHT_SUPPORT)
s = m_taskLF->getDesiredAcceleration();
else
s = m_contactLF->getMotionTask().getDesiredAcceleration();
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(right_foot_acc_des, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_acc_des before initialization!");
return s;
}
m_tau_desSOUT(iter);
if(m_contactState == LEFT_SUPPORT)
s = m_taskRF->getDesiredAcceleration();
else
s = m_contactRF->getMotionTask().getDesiredAcceleration();
return s;
}
/* --- COMMANDS ---------------------------------------------------------- */
......
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