From e7b9b1edc8c9129b521f59def25a020d7432fc38 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 26 Apr 2020 23:51:05 +0200 Subject: [PATCH 1/7] Update base velocity and acceleration APIs - Get both body and world velocity - Update the setters to make explicit that they set the world velocity and acceleration --- .../gazebo/include/scenario/gazebo/Model.h | 44 +++++++----- cpp/scenario/gazebo/src/Model.cpp | 71 +++++++++++++------ 2 files changed, 76 insertions(+), 39 deletions(-) diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h index 623cf82b9..bccb965a8 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h @@ -178,15 +178,18 @@ class scenario::gazebo::Model std::array basePosition() const; std::array baseOrientation() const; - std::array baseLinearVelocity() const; - std::array baseAngularVelocity() const; - - bool - resetBaseLinearVelocity(const std::array& linear = {0, 0, 0}); - bool - resetBaseAngularVelocity(const std::array& angular = {0, 0, 0}); - bool resetBaseVelocity(const std::array& linear = {0, 0, 0}, - const std::array& angular = {0, 0, 0}); + std::array baseBodyLinearVelocity() const; + std::array baseBodyAngularVelocity() const; + std::array baseWorldLinearVelocity() const; + std::array baseWorldAngularVelocity() const; + + bool resetBaseWorldLinearVelocity( + const std::array& linear = {0, 0, 0}); + bool resetBaseWorldAngularVelocity( + const std::array& angular = {0, 0, 0}); + bool resetBaseWorldVelocity( // + const std::array& linear = {0, 0, 0}, + const std::array& angular = {0, 0, 0}); bool resetBasePose(const std::array& position = {0, 0, 0}, const std::array& orientation = {0, 0, 0, 0}); @@ -203,19 +206,22 @@ class scenario::gazebo::Model bool setBasePositionTarget(const std::array& position); bool setBaseOrientationTarget(const std::array& orientation); - bool setBaseVelocityTarget(const std::array& linear, - const std::array& angular); - bool setBaseLinearVelocityTarget(const std::array& linear); - bool setBaseAngularVelocityTarget(const std::array& angular); - bool setBaseLinearAccelerationTarget(const std::array& linear); - bool setBaseAngularAccelerationTarget(const std::array& angular); + bool setBaseWorldVelocityTarget(const std::array& linear, + const std::array& angular); + bool setBaseWorldLinearVelocityTarget(const std::array& linear); + bool setBaseWorldAngularVelocityTarget( // + const std::array& angular); + bool setBaseWorldLinearAccelerationTarget( // + const std::array& linear); + bool setBaseWorldAngularAccelerationTarget( // + const std::array& angular); std::array basePositionTarget() const; std::array baseOrientationTarget() const; - std::array baseLinearVelocityTarget() const; - std::array baseAngularVelocityTarget() const; - std::array baseLinearAccelerationTarget() const; - std::array baseAngularAccelerationTarget() const; + std::array baseWorldLinearVelocityTarget() const; + std::array baseWorldAngularVelocityTarget() const; + std::array baseWorldLinearAccelerationTarget() const; + std::array baseWorldAngularAccelerationTarget() const; private: class Impl; diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp index fd6d1e7f0..ee4c35472 100644 --- a/cpp/scenario/gazebo/src/Model.cpp +++ b/cpp/scenario/gazebo/src/Model.cpp @@ -753,7 +753,33 @@ std::array Model::baseOrientation() const return utils::fromIgnitionPose(world_H_model).orientation; } -std::array Model::baseLinearVelocity() const +std::array 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 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 Model::baseWorldLinearVelocity() const { // Get the entity of the canonical link auto canonicalLinkEntity = pImpl->ecm->EntityByComponents( @@ -792,7 +818,7 @@ std::array Model::baseLinearVelocity() const return utils::fromIgnitionVector(modelLinearVelocity); } -std::array Model::baseAngularVelocity() const +std::array 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 +879,20 @@ bool Model::resetBaseOrientation(const std::array& orientation) return this->resetBasePose(this->basePosition(), orientation); } -bool Model::resetBaseLinearVelocity(const std::array& linear) +bool Model::resetBaseWorldLinearVelocity(const std::array& linear) { - return this->resetBaseVelocity(linear, this->baseAngularVelocity()); + return this->resetBaseWorldVelocity(linear, + this->baseWorldAngularVelocity()); } -bool Model::resetBaseAngularVelocity(const std::array& angular) +bool Model::resetBaseWorldAngularVelocity(const std::array& angular) { - return this->resetBaseVelocity(this->baseLinearVelocity(), angular); + return this->resetBaseWorldVelocity(this->baseWorldLinearVelocity(), + angular); } -bool Model::resetBaseVelocity(const std::array& linear, - const std::array& angular) +bool Model::resetBaseWorldVelocity(const std::array& linear, + const std::array& angular) { // Get the entity of the canonical (base) link auto canonicalLinkEntity = pImpl->ecm->EntityByComponents( @@ -944,16 +972,17 @@ bool Model::setBaseOrientationTarget(const std::array& orientation) return true; } -bool Model::setBaseVelocityTarget(const std::array& linear, - const std::array& angular) +bool Model::setBaseWorldVelocityTarget(const std::array& linear, + const std::array& 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& linear) +bool Model::setBaseWorldLinearVelocityTarget( + const std::array& linear) { ignition::math::Vector3d baseWorldLinearVelocity = utils::toIgnitionVector3(linear); @@ -965,7 +994,8 @@ bool Model::setBaseLinearVelocityTarget(const std::array& linear) return true; } -bool Model::setBaseAngularVelocityTarget(const std::array& angular) +bool Model::setBaseWorldAngularVelocityTarget( + const std::array& angular) { ignition::math::Vector3d baseWorldAngularVelocity = utils::toIgnitionVector3(angular); @@ -977,7 +1007,8 @@ bool Model::setBaseAngularVelocityTarget(const std::array& angular) return true; } -bool Model::setBaseLinearAccelerationTarget(const std::array& linear) +bool Model::setBaseWorldLinearAccelerationTarget( + const std::array& linear) { ignition::math::Vector3d baseWorldLinearAcceleration = utils::toIgnitionVector3(linear); @@ -993,7 +1024,7 @@ bool Model::setBaseLinearAccelerationTarget(const std::array& linear) return true; } -bool Model::setBaseAngularAccelerationTarget( +bool Model::setBaseWorldAngularAccelerationTarget( const std::array& angular) { ignition::math::Vector3d baseWorldAngularAcceleration = @@ -1024,7 +1055,7 @@ std::array Model::baseOrientationTarget() const return utils::fromIgnitionPose(basePoseTarget).orientation; } -std::array Model::baseLinearVelocityTarget() const +std::array Model::baseWorldLinearVelocityTarget() const { ignition::math::Vector3d& baseLinTarget = utils::getExistingComponentData< ignition::gazebo::components::BaseWorldLinearVelocityTarget>( @@ -1033,7 +1064,7 @@ std::array Model::baseLinearVelocityTarget() const return utils::fromIgnitionVector(baseLinTarget); } -std::array Model::baseAngularVelocityTarget() const +std::array Model::baseWorldAngularVelocityTarget() const { ignition::math::Vector3d& baseAngTarget = utils::getExistingComponentData< ignition::gazebo::components::BaseWorldAngularVelocityTarget>( @@ -1042,7 +1073,7 @@ std::array Model::baseAngularVelocityTarget() const return utils::fromIgnitionVector(baseAngTarget); } -std::array Model::baseLinearAccelerationTarget() const +std::array Model::baseWorldLinearAccelerationTarget() const { ignition::math::Vector3d& baseLinTarget = utils::getExistingComponentData< ignition::gazebo::components::BaseWorldLinearAccelerationTarget>( @@ -1051,7 +1082,7 @@ std::array Model::baseLinearAccelerationTarget() const return utils::fromIgnitionVector(baseLinTarget); } -std::array Model::baseAngularAccelerationTarget() const +std::array Model::baseWorldAngularAccelerationTarget() const { ignition::math::Vector3d& baseAngTarget = utils::getExistingComponentData< ignition::gazebo::components::BaseWorldAngularAccelerationTarget>( From ea0d34f2c557cf3ecd50ab620fac4330817ee8b9 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 27 Apr 2020 00:00:22 +0200 Subject: [PATCH 2/7] Fix wrong methods implementation --- cpp/scenario/gazebo/src/Link.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cpp/scenario/gazebo/src/Link.cpp b/cpp/scenario/gazebo/src/Link.cpp index 0541448ef..3ee9ac57d 100644 --- a/cpp/scenario/gazebo/src/Link.cpp +++ b/cpp/scenario/gazebo/src/Link.cpp @@ -255,8 +255,8 @@ std::array Link::worldAngularVelocity() const std::array 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); } @@ -264,8 +264,8 @@ std::array Link::bodyLinearVelocity() const std::array 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); } From 861595a69bce16fe29d09f5039c8cf2d72c653d9 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 26 Apr 2020 23:54:57 +0200 Subject: [PATCH 3/7] Update current tests to new base APIs --- tests/test_scenario/test_model.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/tests/test_scenario/test_model.py b/tests/test_scenario/test_model.py index ec04ae8aa..e8ae4f9e4 100644 --- a/tests/test_scenario/test_model.py +++ b/tests/test_scenario/test_model.py @@ -138,10 +138,10 @@ def test_model_base(gazebo: bindings.GazeboSimulator): # Reset the linear velocity lin_velocity = [0, 0, 5.0] - assert model.resetBaseLinearVelocity(lin_velocity) - assert model.baseLinearVelocity() == pytest.approx([0, 0, 0]) + assert model.resetBaseWorldLinearVelocity(lin_velocity) + assert model.baseWorldLinearVelocity() == pytest.approx([0, 0, 0]) gazebo.run() - assert model.baseLinearVelocity() == pytest.approx(lin_velocity, abs=0.01) + assert model.baseWorldLinearVelocity() == pytest.approx(lin_velocity, abs=0.01) # The linear velocity of the support must be the same assert "support" in model.linkNames() @@ -150,10 +150,10 @@ def test_model_base(gazebo: bindings.GazeboSimulator): # Reset the angular velocity ang_velocity = [0.1, 0.5, -3.0] - assert model.resetBaseAngularVelocity(ang_velocity) - assert model.baseAngularVelocity() == pytest.approx([0, 0, 0]) + assert model.resetBaseWorldAngularVelocity(ang_velocity) + assert model.baseWorldAngularVelocity() == pytest.approx([0, 0, 0]) gazebo.run() - assert model.baseAngularVelocity() == pytest.approx(ang_velocity, abs=0.01) + assert model.baseWorldAngularVelocity() == pytest.approx(ang_velocity, abs=0.01) # The angular velocity of the support must be the same assert "support" in model.linkNames() @@ -196,12 +196,12 @@ def test_model_references(gazebo: bindings.GazeboSimulator): assert model.basePositionTarget() == pytest.approx([0, 0, 5]) assert model.baseOrientationTarget() == pytest.approx([0, 0, 1.0, 0]) - assert model.setBaseLinearVelocityTarget([1, 2, 3]) - assert model.setBaseAngularVelocityTarget([4, 5, 6]) - assert model.setBaseAngularAccelerationTarget([-1, -2, -3]) - assert model.baseLinearVelocityTarget() == pytest.approx([1, 2, 3]) - assert model.baseAngularVelocityTarget() == pytest.approx([4, 5, 6]) - assert model.baseAngularAccelerationTarget() == pytest.approx([-1, -2, -3]) + assert model.setBaseWorldLinearVelocityTarget([1, 2, 3]) + assert model.setBaseWorldAngularVelocityTarget([4, 5, 6]) + assert model.setBaseWorldAngularAccelerationTarget([-1, -2, -3]) + assert model.baseWorldLinearVelocityTarget() == pytest.approx([1, 2, 3]) + assert model.baseWorldAngularVelocityTarget() == pytest.approx([4, 5, 6]) + assert model.baseWorldAngularAccelerationTarget() == pytest.approx([-1, -2, -3]) # def test_model_contacts(): From f75760508243daaed5de6e412cf6b6f50ca95b89 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 26 Apr 2020 23:59:51 +0200 Subject: [PATCH 4/7] Get all joint limits from the Model --- .../gazebo/include/scenario/gazebo/Model.h | 7 +++++-- cpp/scenario/gazebo/src/Model.cpp | 21 +++++++++++++++++++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h index bccb965a8..6a4aafe77 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h +++ b/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 #include #include @@ -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; using JointPtr = std::shared_ptr; @@ -125,6 +125,9 @@ class scenario::gazebo::Model std::vector jointVelocities( // const std::vector& jointNames = {}) const; + base::JointLimit jointLimits( // + const std::vector& jointNames = {}) const; + bool setJointControlMode(const base::JointControlMode mode, const std::vector& jointNames = {}); diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp index ee4c35472..066baeb10 100644 --- a/cpp/scenario/gazebo/src/Model.cpp +++ b/cpp/scenario/gazebo/src/Model.cpp @@ -525,6 +525,27 @@ Model::jointVelocities(const std::vector& jointNames) const return Impl::getJointDataSerialized(this, jointNames, lambda); } +scenario::base::JointLimit +Model::jointLimits(const std::vector& jointNames) const +{ + const std::vector jointSerialization = + jointNames.empty() ? this->jointNames() : jointNames; + + std::vector low; + std::vector 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& jointNames) { From 1dd06350687fe0d7fe6d43bdbbd067c63f33fff5 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 26 Apr 2020 23:56:36 +0200 Subject: [PATCH 5/7] New fixture with default world --- tests/common/utils.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/tests/common/utils.py b/tests/common/utils.py index ce64684df..f753a801b 100644 --- a/tests/common/utils.py +++ b/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""" From ed8b8f8e9ea02371a969c4fb23109047889c7d5d Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 27 Apr 2020 00:19:24 +0200 Subject: [PATCH 6/7] Helper to create a Space from joint limits of a model --- python/gym_ignition/utils/scenario.py | 31 ++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/python/gym_ignition/utils/scenario.py b/python/gym_ignition/utils/scenario.py index 04656dd98..31880b2f7 100644 --- a/python/gym_ignition/utils/scenario.py +++ b/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 From 70f87123bb61f0b189428302de5a6e086dd15f61 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 26 Apr 2020 23:55:53 +0200 Subject: [PATCH 7/7] Add new tests for the velocities and accelerations --- tests/test_scenario/test_link_velocities.py | 314 ++++++++++++++++++++ 1 file changed, 314 insertions(+) create mode 100644 tests/test_scenario/test_link_velocities.py diff --git a/tests/test_scenario/test_link_velocities.py b/tests/test_scenario/test_link_velocities.py new file mode 100644 index 000000000..7e59754dd --- /dev/null +++ b/tests/test_scenario/test_link_velocities.py @@ -0,0 +1,314 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import pytest +pytestmark = pytest.mark.scenario + +import numpy as np +from ..common import utils +import gym_ignition_models +from typing import Callable, List, Tuple +from scipy.spatial.transform import Rotation +from gym_ignition import scenario_bindings as bindings +from ..common.utils import default_world_fixture as default_world +from gym_ignition.utils.scenario import get_joint_positions_space + + +def to_wxyz(xyzw: np.ndarray) -> np.ndarray: + + if xyzw.shape != (4,): + raise ValueError(xyzw) + + return xyzw[[3, 0, 1, 2]] + + +def to_xyzw(wxyz: np.ndarray) -> np.ndarray: + + if wxyz.shape != (4,): + raise ValueError(wxyz) + + return wxyz[[1, 2, 3, 0]] + + +def to_matrix(quaternion: List[float]) -> np.ndarray: + + quaternion_xyzw = to_xyzw(np.array(quaternion)) + return Rotation.from_quat(quaternion_xyzw).as_matrix() + + +def get_random_panda(gazebo: bindings.GazeboSimulator, + world: bindings.World) -> bindings.Model: + + panda_urdf = gym_ignition_models.get_model_file("panda") + assert world.insertModel(panda_urdf) + assert "panda" in world.modelNames() + + panda = world.getModel("panda") + + joint_space = get_joint_positions_space(model=panda) + joint_space.seed(10) + + q = joint_space.sample() + dq = joint_space.np_random.uniform(low=-1.0, high=1.0, size=q.shape) + + assert panda.resetJointPositions(q.tolist()) + assert panda.resetJointVelocities(dq.tolist()) + + assert gazebo.run(paused=True) + return panda + + +def get_cube(gazebo: bindings.GazeboSimulator, + world: bindings.World) -> bindings.Model: + + quaternion = to_wxyz(Rotation.from_euler('x', 45, degrees=True).as_quat()) + initial_pose = bindings.Pose([0, 0, 0.5], quaternion.tolist()) + + cube_urdf = utils.get_cube_urdf() + assert world.insertModel(cube_urdf, initial_pose) + assert "cube_robot" in world.modelNames() + + cube = world.getModel("cube_robot") + + assert cube.resetBaseWorldLinearVelocity([0.1, -0.2, -0.3]) + assert cube.resetBaseWorldAngularVelocity([-0.1, 2.0, 0.3]) + + assert gazebo.run(paused=True) + return cube + + +# 1. VELOCITY LINEAR +@pytest.mark.parametrize("default_world, get_model, link_name", [ + ((1.0 / 10_000, 1.0, 1), get_random_panda, "panda_link7"), + ((1.0 / 10_000, 1.0, 1), get_cube, "cube"), + ], + indirect=["default_world"]) +def test_linear_velocity( + default_world: Tuple[bindings.GazeboSimulator, bindings.World], + get_model: Callable[[bindings.GazeboSimulator, bindings.World], bindings.Model], + link_name: str): + + # Get the simulator and the world + gazebo, world = default_world + dt = gazebo.stepSize() + + # Get the model + model = get_model(gazebo, world) + + # Get the link. + # If the link is the base link, get the data through the base methods. + link = model.getLink(link_name) + + if link.name() != model.baseFrame(): + + position = link.position + orientation = link.orientation + bodyLinearVelocity = link.bodyLinearVelocity + worldLinearVelocity = link.worldLinearVelocity + + else: + + position = model.basePosition + orientation = model.baseOrientation + bodyLinearVelocity = model.baseBodyLinearVelocity + worldLinearVelocity = model.baseWorldLinearVelocity + + # 0.5 seconds of simulation + for _ in range(int(0.5 / dt)): + + position_old = np.array(position()) + assert gazebo.run() + position_new = np.array(position()) + + world_velocity = (position_new - position_old) / dt + + # Test the world velocity (MIXED) + assert world_velocity == pytest.approx(worldLinearVelocity(), abs=1E-2) + + # Eq 18 + # Test the BODY velocity + W_R_L = to_matrix(orientation()) + body_velocity = W_R_L.transpose() @ np.array(worldLinearVelocity()) + assert body_velocity == pytest.approx(bodyLinearVelocity()) + + gazebo.close() + + +# 2. VELOCITY ANGULAR +@pytest.mark.parametrize("default_world, get_model, link_name", [ + ((1.0 / 10_000, 1.0, 1), get_random_panda, "panda_link7"), + ((1.0 / 10_000, 1.0, 1), get_cube, "cube"), + ], + indirect=["default_world"]) +def test_angular_velocity( + default_world: Tuple[bindings.GazeboSimulator, bindings.World], + get_model: Callable[[bindings.GazeboSimulator, bindings.World], bindings.Model], + link_name: str): + + # Get the simulator and the world + gazebo, world = default_world + dt = gazebo.stepSize() + + # Get the model + model = get_model(gazebo, world) + + # Get the link. + # If the link is the base link, get the data through the base methods. + link = model.getLink(link_name) + + if link.name() != model.baseFrame(): + + orientation = link.orientation + bodyAngularVelocity = link.bodyAngularVelocity + worldAngularVelocity = link.worldAngularVelocity + + else: + + orientation = model.baseOrientation + bodyAngularVelocity = model.baseBodyAngularVelocity + worldAngularVelocity = model.baseWorldAngularVelocity + + skew = lambda matrix: (matrix - matrix.transpose()) / 2 + vee = lambda matrix: [matrix[2, 1], matrix[0,2], matrix[1, 0]] + + for _ in range(int(0.5 / dt)): + + W_R_L_old = to_matrix(orientation()) + assert gazebo.run() + W_R_L_new = to_matrix(orientation()) + + dot_rotation_matrix = (W_R_L_new - W_R_L_old) / dt + world_velocity = dot_rotation_matrix @ W_R_L_new.transpose() + + # Test the world velocity (MIXED) + assert vee(skew(world_velocity)) == pytest.approx(worldAngularVelocity(), + abs=0.005) + + # Test the BODY velocity + body_velocity = W_R_L_new.transpose() @ dot_rotation_matrix + assert vee(skew(body_velocity)) == pytest.approx(bodyAngularVelocity(), + abs=0.005) + + +# 3. ACCELERATION LINEAR +@pytest.mark.parametrize("default_world, get_model, link_name", [ + ((1.0 / 10_000, 1.0, 1), get_random_panda, "panda_link7"), + # ((1.0 / 10_000, 1.0, 1), get_cube, "cube"), # TODO + ], + indirect=["default_world"]) +def test_linear_acceleration( + default_world: Tuple[bindings.GazeboSimulator, bindings.World], + get_model: Callable[[bindings.GazeboSimulator, bindings.World], bindings.Model], + link_name: str): + + # Get the simulator and the world + gazebo, world = default_world + dt = gazebo.stepSize() + + # Get the model + model = get_model(gazebo, world) + + # Get the link. + # If the link is the base link, get the data through the base methods. + link = model.getLink(link_name) + + if link.name() != model.baseFrame(): + + orientation = link.orientation + worldLinearVelocity = link.worldLinearVelocity + bodyLinearAcceleration = link.bodyLinearAcceleration + worldLinearAcceleration = link.worldLinearAcceleration + + else: + + orientation = model.baseOrientation + worldLinearVelocity = model.baseWorldLinearVelocity + bodyLinearAcceleration = model.baseBodyLinearAcceleration + worldLinearAcceleration = model.baseWordLinearAcceleration + + # 0.5 seconds of simulation + for _ in range(int(0.5 / dt)): + + velocity_old = np.array(worldLinearVelocity()) + assert gazebo.run() + velocity_new = np.array(worldLinearVelocity()) + + world_acceleration = (velocity_new - velocity_old) / dt + + # By time to time there are steps where the acceleration becomes extremely high, + # like 1000 m/s/s when the average is never exceeds 4 m/s/s. + # We exclude those points. We should understand why this happens. + if (np.array(worldLinearAcceleration()) > 100.0).any(): + continue + + # Test the world acceleration (MIXED) + assert worldLinearAcceleration() == pytest.approx(world_acceleration, abs=0.5) + + # Test the BODY acceleration + # Note: https://github.com/ignitionrobotics/ign-gazebo/issues/87 + W_R_L = to_matrix(orientation()) + body_acceleration = W_R_L.transpose() @ np.array(worldLinearAcceleration()) + assert body_acceleration == pytest.approx(bodyLinearAcceleration()) + + gazebo.close() + + +# 2. ACCELERATION ANGULAR +@pytest.mark.parametrize("default_world, get_model, link_name", [ + ((1.0 / 10_000, 1.0, 1), get_random_panda, "panda_link7"), + # ((1.0 / 10_000, 1.0, 1), get_cube, "cube"), # TODO + ], + indirect=["default_world"]) +def test_angular_acceleration( + default_world: Tuple[bindings.GazeboSimulator, bindings.World], + get_model: Callable[[bindings.GazeboSimulator, bindings.World], bindings.Model], + link_name: str): + + # Get the simulator and the world + gazebo, world = default_world + dt = gazebo.stepSize() + + # Get the model + model = get_model(gazebo, world) + + # Get the link. + # If the link is the base link, get the data through the base methods. + link = model.getLink(link_name) + + if link.name() != model.baseFrame(): + + orientation = link.orientation + worldAngularVelocity = link.worldAngularVelocity + bodyAngularAcceleration = link.bodyAngularAcceleration + worldAngularAcceleration = link.worldAngularAcceleration + + else: + + orientation = model.baseOrientation + worldAngularVelocity = model.baseWorldAngularVelocity + bodyAngularAcceleration = model.baseBodyAngularAcceleration + worldAngularAcceleration = model.baseWordAngularAcceleration + + for _ in range(int(0.5 / dt)): + + world_velocity_old = np.array(worldAngularVelocity()) + assert gazebo.run() + world_velocity_new = np.array(worldAngularVelocity()) + + world_acceleration = (world_velocity_new - world_velocity_old) / dt + + # By time to time there are steps where the acceleration becomes extremely high, + # like 1000 rad/s/s when the average is never exceeds 20 rad/s/s. + # We exclude those points. We should understand why this happens. + if (np.array(worldAngularAcceleration()) > 100.0).any(): + continue + + # Test the world acceleration (MIXED) + assert world_acceleration == pytest.approx(worldAngularAcceleration(), + abs=0.2) + + # Note: https://github.com/ignitionrobotics/ign-gazebo/issues/87 + W_R_L = to_matrix(orientation()) + body_acceleration = W_R_L.transpose() @ np.array(worldAngularAcceleration()) + assert body_acceleration == pytest.approx(bodyAngularAcceleration())