Skip to content

Commit

Permalink
Populate JointConstraintWrench from physics
Browse files Browse the repository at this point in the history 
Signed-off-by: Addisu Z. Taddese <[email protected]>
  • Loading branch information
@azeey
azeey committed Aug 20, 2021
1 parent d468c05 commit 1eaf18c
Show file tree
Hide file tree
Showing 5 changed files with 329 additions and 1 deletion.
52 changes: 52 additions & 0 deletions include/ignition/gazebo/components/JointConstraintWrench.hh
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
/*
* Copyright (C) 2021 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONSTRAINTWRENCH_HH_
#define IGNITION_GAZEBO_COMPONENTS_JOINTCONSTRAINTWRENCH_HH_

#include <array>

#include <ignition/gazebo/components/Factory.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/components/Serialization.hh>
#include <ignition/gazebo/config.hh>

namespace ignition
{
namespace gazebo
{
// Inline bracket to help doxygen filtering.
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
namespace components
{
/// \brief Joint Constraint wrench in SI units (Nm for revolute, N for
/// prismatic). The first set of 3 values coorespond to the torque while the
/// second set of 3 correspond to the force. The wrench is expressed in the
/// Joint frame and the force component is applied at the joint origin.
/// \note The term Wrench is used here to mean a 6D vector formed by stacking
/// torque and force vectors. This is different from the Wrench used in screw
/// theory.
using JointConstraintWrench =
Component<std::array<double, 6>, class JointConstraintWrenchTag,
serializers::ArrayDoubleSerializer<6>>;
IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointConstraintWrench",
JointConstraintWrench)
}
}
}
}

#endif
38 changes: 38 additions & 0 deletions include/ignition/gazebo/components/Serialization.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <google/protobuf/message_lite.h>
#include <ignition/msgs/double_v.pb.h>

#include <array>
#include <string>
#include <vector>
#include <sdf/Sensor.hh>
Expand Down Expand Up @@ -131,6 +132,43 @@ namespace serializers
return _in;
}
};
/// \brief Serializer for components that hold `std::array<double, N>`.
template <std::size_t N>
class ArrayDoubleSerializer
{
/// \brief Serialization
/// \param[in] _out Output stream.
/// \param[in] _arr Array to stream
/// \return The stream.
public: static std::ostream &Serialize(std::ostream &_out,
const std::array<double, N> &_arr)
{
ignition::msgs::Double_V msg;
*msg.mutable_data() = {_arr.begin(), _arr.end()};
msg.SerializeToOstream(&_out);
return _out;
}

/// \brief Deserialization
/// \param[in] _in Input stream.
/// \param[in] _arr Array to populate
/// \return The stream.
public: static std::istream &Deserialize(std::istream &_in,
std::array<double, N> &_arr)
{
ignition::msgs::Double_V msg;
msg.ParseFromIstream(&_in);
if (static_cast<std::size_t>(msg.data_size()) > N)
{
ignerr << "Size of input array exceeds " << N << ". Cannot deserialize"
<< std::endl;
return _in;
}

std::copy(msg.data().begin(), msg.data().end(), _arr.begin());
return _in;
}
};

/// \brief Serializer for components that hold protobuf messages.
class MsgSerializer
Expand Down
43 changes: 42 additions & 1 deletion src/systems/physics/Physics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <ignition/msgs/entity.pb.h>
#include <ignition/msgs/Utility.hh>

#include <array>
#include <algorithm>
#include <iostream>
#include <deque>
Expand Down Expand Up @@ -109,6 +110,7 @@
#include "ignition/gazebo/components/ParentEntity.hh"
#include "ignition/gazebo/components/ParentLinkName.hh"
#include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh"
#include "ignition/gazebo/components/JointConstraintWrench.hh"
#include "ignition/gazebo/components/JointForceCmd.hh"
#include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
#include "ignition/gazebo/components/Pose.hh"
Expand Down Expand Up @@ -284,6 +286,12 @@ class ignition::gazebo::systems::PhysicsPrivate
physics::DetachJointFeature,
physics::SetJointTransformFromParentFeature>{};

//////////////////////////////////////////////////
// Joint constraint wrench
/// \brief Feature list for getting joint constraint wrenches.
public: struct JointGetConstraintWrenchFeatureList : physics::FeatureList<
physics::GetJointConstraintWrench>{};

//////////////////////////////////////////////////
// Collisions

Expand Down Expand Up @@ -397,7 +405,8 @@ class ignition::gazebo::systems::PhysicsPrivate
physics::Joint,
JointFeatureList,
DetachableJointFeatureList,
JointVelocityCommandFeatureList
JointVelocityCommandFeatureList,
JointGetConstraintWrenchFeatureList
>;

/// \brief A map between joint entity ids in the ECM to Joint Entities in
Expand Down Expand Up @@ -2189,6 +2198,38 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm)
return true;
});

// Update joint constraints
_ecm.Each<components::Joint, components::JointConstraintWrench>(
[&](const Entity &_entity, components::Joint *,
components::JointConstraintWrench *_wrench) -> bool
{
auto jointPhys =
this->entityJointMap
.EntityCast<JointGetConstraintWrenchFeatureList>(_entity);
if (jointPhys)
{
const auto &jointWrench = jointPhys->GetConstraintWrench();
std::copy(jointWrench.data(), jointWrench.data() + jointWrench.size(),
std::begin(_wrench->Data()));
_ecm.SetChanged(_entity, components::JointConstraintWrench::typeId,
ComponentState::PeriodicChange);
}
else
{
static bool informed{false};
if (!informed)
{
igndbg
<< "Attempting to get joint constraint wrenches, but the "
"physics engine doesn't support this feature. Values in the "
"JointConstraintWrench component will not be meaningful."
<< std::endl;
informed = true;
}
}
return true;
});

// TODO(louise) Skip this if there are no collision features
this->UpdateCollisions(_ecm);
}
Expand Down
60 changes: 60 additions & 0 deletions test/integration/physics_system.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include "ignition/gazebo/components/Geometry.hh"
#include "ignition/gazebo/components/Inertial.hh"
#include "ignition/gazebo/components/Joint.hh"
#include "ignition/gazebo/components/JointConstraintWrench.hh"
#include "ignition/gazebo/components/JointPosition.hh"
#include "ignition/gazebo/components/JointPositionReset.hh"
#include "ignition/gazebo/components/JointVelocity.hh"
Expand Down Expand Up @@ -981,3 +982,62 @@ TEST_F(PhysicsSystemFixture, NestedModel)
EXPECT_NE(parents.end(), parentIt);
EXPECT_EQ("model_01", parentIt->second);
}

/////////////////////////////////////////////////
// Joint force
TEST_F(PhysicsSystemFixture, JointConstraintWrench)
{
common::Console::SetVerbosity(4);
ignition::gazebo::ServerConfig serverConfig;

const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/joint_constraints.sdf";

serverConfig.SetSdfFile(sdfFile);

gazebo::Server server(serverConfig);

server.SetUpdatePeriod(1us);

// Create a system that records the poses of the links after physics
test::Relay testSystem;

testSystem.OnPreUpdate(
[&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm)
{
if (_info.iterations == 1)
{
_ecm.Each<components::Joint>(
[&](const ignition::gazebo::Entity &_entity,
const components::Joint *) -> bool
{
_ecm.CreateComponent(_entity,
components::JointConstraintWrench());
return true;
});
}
});

testSystem.OnPostUpdate(
[&](const gazebo::UpdateInfo &_info,
const gazebo::EntityComponentManager &_ecm)
{
_ecm.Each<components::Name, components::JointConstraintWrench>(
[&](const ignition::gazebo::Entity &, const components::Name *_name,
const components::JointConstraintWrench *_force) -> bool
{
std::cout << "Joint: " << _name->Data() << " "
<< _force->Data().size() << " " << _info.iterations
<< ": ";
for (const auto &jf : _force->Data())
{
std::cout << jf << " ";
}
std::cout << std::endl;
return true;
});
});
server.AddSystem(testSystem.systemPtr);
server.Run(true, 1800, false);
SUCCEED();
}
137 changes: 137 additions & 0 deletions test/worlds/joint_constraints.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="joint_constraints">
<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>
<emissive>0.8 0.8 0.8 1</emissive>
</material>
</visual>
</link>
</model>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="scale">
<pose>0 0 0.02 0 0 0</pose>
<link name="base">
<inertial>
<mass>2.0</mass>
<inertia>
<ixx>0.166933</ixx>
<iyy>0.166933</iyy>
<izz>0.333333</izz>
</inertia>
</inertial>
<visual name="v1">
<geometry>
<box>
<size>1 1 0.04</size>
</box>
</geometry>
</visual>
<collision name="c1">
<geometry>
<box>
<size>1 1 0.04</size>
</box>
</geometry>
</collision>
</link>
<link name="sensor_plate">
<pose relative_to="base">0 0 0.03 0 0 0</pose>
<inertial>
<mass>0.2</mass>
<inertia>
<ixx>0.0166933</ixx>
<iyy>0.0166933</iyy>
<izz>0.0333333</izz>
</inertia>
</inertial>
<visual name="v1">
<geometry>
<box>
<size>0.9 0.9 0.01</size>
</box>
</geometry>
<material>
<ambient>0.8 0.2 0.2 1</ambient>
<diffuse>0.8 0.2 0.2 1</diffuse>
<specular>0.8 0.2 0.2 1</specular>
</material>
</visual>
<collision name="c1">
<geometry>
<box>
<size>0.9 0.9 0.01</size>
</box>
</geometry>
</collision>
</link>

<joint name="sensor_joint" type="fixed">
<parent>base</parent>
<child>sensor_plate</child>
</joint>
</model>

<model name="weight">
<pose>0 0 0.5 0 0 0</pose>
<link name="base">
<inertial>
<mass>10</mass>
</inertial>
<visual name="v1">
<geometry>
<box>
<size>0.5 0.5 0.5</size>
</box>
</geometry>
<material>
<ambient>0.0 0.8 0.8 1</ambient>
<diffuse>0.0 0.8 0.8 1</diffuse>
<specular>0.0 0.8 0.8 1</specular>
</material>
</visual>
<collision name="c1">
<geometry>
<box>
<size>0.5 0.5 0.5</size>
</box>
</geometry>
</collision>
</link>
</model>

</world>
</sdf>

0 comments on commit 1eaf18c

Please sign in to comment.