diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index ac6d4161..55b7ea4c 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -25,11 +25,12 @@ #include #include -#include #include #include -#include +#include +#include #include +#include #include #include #include @@ -46,6 +47,9 @@ struct jointData /// \brief Joint's names. std::string name; + /// \brief Joint's type. + sdf::JointType joint_type; + /// \brief Current joint position double joint_position; @@ -200,6 +204,7 @@ bool IgnitionSystem::initSim( ignition::gazebo::Entity simjoint = enableJoints[joint_name]; this->dataPtr->joints_[j].sim_joint = simjoint; + this->dataPtr->joints_[j].joint_type = _ecm.Component(simjoint)->Data(); // Create joint position component if one doesn't exist if (!_ecm.EntityHasComponentType( @@ -217,12 +222,12 @@ bool IgnitionSystem::initSim( _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointVelocity()); } - // Create joint force component if one doesn't exist + // Create joint transmitted wrench component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - ignition::gazebo::components::JointForce().TypeId())) + ignition::gazebo::components::JointTransmittedWrench().TypeId())) { - _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointTransmittedWrench()); } // Accept this joint and continue configuration @@ -502,11 +507,10 @@ hardware_interface::return_type IgnitionSystem::read( this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); - // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 - // Get the joint force - // const auto * jointForce = - // _ecm.Component( - // this->dataPtr->sim_joints_[j]); + // Get the joint force via joint transmitted wrench + const auto * jointWrench = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); // Get the joint position const auto * jointPositions = @@ -515,7 +519,11 @@ hardware_interface::return_type IgnitionSystem::read( this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; - // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; + if (this->dataPtr->joints_[i].joint_type == sdf::JointType::PRISMATIC) { + this->dataPtr->joints_[i].joint_effort = jointWrench->Data().force().z(); + } else { // REVOLUTE and CONTINUOUS + this->dataPtr->joints_[i].joint_effort = jointWrench->Data().torque().z(); + } } for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { @@ -705,8 +713,8 @@ hardware_interface::return_type IgnitionSystem::write( } } if (mimic_interface == "effort") { - // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 - // Get the joint force + // TODO(ahb): + // Get the joint force via joint transmitted wrench // const auto * jointForce = // _ecm.Component( // this->dataPtr->sim_joints_[j]);