Skip to content

Commit

Permalink
Merge branch 'main' into ahcorde/plugin/VisualizationCapabilities
Browse files Browse the repository at this point in the history
  • Loading branch information
@ahcorde
ahcorde authored Sep 14, 2021
2 parents 616551e + b965575 commit 74b647e
Show file tree
Hide file tree
Showing 4 changed files with 305 additions and 2 deletions.
66 changes: 65 additions & 1 deletion src/systems/joint_position_controller/JointPositionController.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <ignition/transport/Node.hh>

#include "ignition/gazebo/components/JointForceCmd.hh"
#include "ignition/gazebo/components/JointVelocityCmd.hh"
#include "ignition/gazebo/components/JointPosition.hh"
#include "ignition/gazebo/Model.hh"

Expand Down Expand Up @@ -63,6 +64,18 @@ class ignition::gazebo::systems::JointPositionControllerPrivate

/// \brief Joint index to be used.
public: unsigned int jointIndex = 0u;

/// \brief Operation modes
enum OperationMode {
/// \brief Use PID to achieve positional control
PID,
/// \brief Bypass PID completely. This means the joint will move to that
/// position bypassing the physics engine.
ABSOLUTE
};

/// \brief Joint position mode
public: OperationMode mode = OperationMode::PID;
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -143,6 +156,15 @@ void JointPositionController::Configure(const Entity &_entity,
{
cmdOffset = _sdf->Get<double>("cmd_offset");
}
if (_sdf->HasElement("use_velocity_commands"))
{
auto useVelocityCommands = _sdf->Get<bool>("use_velocity_commands");
if (useVelocityCommands)
{
this->dataPtr->mode =
JointPositionControllerPrivate::OperationMode::ABSOLUTE;
}
}

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

Expand Down Expand Up @@ -240,14 +262,56 @@ void JointPositionController::PreUpdate(
return;
}

// Update force command.
// Get error in position
double error;
{
std::lock_guard<std::mutex> lock(this->dataPtr->jointCmdMutex);
error = jointPosComp->Data().at(this->dataPtr->jointIndex) -
this->dataPtr->jointPosCmd;
}

// Check if the mode is ABSOLUTE
if (this->dataPtr->mode ==
JointPositionControllerPrivate::OperationMode::ABSOLUTE)
{
// Calculate target velcity
double targetVel = 0;

// Get time in seconds
auto dt = std::chrono::duration<double>(_info.dt).count();

// Get the maximum amount in m that this joint may move
auto maxMovement = this->dataPtr->posPid.CmdMax() * dt;

// Limit the maximum change to maxMovement
if (abs(error) > maxMovement)
{
targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() :
-this->dataPtr->posPid.CmdMax();
}
else
{
targetVel = -error;
}

// Set velocity and return
auto vel =
_ecm.Component<components::JointVelocityCmd>(this->dataPtr->jointEntity);

if (vel == nullptr)
{
_ecm.CreateComponent(
this->dataPtr->jointEntity,
components::JointVelocityCmd({targetVel}));
}
else if (!vel->Data().empty())
{
vel->Data()[0] = targetVel;
}
return;
}

// Update force command.
double force = this->dataPtr->posPid.Update(error, _info.dt);

auto forceComp =
Expand Down
4 changes: 4 additions & 0 deletions src/systems/joint_position_controller/JointPositionController.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,10 @@ namespace systems
///
/// `<cmd_offset>` Command offset (feed-forward) of the PID. Optional
/// parameter. The default value is 0.
///
/// `<use_velocity_commands>` Bypasses the PID and creates a perfect
/// position. The maximum speed on the joint can be set using the `<cmd_max>`
/// tag.
class JointPositionController
: public System,
public ISystemConfigure,
Expand Down
78 changes: 77 additions & 1 deletion test/integration/joint_position_controller_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ class JointPositionControllerTestFixture

/////////////////////////////////////////////////
// Tests that the JointPositionController accepts joint position commands
TEST_F(JointPositionControllerTestFixture, JointPositionCommand)
TEST_F(JointPositionControllerTestFixture, JointPositionForceCommand)
{
using namespace std::chrono_literals;

Expand Down Expand Up @@ -120,3 +120,79 @@ TEST_F(JointPositionControllerTestFixture, JointPositionCommand)

EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
}

/////////////////////////////////////////////////
// Tests that the JointPositionController accepts joint position commands
TEST_F(JointPositionControllerTestFixture, JointPositonVelocityCommand)
{
using namespace std::chrono_literals;

// Start server
ServerConfig serverConfig;
const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/joint_position_controller_velocity.sdf";
serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

server.SetUpdatePeriod(0ns);

const std::string jointName = "j1";

test::Relay testSystem;
std::vector<double> currentPosition;
testSystem.OnPreUpdate(
[&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
{
auto joint = _ecm.EntityByComponents(components::Joint(),
components::Name(jointName));
// Create a JointPosition component if it doesn't exist. This signals
// physics system to populate the component
if (nullptr == _ecm.Component<components::JointPosition>(joint))
{
_ecm.CreateComponent(joint, components::JointPosition());
}
});

testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &,
const gazebo::EntityComponentManager &_ecm)
{
_ecm.Each<components::Joint, components::Name,
components::JointPosition>(
[&](const ignition::gazebo::Entity &,
const components::Joint *,
const components::Name *_name,
const components::JointPosition *_position) -> bool
{
EXPECT_EQ(_name->Data(), jointName);
currentPosition = _position->Data();
return true;
});
});

server.AddSystem(testSystem.systemPtr);

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

// Publish command and check that the joint position is set
transport::Node node;
auto pub = node.Advertise<msgs::Double>(
"/model/joint_position_controller_test/joint/j1/0/cmd_pos");

const double targetPosition{2.0};
msgs::Double msg;
msg.set_data(targetPosition);

pub.Publish(msg);
// Wait for the message to be published
std::this_thread::sleep_for(100ms);

const std::size_t testIters = 1000;
server.Run(true, testIters , false);

EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL);
}
159 changes: 159 additions & 0 deletions test/worlds/joint_position_controller_velocity.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,159 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<gui fullscreen="0">

<!-- 3D scene -->
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="z">0</property>
<anchor line="right" target="window" target_line="right"/>
<anchor line="left" target="window" target_line="left"/>
<anchor line="top" target="window" target_line="top"/>
<anchor line="bottom" target="window" target_line="bottom"/>
</ignition-gui>

<engine>ogre</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-1 0 1 0 0.5 0</camera_pose>
</plugin>

<!-- World control -->
<plugin filename="WorldControl" name="World control">
<ignition-gui>
<title>World control</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">72</property>
<property type="double" key="width">121</property>
<property type="double" key="z">1</property>
<anchor line="left" target="window" target_line="left"/>
<anchor line="bottom" target="window" target_line="bottom"/>
</ignition-gui>

<play_pause>true</play_pause>
<step>true</step>
<start_paused>true</start_paused>

</plugin>
</gui>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="joint_position_controller_test">
<pose>0 0 0.005 0 0 0</pose>
<link name="base_link">
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertial>
<inertia>
<ixx>2.501</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2.501</iyy>
<iyz>0</iyz>
<izz>5</izz>
</inertia>
<mass>120.0</mass>
</inertial>
<visual name="base_visual">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.5 0.5 0.01</size>
</box>
</geometry>
</visual>
<collision name="base_collision">
<pose>0.0 0.0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.5 0.5 0.01</size>
</box>
</geometry>
</collision>
</link>
<link name="rotor">
<pose>0.0 0.0 1.0 0.0 0 0</pose>
<inertial>
<pose>0.0 0.0 0.0 0 0 0</pose>
<inertia>
<ixx>0.032</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.032</iyy>
<iyz>0</iyz>
<izz>0.00012</izz>
</inertia>
<mass>0.6</mass>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.25 0.25 0.05</size>
</box>
</geometry>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.25 0.25 0.05</size>
</box>
</geometry>
</collision>
</link>

<joint name="j1" type="revolute">
<pose>0 0 -0.5 0 0 0</pose>
<parent>base_link</parent>
<child>rotor</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<plugin
filename="ignition-gazebo-joint-position-controller-system"
name="ignition::gazebo::systems::JointPositionController">
<joint_name>j1</joint_name>
<use_velocity_commands>true</use_velocity_commands>
<cmd_max>1000</cmd_max>
</plugin>
</model>
</world>
</sdf>

0 comments on commit 74b647e

Please sign in to comment.