Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add <initial_position> param to JointPositionController system #1406

Merged
merged 1 commit into from
Mar 23, 2022
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -46,13 +46,13 @@ class ignition::gazebo::systems::JointPositionControllerPrivate
public: transport::Node node;

/// \brief Joint Entity
public: Entity jointEntity;
public: Entity jointEntity{kNullEntity};

/// \brief Joint name
public: std::string jointName;

/// \brief Commanded joint position
public: double jointPosCmd;
public: double jointPosCmd{0.0};

/// \brief mutex to protect joint commands
public: std::mutex jointCmdMutex;
@@ -170,6 +170,12 @@ void JointPositionController::Configure(const Entity &_entity,

this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);


if (_sdf->HasElement("initial_position"))
{
this->dataPtr->jointPosCmd = _sdf->Get<double>("initial_position");
}

// Subscribe to commands
std::string topic = transport::TopicUtils::AsValidTopic("/model/" +
this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
@@ -206,6 +212,8 @@ void JointPositionController::Configure(const Entity &_entity,
igndbg << "cmd_min: [" << cmdMin << "]" << std::endl;
igndbg << "cmd_offset: [" << cmdOffset << "]" << std::endl;
igndbg << "Topic: [" << topic << "]" << std::endl;
igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]"
<< std::endl;
}

//////////////////////////////////////////////////
Original file line number Diff line number Diff line change
@@ -81,6 +81,9 @@ namespace systems
/// `<topic>` If you wish to listen on a non-default topic you may specify it
/// here, otherwise the controller defaults to listening on
/// "/model/<model_name>/joint/<joint_name>/<joint_index>/cmd_pos".
///
/// `<initial_position>` Initial position of a joint. Optional parameter.
/// The default value is 0.
class JointPositionController
: public System,
public ISystemConfigure,
9 changes: 8 additions & 1 deletion test/integration/joint_position_controller_system.cc
Original file line number Diff line number Diff line change
@@ -178,10 +178,17 @@ TEST_F(JointPositionControllerTestFixture,

server.AddSystem(testSystem.systemPtr);

const std::size_t initIters = 10;
// joint pos starts at 0
const std::size_t initIters = 1;
server.Run(true, initIters, false);
EXPECT_NEAR(0, currentPosition.at(0), TOL);

// joint moves to initial_position at -2.0
const std::size_t initPosIters = 1000;
server.Run(true, initPosIters, false);
double expectedInitialPosition = -2.0;
EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL);

// Publish command and check that the joint position is set
transport::Node node;
auto pub = node.Advertise<msgs::Double>(
1 change: 1 addition & 0 deletions test/worlds/joint_position_controller_velocity.sdf
Original file line number Diff line number Diff line change
@@ -112,6 +112,7 @@
<joint_name>j1</joint_name>
<use_velocity_commands>true</use_velocity_commands>
<cmd_max>1000</cmd_max>
<initial_position>-2.0</initial_position>
</plugin>
</model>
</world>