Skip to content
GitLab
Commit 474a7931 authored by Noëlie Ramuzat's avatar Noëlie Ramuzat
Browse files

[tools] Change RobotUtil pointer to Share pointer 

Change the pointer RobotUtil to share pointer in the different classes, accordingly to the changes done in sot-core.
Refactoring of the code.
parent f44657a9
Hide whitespace changes
Inline Side-by-side
include/sot/torque_control/admittance-controller.hh
View file @ 474a7931
......@@ -146,7 +146,7 @@ namespace dynamicgraph {
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
......
include/sot/torque_control/base-estimator.hh
View file @ 474a7931
......@@ -181,7 +181,7 @@ namespace dynamicgraph {
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_reset_foot_pos; /// true after the command resetFootPositions is called
double m_dt; /// sampling time step
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
bool m_left_foot_is_stable; /// True if left foot as been stable for the last 'm_fz_stable_windows_size' samples
bool m_right_foot_is_stable; /// True if right foot as been stable for the last 'm_fz_stable_windows_size' samples
......
include/sot/torque_control/control-manager.hh
View file @ 474a7931
......@@ -88,9 +88,9 @@ namespace dynamicgraph {
/* --- CONSTRUCTOR ---- */
ControlManager( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
......@@ -112,26 +112,26 @@ namespace dynamicgraph {
/* --- COMMANDS --- */
/// Commands related to the control mode.
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId,
const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId,
const dynamicgraph::Vector &lq,
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId,
const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId,
const dynamicgraph::Vector &lq,
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
......@@ -143,8 +143,8 @@ namespace dynamicgraph {
/// Commands related to HandUtil
void setHandFrameName(const std::string &, const std::string &);
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
void setStreamPrintPeriod(const double & s);
void setSleepTime(const double &seconds);
......@@ -159,7 +159,7 @@ namespace dynamicgraph {
}
protected:
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
tsid::robots::RobotWrapper * m_robot;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
include/sot/torque_control/current-controller.hh
View file @ 474a7931
......@@ -119,7 +119,7 @@ namespace dynamicgraph {
}
protected:
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_emergency_stop_triggered;
double m_dt; /// control loop time period
......
include/sot/torque_control/device-torque-ctrl.hh
View file @ 474a7931
......@@ -176,7 +176,7 @@ namespace dynamicgraph
DIST normalDistribution_;
GEN normalRandomNumberGenerator_;
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
};
} // end namespace torque_control
......
include/sot/torque_control/free-flyer-locator.hh
View file @ 474a7931
......@@ -115,7 +115,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_v_pin; /// robot velocities according to pinocchio convention
Eigen::VectorXd m_v_sot; /// robot velocities according to SoT convention
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
}; // class FreeFlyerLocator
......
include/sot/torque_control/inverse-dynamics-balance-controller.hh
View file @ 474a7931
......@@ -308,7 +308,7 @@ namespace dynamicgraph {
tsid::math::Vector6 m_v_LF_int;
unsigned int m_timeLast;
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
}; // class InverseDynamicsBalanceController
} // namespace torque_control
......
include/sot/torque_control/joint-torque-controller.hh
View file @ 474a7931
......@@ -124,7 +124,7 @@ namespace dynamicgraph {
// Eigen::VectorXd m_dqDesIntegral; /// integral of the desired velocity
Eigen::VectorXd m_dqErrIntegral; /// integral of the velocity error
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
......
include/sot/torque_control/joint-trajectory-generator.hh
View file @ 474a7931
......@@ -182,7 +182,7 @@ namespace dynamicgraph {
bool m_firstIter; /// true if it is the first iteration, false otherwise
double m_dt; /// control loop time period
RobotUtil *m_robot_util;
RobotUtilShrPtr m_robot_util;
std::vector<int> m_iterForceSignals;
......
include/sot/torque_control/position-controller.hh
View file @ 474a7931
......@@ -93,7 +93,7 @@ namespace dynamicgraph {
}
protected:
RobotUtil * m_robot_util; /// Robot Util
RobotUtilShrPtr m_robot_util; /// Robot Util
Eigen::VectorXd m_pwmDes;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
include/sot/torque_control/torque-offset-estimator.hh
View file @ 474a7931
......@@ -94,7 +94,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector);
protected:
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
int n_iterations; //Number of iterations to consider
......
src/control-manager.cpp
View file @ 474a7931
......@@ -187,28 +187,28 @@ namespace dynamicgraph
void ControlManager::init(const double & dt,
const std::string & urdfFile,
const std::string &robotRef)
const std::string & robotRef)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_dt = dt;
m_emergency_stop_triggered = false;
m_emergency_stop_triggered = false;
m_initSucceeded = true;
vector<string> package_dirs;
m_robot = new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
std::string localName(robotRef);
if (!isNameInRobotUtil(localName))
{
m_robot_util = createRobotUtil(localName);
std::cout << "createRobotUtil success" << std::endl;
}
else
{
m_robot_util = getRobotUtil(localName);
std::cout << "getRobotUtil success" << std::endl;
}
std::cout << m_robot_util->m_urdf_filename << std::endl;
m_robot_util->m_urdf_filename = urdfFile;
addCommand("getJointsUrdfToSot",
makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
docDirectSetter("Display map Joints From URDF to SoT.",
......@@ -317,7 +317,7 @@ namespace dynamicgraph
for(std::size_t i=0;i<m_emergencyStopSIN.size();i++)
{
if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter))
if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter))
{
m_emergency_stop_triggered = true;
SEND_MSG("Emergency Stop has been triggered by an external entity", MSG_TYPE_ERROR);
......@@ -408,7 +408,7 @@ namespace dynamicgraph
CtrlMode cm;
if(convertStringToCtrlMode(ctrlMode,cm)==false)
return;
if(jointName=="all")
{
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
......@@ -527,7 +527,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot set joints limits from joint id before initialization!");
return;
}
m_robot_util->set_joint_limits_for_id((Index)jointId,lq,uq);
}
......@@ -600,7 +600,7 @@ namespace dynamicgraph
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
else if (FootName=="Right")
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
else
else
SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
}
......@@ -619,7 +619,7 @@ namespace dynamicgraph
else
SEND_WARNING_STREAM_MSG("Did not understand the hand name !" + HandName);
}
void ControlManager::setImuJointName(const std::string &JointName)
{
if(!m_initSucceeded)
......@@ -629,7 +629,7 @@ namespace dynamicgraph
}
m_robot_util->m_imu_joint_name = JointName;
}
void ControlManager::displayRobotUtil()
{
m_robot_util->display(std::cout);
......@@ -729,7 +729,7 @@ namespace dynamicgraph
}
catch (ExceptionSignal e) {}
}
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
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