Update link / base velocities and accelerations

cpp/scenario/gazebo/include/scenario/gazebo/Model.h

@@ -27,6 +27,8 @@
 #ifndef SCENARIO_GAZEBO_MODEL_H
 #define SCENARIO_GAZEBO_MODEL_H
 
+#include "scenario/gazebo/Joint.h"
+
 #include <ignition/gazebo/Entity.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
 #include <ignition/gazebo/EventManager.hh>
@@ -39,11 +41,9 @@
 namespace scenario {
     namespace base {
         struct ContactData;
-        enum class JointControlMode;
     } // namespace base
     namespace gazebo {
         class Link;
-        class Joint;
         class Model;
         using LinkPtr = std::shared_ptr<Link>;
         using JointPtr = std::shared_ptr<Joint>;
@@ -125,6 +125,9 @@ class scenario::gazebo::Model
     std::vector<double> jointVelocities( //
         const std::vector<std::string>& jointNames = {}) const;
 
+    base::JointLimit jointLimits( //
+        const std::vector<std::string>& jointNames = {}) const;
+
     bool setJointControlMode(const base::JointControlMode mode,
                              const std::vector<std::string>& jointNames = {});
 
@@ -178,15 +181,18 @@ class scenario::gazebo::Model
 
     std::array<double, 3> basePosition() const;
     std::array<double, 4> baseOrientation() const;
-    std::array<double, 3> baseLinearVelocity() const;
-    std::array<double, 3> baseAngularVelocity() const;
-
-    bool
-    resetBaseLinearVelocity(const std::array<double, 3>& linear = {0, 0, 0});
-    bool
-    resetBaseAngularVelocity(const std::array<double, 3>& angular = {0, 0, 0});
-    bool resetBaseVelocity(const std::array<double, 3>& linear = {0, 0, 0},
-                           const std::array<double, 3>& angular = {0, 0, 0});
+    std::array<double, 3> baseBodyLinearVelocity() const;
+    std::array<double, 3> baseBodyAngularVelocity() const;
+    std::array<double, 3> baseWorldLinearVelocity() const;
+    std::array<double, 3> baseWorldAngularVelocity() const;
+
+    bool resetBaseWorldLinearVelocity(
+        const std::array<double, 3>& linear = {0, 0, 0});
+    bool resetBaseWorldAngularVelocity(
+        const std::array<double, 3>& angular = {0, 0, 0});
+    bool resetBaseWorldVelocity( //
+        const std::array<double, 3>& linear = {0, 0, 0},
+        const std::array<double, 3>& angular = {0, 0, 0});
 
     bool resetBasePose(const std::array<double, 3>& position = {0, 0, 0},
                        const std::array<double, 4>& orientation = {0, 0, 0, 0});
@@ -203,19 +209,22 @@ class scenario::gazebo::Model
     bool setBasePositionTarget(const std::array<double, 3>& position);
     bool setBaseOrientationTarget(const std::array<double, 4>& orientation);
 
-    bool setBaseVelocityTarget(const std::array<double, 3>& linear,
-                               const std::array<double, 3>& angular);
-    bool setBaseLinearVelocityTarget(const std::array<double, 3>& linear);
-    bool setBaseAngularVelocityTarget(const std::array<double, 3>& angular);
-    bool setBaseLinearAccelerationTarget(const std::array<double, 3>& linear);
-    bool setBaseAngularAccelerationTarget(const std::array<double, 3>& angular);
+    bool setBaseWorldVelocityTarget(const std::array<double, 3>& linear,
+                                    const std::array<double, 3>& angular);
+    bool setBaseWorldLinearVelocityTarget(const std::array<double, 3>& linear);
+    bool setBaseWorldAngularVelocityTarget( //
+        const std::array<double, 3>& angular);
+    bool setBaseWorldLinearAccelerationTarget( //
+        const std::array<double, 3>& linear);
+    bool setBaseWorldAngularAccelerationTarget( //
+        const std::array<double, 3>& angular);
 
     std::array<double, 3> basePositionTarget() const;
     std::array<double, 4> baseOrientationTarget() const;
-    std::array<double, 3> baseLinearVelocityTarget() const;
-    std::array<double, 3> baseAngularVelocityTarget() const;
-    std::array<double, 3> baseLinearAccelerationTarget() const;
-    std::array<double, 3> baseAngularAccelerationTarget() const;
+    std::array<double, 3> baseWorldLinearVelocityTarget() const;
+    std::array<double, 3> baseWorldAngularVelocityTarget() const;
+    std::array<double, 3> baseWorldLinearAccelerationTarget() const;
+    std::array<double, 3> baseWorldAngularAccelerationTarget() const;
 
 private:
     class Impl;

cpp/scenario/gazebo/src/Link.cpp

@@ -255,17 +255,17 @@ std::array<double, 3> Link::worldAngularVelocity() const
 std::array<double, 3> Link::bodyLinearVelocity() const
 {
     auto linkBodyLinVel = utils::getComponentData< //
-        ignition::gazebo::components::AngularAcceleration>(pImpl->ecm,
-                                                           pImpl->linkEntity);
+        ignition::gazebo::components::LinearVelocity>(pImpl->ecm,
+                                                      pImpl->linkEntity);
 
     return utils::fromIgnitionVector(linkBodyLinVel);
 }
 
 std::array<double, 3> Link::bodyAngularVelocity() const
 {
     auto linkBodyAngVel = utils::getComponentData< //
-        ignition::gazebo::components::AngularAcceleration>(pImpl->ecm,
-                                                           pImpl->linkEntity);
+        ignition::gazebo::components::AngularVelocity>(pImpl->ecm,
+                                                       pImpl->linkEntity);
 
     return utils::fromIgnitionVector(linkBodyAngVel);
 }

cpp/scenario/gazebo/src/Model.cpp

@@ -525,6 +525,27 @@ Model::jointVelocities(const std::vector<std::string>& jointNames) const
     return Impl::getJointDataSerialized(this, jointNames, lambda);
 }
 
+scenario::base::JointLimit
+Model::jointLimits(const std::vector<std::string>& jointNames) const
+{
+    const std::vector<std::string> jointSerialization =
+        jointNames.empty() ? this->jointNames() : jointNames;
+
+    std::vector<double> low;
+    std::vector<double> high;
+
+    low.reserve(jointSerialization.size());
+    high.reserve(jointSerialization.size());
+
+    for (const auto& joint : this->joints(jointSerialization)) {
+        auto limit = joint->jointPositionLimit();
+        std::move(limit.min.begin(), limit.min.end(), std::back_inserter(low));
+        std::move(limit.max.begin(), limit.max.end(), std::back_inserter(high));
+    }
+
+    return base::JointLimit(low, high);
+}
+
 bool Model::setJointControlMode(const scenario::base::JointControlMode mode,
                                 const std::vector<std::string>& jointNames)
 {
@@ -753,7 +774,33 @@ std::array<double, 4> Model::baseOrientation() const
     return utils::fromIgnitionPose(world_H_model).orientation;
 }
 
-std::array<double, 3> Model::baseLinearVelocity() const
+std::array<double, 3> Model::baseBodyLinearVelocity() const
+{
+    auto baseWorldLinearVelocity =
+        utils::toIgnitionVector3(this->baseWorldLinearVelocity());
+
+    // Get the model pose
+    ignition::math::Pose3d world_H_model = utils::getExistingComponentData< //
+        ignition::gazebo::components::Pose>(pImpl->ecm, pImpl->modelEntity);
+
+    return utils::fromIgnitionVector( //
+        world_H_model.Inverse().Rot() * baseWorldLinearVelocity);
+}
+
+std::array<double, 3> Model::baseBodyAngularVelocity() const
+{
+    auto baseWorldAngularVelocity =
+        utils::toIgnitionVector3(this->baseWorldAngularVelocity());
+
+    // Get the model pose
+    ignition::math::Pose3d world_H_model = utils::getExistingComponentData< //
+        ignition::gazebo::components::Pose>(pImpl->ecm, pImpl->modelEntity);
+
+    return utils::fromIgnitionVector( //
+        world_H_model.Inverse().Rot() * baseWorldAngularVelocity);
+}
+
+std::array<double, 3> Model::baseWorldLinearVelocity() const
 {
     // Get the entity of the canonical link
     auto canonicalLinkEntity = pImpl->ecm->EntityByComponents(
@@ -792,7 +839,7 @@ std::array<double, 3> Model::baseLinearVelocity() const
     return utils::fromIgnitionVector(modelLinearVelocity);
 }
 
-std::array<double, 3> Model::baseAngularVelocity() const
+std::array<double, 3> Model::baseWorldAngularVelocity() const
 {
     // We could use the helper to convert the base link velocity to the model
     // mixed velocity. However, since there's only a rigid transformation
@@ -853,18 +900,20 @@ bool Model::resetBaseOrientation(const std::array<double, 4>& orientation)
     return this->resetBasePose(this->basePosition(), orientation);
 }
 
-bool Model::resetBaseLinearVelocity(const std::array<double, 3>& linear)
+bool Model::resetBaseWorldLinearVelocity(const std::array<double, 3>& linear)
 {
-    return this->resetBaseVelocity(linear, this->baseAngularVelocity());
+    return this->resetBaseWorldVelocity(linear,
+                                        this->baseWorldAngularVelocity());
 }
 
-bool Model::resetBaseAngularVelocity(const std::array<double, 3>& angular)
+bool Model::resetBaseWorldAngularVelocity(const std::array<double, 3>& angular)
 {
-    return this->resetBaseVelocity(this->baseLinearVelocity(), angular);
+    return this->resetBaseWorldVelocity(this->baseWorldLinearVelocity(),
+                                        angular);
 }
 
-bool Model::resetBaseVelocity(const std::array<double, 3>& linear,
-                              const std::array<double, 3>& angular)
+bool Model::resetBaseWorldVelocity(const std::array<double, 3>& linear,
+                                   const std::array<double, 3>& angular)
 {
     // Get the entity of the canonical (base) link
     auto canonicalLinkEntity = pImpl->ecm->EntityByComponents(
@@ -944,16 +993,17 @@ bool Model::setBaseOrientationTarget(const std::array<double, 4>& orientation)
     return true;
 }
 
-bool Model::setBaseVelocityTarget(const std::array<double, 3>& linear,
-                                  const std::array<double, 3>& angular)
+bool Model::setBaseWorldVelocityTarget(const std::array<double, 3>& linear,
+                                       const std::array<double, 3>& angular)
 {
     // TODO: the velocity target is not used by Gazebo.
     //       Should we compute the base velocity from the inputs?
-    return this->setBaseLinearVelocityTarget(linear)
-           && this->setBaseAngularVelocityTarget(angular);
+    return this->setBaseWorldLinearVelocityTarget(linear)
+           && this->setBaseWorldAngularVelocityTarget(angular);
 }
 
-bool Model::setBaseLinearVelocityTarget(const std::array<double, 3>& linear)
+bool Model::setBaseWorldLinearVelocityTarget(
+    const std::array<double, 3>& linear)
 {
     ignition::math::Vector3d baseWorldLinearVelocity =
         utils::toIgnitionVector3(linear);
@@ -965,7 +1015,8 @@ bool Model::setBaseLinearVelocityTarget(const std::array<double, 3>& linear)
     return true;
 }
 
-bool Model::setBaseAngularVelocityTarget(const std::array<double, 3>& angular)
+bool Model::setBaseWorldAngularVelocityTarget(
+    const std::array<double, 3>& angular)
 {
     ignition::math::Vector3d baseWorldAngularVelocity =
         utils::toIgnitionVector3(angular);
@@ -977,7 +1028,8 @@ bool Model::setBaseAngularVelocityTarget(const std::array<double, 3>& angular)
     return true;
 }
 
-bool Model::setBaseLinearAccelerationTarget(const std::array<double, 3>& linear)
+bool Model::setBaseWorldLinearAccelerationTarget(
+    const std::array<double, 3>& linear)
 {
     ignition::math::Vector3d baseWorldLinearAcceleration =
         utils::toIgnitionVector3(linear);
@@ -993,7 +1045,7 @@ bool Model::setBaseLinearAccelerationTarget(const std::array<double, 3>& linear)
     return true;
 }
 
-bool Model::setBaseAngularAccelerationTarget(
+bool Model::setBaseWorldAngularAccelerationTarget(
     const std::array<double, 3>& angular)
 {
     ignition::math::Vector3d baseWorldAngularAcceleration =
@@ -1024,7 +1076,7 @@ std::array<double, 4> Model::baseOrientationTarget() const
     return utils::fromIgnitionPose(basePoseTarget).orientation;
 }
 
-std::array<double, 3> Model::baseLinearVelocityTarget() const
+std::array<double, 3> Model::baseWorldLinearVelocityTarget() const
 {
     ignition::math::Vector3d& baseLinTarget = utils::getExistingComponentData<
         ignition::gazebo::components::BaseWorldLinearVelocityTarget>(
@@ -1033,7 +1085,7 @@ std::array<double, 3> Model::baseLinearVelocityTarget() const
     return utils::fromIgnitionVector(baseLinTarget);
 }
 
-std::array<double, 3> Model::baseAngularVelocityTarget() const
+std::array<double, 3> Model::baseWorldAngularVelocityTarget() const
 {
     ignition::math::Vector3d& baseAngTarget = utils::getExistingComponentData<
         ignition::gazebo::components::BaseWorldAngularVelocityTarget>(
@@ -1042,7 +1094,7 @@ std::array<double, 3> Model::baseAngularVelocityTarget() const
     return utils::fromIgnitionVector(baseAngTarget);
 }
 
-std::array<double, 3> Model::baseLinearAccelerationTarget() const
+std::array<double, 3> Model::baseWorldLinearAccelerationTarget() const
 {
     ignition::math::Vector3d& baseLinTarget = utils::getExistingComponentData<
         ignition::gazebo::components::BaseWorldLinearAccelerationTarget>(
@@ -1051,7 +1103,7 @@ std::array<double, 3> Model::baseLinearAccelerationTarget() const
     return utils::fromIgnitionVector(baseLinTarget);
 }
 
-std::array<double, 3> Model::baseAngularAccelerationTarget() const
+std::array<double, 3> Model::baseWorldAngularAccelerationTarget() const
 {
     ignition::math::Vector3d& baseAngTarget = utils::getExistingComponentData<
         ignition::gazebo::components::BaseWorldAngularAccelerationTarget>(

python/gym_ignition/utils/scenario.py

@@ -2,8 +2,10 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-from typing import Tuple
+import gym.spaces
+import numpy as np
 import gym_ignition_models
+from typing import List, Tuple
 from gym_ignition import scenario_bindings as bindings
 
 
@@ -100,3 +102,30 @@ def init_gazebo_sim(step_size: float = 0.001,
         raise RuntimeError("Failed to insert the physics plugin")
 
     return gazebo, world
+
+
+def get_joint_positions_space(model: bindings.Model,
+                              considered_joints: List[str] = None) -> gym.spaces.Box:
+    """
+    Build a Box space from the joint position limits.
+
+    Args:
+        model: The model from which generating the joint space.
+        considered_joints: List of considered joints. It is helpful to restrict the set
+            of joints and to enforce a custom joint serialization.
+
+    Returns:
+        A box space created from the model's joint position limits.
+    """
+
+    if considered_joints is None:
+        considered_joints = model.jointNames()
+
+    # Get the joint limits
+    joint_limits = model.jointLimits(considered_joints)
+
+    # Build the space
+    space = gym.spaces.Box(low=np.array(joint_limits.min),
+                           high=np.array(joint_limits.max))
+
+    return space

tests/common/utils.py

@@ -4,6 +4,7 @@
 
 import pytest
 from typing import Tuple
+import gym_ignition_models
 from gym_ignition.utils import misc
 from gym_ignition import scenario_bindings as bindings
 
@@ -47,6 +48,26 @@ def test_foo(gazebo: bindings.GazeboSimulator, name: str):
     gazebo.close()
 
 
+@pytest.fixture(scope="function")
+def default_world_fixture(request):
+    """
+    """
+
+    step_size, rtf, iterations = request.param
+    gazebo = bindings.GazeboSimulator(step_size, rtf, iterations)
+
+    assert gazebo.initialize()
+
+    world = gazebo.getWorld()
+    assert world.insertModel(gym_ignition_models.get_model_file("ground_plane"))
+    assert world.insertWorldPlugin("libPhysicsSystem.so",
+                                   "scenario::plugins::gazebo::Physics")
+
+    yield gazebo, world
+
+    gazebo.close()
+
+
 def get_multi_world_sdf_file() -> str:
 
     multi_world_sdf_string = f"""