Skip to content
Snippets Groups Projects
Commit f15791b6 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add Measured Force.

parent 55714ced
No related branches found
No related tags found
No related merge requests found
......@@ -36,11 +36,13 @@ class ControlJointValue {
double Cmd() const { return cmd_;}
double PosMes() const { return pos_mes_;}
double VelMes() const { return vel_mes_;}
double ForceMes() const { return force_mes_;}
double PosDes() const { return pos_des_;}
double VelDes() const { return vel_des_;}
void SetPosMes(double &pos_mes) { pos_mes_ = pos_mes;}
void SetVelMes(double &vel_mes) { vel_mes_ = vel_mes;}
void SetForceMes(double &force_mes) { force_mes_ = force_mes;}
void ComputeCmd();
......@@ -54,9 +56,9 @@ class ControlJointValue {
// Clamping i value to i_clamp
double i_clamp_;
// Measured quantities
double pos_mes_, vel_mes_;
double pos_mes_, vel_mes_, force_mes_;
/// Command
double cmd_;
......@@ -80,12 +82,13 @@ bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
const std::string &afilename);
/// Intermediate structure used to read information from Gazebo "joint_name/joint_state" topic.
///
///
class GZJointValues {
public:
/// Measured quantities
double pos_mes;
double vel_mes;
double force_mes;
/// Control
double force_ctrl;
};
......
......@@ -88,7 +88,8 @@ bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
for(auto it_joint_info = a_rbt_ctrl_joint_infos.begin();
it_joint_info != a_rbt_ctrl_joint_infos.end();
it_joint_info++)
ofs_position_robot << it_joint_info->second.Cmd() << " ";
ofs_position_robot << it_joint_info->second.Cmd() << " "
<< it_joint_info->second.ForceMes() << " ";
ofs_position_robot << std::endl;
ofs_position_robot.close();
return true;
......@@ -97,12 +98,12 @@ bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
const std::string &afilename)
{
std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
std::ofstream ofs_position_robot(afilename.c_str());
unsigned int idx=1;
for(auto it_joint_info = a_rbt_ctrl_joint_infos.begin();
it_joint_info != a_rbt_ctrl_joint_infos.end();
it_joint_info++)
ofs_position_robot << it_joint_info->first << " ";
ofs_position_robot << std::endl;
ofs_position_robot << idx++ << " - " << it_joint_info->first << " " << std::endl;
ofs_position_robot.close();
return true;
}
......@@ -167,6 +168,7 @@ void JointStateInterface::CallbackJointState(
gz_robot_joints_.dict_joint_values[jointItr->name()].pos_mes = axis1.position();
gz_robot_joints_.dict_joint_values[jointItr->name()].vel_mes = axis1.velocity();
gz_robot_joints_.dict_joint_values[jointItr->name()].force_mes = axis1.force();
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment