Commit de8c9945 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Remove hard-coded references to the robot.

parent 7b0cdf56
......@@ -124,7 +124,7 @@ namespace dynamicgraph {
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
unsigned int m_nbJoints;
double m_Right_Foot_Sole_XYZ[3];
}; // class FreeFlyerLocator
} // namespace torque_control
......
......@@ -39,7 +39,6 @@
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/hrp2-common.hh>
#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
......@@ -232,7 +231,8 @@ namespace dynamicgraph {
pininvdyn::math::Vector m_v_urdf;
unsigned int m_timeLast;
unsigned int m_nbJoints;
}; // class InverseDynamicsBalanceController
} // namespace torque_control
} // namespace sot
......
......@@ -40,10 +40,8 @@
/* HELPER */
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/metapod-helper.hh>
#include <sot/torque_control/utils/stop-watch.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/hrp2-common.hh>
/*Motor model*/
#include <sot/torque_control/motor-model.hh>
......
......@@ -39,7 +39,6 @@
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/hrp2-common.hh>
#include <sot/torque_control/utils/trajectory-generators.hh>
#include <map>
#include <initializer_list>
......
......@@ -39,7 +39,6 @@
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/hrp2-common.hh>
#include <sot/torque_control/utils/trajectory-generators.hh>
#include <map>
#include <initializer_list>
......
......@@ -39,7 +39,6 @@
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/hrp2-common.hh>
#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
......
......@@ -90,7 +90,7 @@ namespace dynamicgraph
se3::urdf::buildModel(urdfFile,se3::JointModelFreeFlyer(),m_model);
m_nbJoints = m_model.nq - 7;
assert(m_model.nv == m_nbJoints+6);
assert(m_model.existFrame(m_Left_Foot_Frame_Name));
assert(m_model.existFrame(m_Right_Foot_Frame_Name));
......@@ -164,7 +164,7 @@ namespace dynamicgraph
m_Mff = se3::SE3(se3::exp3(w), 0.5 * (iMo1.translation()+iMo2.translation() ));
// due to distance from ankle to ground
Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&RIGHT_FOOT_SOLE_XYZ[0]);
Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&m_Right_Foot_Sole_XYZ[0]);
m_q_sot.tail(m_nbJoints) = q.tail(m_nbJoints);
base_se3_to_sot(m_Mff.translation()-righ_foot_sole_xyz,
......@@ -196,7 +196,7 @@ namespace dynamicgraph
freeflyer << m_Mff.translation(), aa.axis() * aa.angle();
// due to distance from ankle to ground
Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&RIGHT_FOOT_SOLE_XYZ[0]);
Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&m_Right_Foot_Sole_XYZ[0]);
freeflyer.head<3>() -= righ_foot_sole_xyz;
s = freeflyer;
......
......@@ -19,6 +19,7 @@
#include <dynamic-graph/factory.h>
#include <sot/torque_control/commands-helper.hh>
#include <pininvdyn/utils/stop-watch.hpp>
#include <pininvdyn/utils/statistics.hpp>
......@@ -312,6 +313,7 @@ namespace dynamicgraph
assert(kd_contact.size()==6);
assert(kp_com.size()==3);
assert(kd_com.size()==3);
assert(kp_feet.size()==6);
assert(kd_feet.size()==6);
assert(kp_posture.size()==N_JOINTS);
......@@ -332,10 +334,30 @@ namespace dynamicgraph
{
vector<string> package_dirs;
m_robot = new RobotWrapper(urdfFile, package_dirs, se3::JointModelFreeFlyer());
assert(m_robot->nv()>=6);
m_nbJoints = m_robot->nv()-6;
EIGEN_CONST_VECTOR_FROM_SIGNAL(kp_posture, m_kp_postureSIN(0));
assert(kp_posture.size()==m_nbJoints);
EIGEN_CONST_VECTOR_FROM_SIGNAL(kd_posture, m_kd_postureSIN(0));
assert(kd_posture.size()==m_nbJoints);
EIGEN_CONST_VECTOR_FROM_SIGNAL(rotor_inertias, m_rotor_inertiasSIN(0));
assert(rotor_inertias.size()==m_nbJoints);
EIGEN_CONST_VECTOR_FROM_SIGNAL(gear_ratios, m_gear_ratiosSIN(0));
assert(gear_ratios.size()==m_nbJoints);
m_robot->rotor_inertias(rotor_inertias);
m_robot->gear_ratios(gear_ratios);
assert(m_robot->nv()-6==N_JOINTS);
const double & w_com = m_w_comSIN(0);
const double & w_posture = m_w_postureSIN(0);
// const double & w_base_orientation = m_w_base_orientationSIN(0);
// const double & w_torques = m_w_torquesSIN(0);
const double & w_forces = m_w_forcesSIN(0);
const double & mu = m_muSIN(0);
const double & fMin = m_f_minSIN(0);
m_dv_sot.setZero(m_robot->nv());
m_tau_sot.setZero(m_robot->nv()-6);
m_f.setZero(24);
......@@ -406,33 +428,32 @@ namespace dynamicgraph
/** Copy active_joints only if a valid transition occurs. (From all OFF) or (To all OFF)**/
DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector)
{
if(s.size()!=N_JOINTS)
s.resize(N_JOINTS);
if(s.size()!=m_nbJoints)
s.resize(m_nbJoints);
const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
if (m_enabled == false)
{
if (active_joints_sot.any())
{
/* from all OFF to some ON */
m_enabled = true ;
s = active_joints_sot;
Eigen::VectorXd active_joints_urdf(N_JOINTS);
joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
m_taskBlockedJoints = new TaskJointPosture("task-posture", *m_robot);
Eigen::VectorXd blocked_joints(N_JOINTS);
for(unsigned int i=0; i<N_JOINTS; i++)
if(active_joints_urdf(i)==0.0)
blocked_joints(i) = 1.0;
else
blocked_joints(i) = 0.0;
SEND_MSG("Blocked joints: "+toString(blocked_joints.transpose()), MSG_TYPE_INFO);
m_taskBlockedJoints->mask(blocked_joints);
TrajectorySample ref_zero(N_JOINTS);
m_taskBlockedJoints->setReference(ref_zero);
m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
/* from all OFF to some ON */
m_enabled = true ;
EIGEN_VECTOR_TO_VECTOR(active_joints_sot, s);
Eigen::VectorXd active_joints_urdf(m_nbJoints);
joints_sot_to_urdf(active_joints_sot, active_joints_urdf);
m_taskBlockedJoints = new TaskJointPosture("task-posture", *m_robot);
Eigen::VectorXd blocked_joints(m_nbJoints);
for(unsigned int i=0; i<m_nbJoints; i++)
if(active_joints_urdf(i)==0.0)
blocked_joints(i) = 1.0;
else
blocked_joints(i) = 0.0;
SEND_MSG("Blocked joints: "+toString(blocked_joints.transpose()), MSG_TYPE_INFO);
m_taskBlockedJoints->mask(blocked_joints);
TrajectorySample ref_zero(m_nbJoints);
m_taskBlockedJoints->setReference(ref_zero);
m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
}
}
else if (!active_joints_sot.any())
......@@ -441,7 +462,7 @@ namespace dynamicgraph
m_enabled = false ;
}
if (m_enabled == false)
for(int i=0; i<N_JOINTS; i++)
for(int i=0; i<m_nbJoints; i++)
s(i)=false;
return s;
}
......@@ -453,8 +474,8 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal tau_des before initialization!");
return s;
}
if(s.size()!=N_JOINTS)
s.resize(N_JOINTS);
if(s.size()!=m_nbJoints)
s.resize(m_nbJoints);
getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
......@@ -470,23 +491,25 @@ namespace dynamicgraph
getProfiler().start(PROFILE_READ_INPUT_SIGNALS);
m_w_feetSIN(iter);
m_active_joints_checkedSINNER(iter);
const VectorN6& q_sot = m_qSIN(iter);
assert(q_sot.size()==N_JOINTS+6);
const VectorN6& v_sot = m_vSIN(iter);
assert(v_sot.size()==N_JOINTS+6);
const Vector3& x_com_ref = m_com_ref_posSIN(iter);
EIGEN_CONST_VECTOR_FROM_SIGNAL(q_sot, m_qSIN(iter));
assert(q_sot.size()==m_nbJoints+6);
EIGEN_CONST_VECTOR_FROM_SIGNAL(v_sot, m_vSIN(iter));
assert(v_sot.size()==m_nbJoints+6);
EIGEN_CONST_VECTOR_FROM_SIGNAL(x_com_ref, m_com_ref_posSIN(iter));
assert(x_com_ref.size()==3);
const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
assert(dx_com_ref.size()==3);
const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
assert(ddx_com_ref.size()==3);
const VectorN& q_ref = m_posture_ref_posSIN(iter);
assert(q_ref.size()==N_JOINTS);
const VectorN& dq_ref = m_posture_ref_velSIN(iter);
assert(dq_ref.size()==N_JOINTS);
const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
assert(ddq_ref.size()==N_JOINTS);
const Vector6& kp_contact = m_kp_constraintsSIN(iter);
EIGEN_CONST_VECTOR_FROM_SIGNAL(q_ref, m_posture_ref_posSIN(iter));
assert(q_ref.size()==m_nbJoints);
EIGEN_CONST_VECTOR_FROM_SIGNAL(dq_ref, m_posture_ref_velSIN(iter));
assert(dq_ref.size()==m_nbJoints);
EIGEN_CONST_VECTOR_FROM_SIGNAL(ddq_ref, m_posture_ref_accSIN(iter));
assert(ddq_ref.size()==m_nbJoints);
EIGEN_CONST_VECTOR_FROM_SIGNAL(kp_contact, m_kp_constraintsSIN(iter));
assert(kp_contact.size()==6);
const Vector6& kd_contact = m_kd_constraintsSIN(iter);
assert(kd_contact.size()==6);
......@@ -494,6 +517,7 @@ namespace dynamicgraph
assert(kp_com.size()==3);
const Vector3& kd_com = m_kd_comSIN(iter);
assert(kd_com.size()==3);
const VectorN& kp_posture = m_kp_postureSIN(iter);
assert(kp_posture.size()==N_JOINTS);
const VectorN& kd_posture = m_kd_postureSIN(iter);
......@@ -541,6 +565,7 @@ namespace dynamicgraph
m_taskLF->Kp(kp_feet);
m_taskLF->Kd(kd_feet);
}
getProfiler().stop(PROFILE_READ_INPUT_SIGNALS);
getProfiler().start(PROFILE_PREPARE_INV_DYN);
......@@ -634,8 +659,8 @@ namespace dynamicgraph
m_f_LF = m_contactLF->getForceGeneratorMatrix() * tmp;
joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot);
m_tau_sot += kp_pos.cwiseProduct(q_ref-q_sot.tail<N_JOINTS>()) +
kd_pos.cwiseProduct(dq_ref-v_sot.tail<N_JOINTS>());
m_tau_sot += kp_pos.cwiseProduct(q_ref-q_sot.tail(m_nbJoints)) +
kd_pos.cwiseProduct(dq_ref-v_sot.tail(m_nbJoints));
getProfiler().stop(PROFILE_TAU_DES_COMPUTATION);
m_t += m_dt;
......
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