Skip to content

Commit

Permalink
[tools] Robot-utils
Browse files Browse the repository at this point in the history 
Fix minor warnings.
  • Loading branch information
@olivier-stasse
olivier-stasse committed Mar 17, 2019
1 parent 5e04e51 commit af35eb2
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 242 deletions.
231 changes: 1 addition & 230 deletions include/sot/core/robot-utils.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,7 @@ namespace dynamicgraph {
std::vector<Index> m_urdf_to_sot;

/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;
std::size_t m_nbJoints;

/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;
Expand Down Expand Up @@ -273,235 +273,6 @@ namespace dynamicgraph {
typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;

RobotUtilShrPtr RefVoidRobotUtil();
=======
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
upper(u),
lower(l)
{}

void display(std::ostream &os) const;
};


struct SOT_CORE_EXPORT ForceUtil
{
std::map<Index,ForceLimits> m_force_id_to_limits;
std::map<std::string,Index> m_name_to_force_id;
std::map<Index,std::string> m_force_id_to_name;

Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;

void set_name_to_force_id(const std::string & name,
const Index &force_id);


void set_force_id_to_limits(const Index &force_id,
const dg::Vector &lf,
const dg::Vector &uf);

void create_force_id_to_name_map();


Index get_id_from_name(const std::string &name);

const std::string & get_name_from_id(Index idx);
std::string cp_get_name_from_id(Index idx);

const ForceLimits & get_limits_from_id(Index force_id);
ForceLimits cp_get_limits_from_id(Index force_id);

Index get_force_id_left_hand()
{
return m_Force_Id_Left_Hand;
}

void set_force_id_left_hand(Index anId)
{
m_Force_Id_Left_Hand = anId;
}

Index get_force_id_right_hand()
{
return m_Force_Id_Right_Hand;
}

void set_force_id_right_hand( Index anId)
{
m_Force_Id_Right_Hand = anId;
}

Index get_force_id_left_foot()
{
return m_Force_Id_Left_Foot;
}

void set_force_id_left_foot(Index anId)
{
m_Force_Id_Left_Foot = anId;
}

Index get_force_id_right_foot()
{
return m_Force_Id_Right_Foot;
}

void set_force_id_right_foot( Index anId)
{
m_Force_Id_Right_Foot = anId;
}

void display(std::ostream & out) const;

}; // struct ForceUtil


struct SOT_CORE_EXPORT FootUtil
{
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream & os) const;
};



struct SOT_CORE_EXPORT HandUtil
{
std::string m_Left_Hand_Frame_Name;
std::string m_Right_Hand_Frame_Name;
void display(std::ostream & os) const;
};


struct SOT_CORE_EXPORT RobotUtil
{
public:

RobotUtil();

/// Forces data
ForceUtil m_force_util;

/// Foot information
FootUtil m_foot_util;

/// Hand information
HandUtil m_hand_util;

/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;

/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;

/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;

/// The map between id and name
std::map<Index,std::string> m_id_to_name;

/// The joint limits map.
std::map<Index,JointLimits> m_limits_map;

/// The name of the joint IMU is attached to
std::string m_imu_joint_name;

/// This method creates the map between id and name.
/// It is called each time a new link between id and name is inserted
/// (i.e. when set_name_to_id is called).
void create_id_to_name_map();

/// URDF file path
std::string m_urdf_filename;


dynamicgraph::Vector m_dgv_urdf_to_sot;

/** Given a joint name it finds the associated joint id.
* If the specified joint name is not found it returns -1;
* @param name Name of the joint to find.
* @return The id of the specified joint, -1 if not found. */
const Index get_id_from_name(const std::string &name);

/** Given a joint id it finds the associated joint name.
* If the specified joint is not found it returns "Joint name not found";
* @param id Id of the joint to find.
* @return The name of the specified joint, "Joint name not found" if not found. */

/// Get the joint name from its index
const std::string & get_name_from_id(Index id);

/// Set relation between the name and the SoT id
void set_name_to_id(const std::string &jointName,
const Index & jointId);

/// Set the map between urdf index and sot index
void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
void set_urdf_to_sot(const dg::Vector &urdf_to_sot);

/// Set the limits (lq,uq) for joint idx
void set_joint_limits_for_id(const Index &idx,
const double &lq,
const double &uq);

bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);

bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);

bool velocity_urdf_to_sot(ConstRefVector q_urdf,
ConstRefVector v_urdf, RefVector v_sot);

bool velocity_sot_to_urdf(ConstRefVector q_urdf,
ConstRefVector v_sot, RefVector v_urdf);

bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);

bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);


/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
* @param id Id of the joint to find.
* @return The limits of the specified joint, JointLimits(0,0) if not found. */
const JointLimits & get_joint_limits_from_id(Index id);
JointLimits cp_get_joint_limits_from_id(Index id);

/** \name Logger related methods */
/** \{*/
/// \brief Send messages \param msg with level t. Add string file and line to message.
void sendMsg(const std::string &msg,
MsgType t=MSG_TYPE_INFO,
const char *file="",
int line=0);

/// \brief Specify the verbosity level of the logger.
void setLoggerVerbosityLevel(LoggerVerbosity lv)
{logger_.setVerbosity(lv);}

/// \brief Get the logger's verbosity level.
LoggerVerbosity getLoggerVerbosityLevel()
{
return logger_.getVerbosity();
};

void display(std::ostream &os) const;


protected:

Logger logger_;

}; // struct RobotUtil

RobotUtil * RefVoidRobotUtil();
>>>>>>> [tools] Add HandUtil structure to Robot-utils

RobotUtilShrPtr getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtilShrPtr createRobotUtil(std::string &robotName);
Expand Down
24 changes: 12 additions & 12 deletions src/tools/robot-utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,8 +272,8 @@ namespace dynamicgraph
SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
return false;
}
assert(q_urdf.size() == m_nbJoints);
assert(q_sot.size() == m_nbJoints);
assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));

for (unsigned int idx = 0; idx < m_nbJoints; idx++)
q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
Expand Down Expand Up @@ -301,9 +301,9 @@ namespace dynamicgraph
velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
RefVector v_sot)
{
assert(q_urdf.size() == m_nbJoints + 7);
assert(v_urdf.size() == m_nbJoints + 6);
assert(v_sot.size() == m_nbJoints + 6);
assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));

if (m_nbJoints == 0)
{
Expand All @@ -324,9 +324,9 @@ namespace dynamicgraph
velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
RefVector v_urdf)
{
assert(q_urdf.size() == m_nbJoints + 7);
assert(v_urdf.size() == m_nbJoints + 6);
assert(v_sot.size() == m_nbJoints + 6);
assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));

if (m_nbJoints == 0)
{
Expand Down Expand Up @@ -387,8 +387,8 @@ namespace dynamicgraph
bool RobotUtil::
config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
{
assert(q_urdf.size() == m_nbJoints + 7);
assert(q_sot.size() == m_nbJoints + 6);
assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));

base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
Expand All @@ -399,8 +399,8 @@ namespace dynamicgraph
bool RobotUtil::
config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
{
assert(q_urdf.size() == m_nbJoints + 7);
assert(q_sot.size() == m_nbJoints + 6);
assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
joints_sot_to_urdf(q_sot.tail(m_nbJoints),
q_urdf.tail(m_nbJoints));
Expand Down

0 comments on commit af35eb2

Please sign in to comment.