Commit 8e6467a3 authored by Jordan Lack's avatar Jordan Lack

Merge branch 'feature/RDL-118' of gitlab.com:jlack/rdl into feature/RDL-117

parents abae4716 19edbb3f
Pipeline #60845906 skipped
......@@ -11,7 +11,6 @@
#ifndef __RDL_URDFREADER_H__
#define __RDL_URDFREADER_H__
#include "rdl_dynamics/rdl_config.h"
#include "rdl_dynamics/types.hpp"
namespace RobotDynamics
......
......@@ -5,10 +5,10 @@
#include <gtest/gtest.h>
#include <ros/package.h>
#include <rdl_dynamics/Dynamics.h>
#include <rdl_dynamics/Kinematics.h>
#include <rdl_dynamics/Model.h>
#include <rdl_urdfreader/urdfreader.h>
#include "rdl_dynamics/Dynamics.h"
#include "rdl_dynamics/Kinematics.h"
#include "rdl_dynamics/Model.h"
#include "rdl_urdfreader/urdfreader.h"
class UrdfReaderTests : public testing::Test
{
......@@ -59,74 +59,74 @@ class UrdfReaderTests : public testing::Test
TEST_F(UrdfReaderTests, testFirstJointFixedNonTrivialTransform)
{
std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/first_joint_fixed_non_trivial_xform.urdf";
RobotDynamics::Model model;
RobotDynamics::ModelPtr model(new RobotDynamics::Model());
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), &model, false, false));
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), model, false, false));
RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model.q_size);
RobotDynamics::Math::VectorNd qdot = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd qddot = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
RobotDynamics::Math::VectorNd qdot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
RobotDynamics::Math::VectorNd qddot = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
ASSERT_EQ(model.q_size, 1);
ASSERT_EQ(model.qdot_size, 1);
ASSERT_EQ(model->q_size, 1);
ASSERT_EQ(model->qdot_size, 1);
RobotDynamics::updateKinematics(model, q, qdot, qddot);
RobotDynamics::ReferenceFramePtr frame = model.referenceFrameMap["j1_link"];
RobotDynamics::updateKinematics(*model, q, qdot, qddot);
RobotDynamics::ReferenceFramePtr frame = model->referenceFrameMap["j1_link"];
EXPECT_TRUE(model.referenceFrameMap["j1_link"]->getInverseTransformToRoot().r.isApprox(RobotDynamics::Math::Vector3d(0.0, 0.16, 0.8377), 1e-4));
EXPECT_TRUE(model->referenceFrameMap["j1_link"]->getInverseTransformToRoot().r.isApprox(RobotDynamics::Math::Vector3d(0.0, 0.16, 0.8377), 1e-4));
}
TEST_F(UrdfReaderTests, testFixedArm)
{
std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
RobotDynamics::Model model;
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), &model, false, false));
RobotDynamics::ModelPtr model(new RobotDynamics::Model());
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), model, false, false));
// First body in URDf is a fixed joint body, so it'll get mashed together with the root body
EXPECT_EQ(model.mBodies[0].mMass, 4.);
EXPECT_TRUE(model.mBodies[0].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0., 0.), 1e-14));
EXPECT_EQ(model.Ib_c[0].Ixx, 0.0061063308908);
EXPECT_EQ(model.Ib_c[0].Iyx, 0.0);
EXPECT_EQ(model.Ib_c[0].Izx, 0.0);
EXPECT_EQ(model.Ib_c[0].Iyy, 0.0061063308908);
EXPECT_EQ(model.Ib_c[0].Izy, 0.0);
EXPECT_EQ(model.Ib_c[0].Izz, 0.01125);
unsigned int id = model.GetBodyId("test_robot_shoulder_link");
EXPECT_EQ(model.mBodies[id].mMass, 7.778);
EXPECT_TRUE(model.mBodies[id].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0.01, 0.), 1e-14));
EXPECT_EQ(model.Ib_c[id].Ixx, 0.0314743125769);
EXPECT_EQ(model.Ib_c[id].Iyx, 0.);
EXPECT_EQ(model.Ib_c[id].Izx, 0.);
EXPECT_EQ(model.Ib_c[id].Iyy, 0.0314743125769);
EXPECT_EQ(model.Ib_c[id].Izy, 0.);
EXPECT_EQ(model.Ib_c[id].Izz, 0.021875625);
id = model.GetParentBodyId(model.GetBodyId("gripper_right_finger_link"));
EXPECT_EQ(model.GetBodyId("gripper_right_knuckle_link"), id);
EXPECT_EQ(model.mJoints[1].mJointType, RobotDynamics::JointTypePrismatic);
EXPECT_EQ(model.GetBodyId("test_robot_shoulder_link"), model.lambda[model.GetBodyId("test_robot_upper_arm_link")]);
EXPECT_EQ(model->mBodies[0].mMass, 4.);
EXPECT_TRUE(model->mBodies[0].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0., 0.), 1e-14));
EXPECT_EQ(model->Ib_c[0].Ixx, 0.0061063308908);
EXPECT_EQ(model->Ib_c[0].Iyx, 0.0);
EXPECT_EQ(model->Ib_c[0].Izx, 0.0);
EXPECT_EQ(model->Ib_c[0].Iyy, 0.0061063308908);
EXPECT_EQ(model->Ib_c[0].Izy, 0.0);
EXPECT_EQ(model->Ib_c[0].Izz, 0.01125);
unsigned int id = model->GetBodyId("test_robot_shoulder_link");
EXPECT_EQ(model->mBodies[id].mMass, 7.778);
EXPECT_TRUE(model->mBodies[id].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0., 0.01, 0.), 1e-14));
EXPECT_EQ(model->Ib_c[id].Ixx, 0.0314743125769);
EXPECT_EQ(model->Ib_c[id].Iyx, 0.);
EXPECT_EQ(model->Ib_c[id].Izx, 0.);
EXPECT_EQ(model->Ib_c[id].Iyy, 0.0314743125769);
EXPECT_EQ(model->Ib_c[id].Izy, 0.);
EXPECT_EQ(model->Ib_c[id].Izz, 0.021875625);
id = model->GetParentBodyId(model->GetBodyId("gripper_right_finger_link"));
EXPECT_EQ(model->GetBodyId("gripper_right_knuckle_link"), id);
EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypePrismatic);
EXPECT_EQ(model->GetBodyId("test_robot_shoulder_link"), model->lambda[model->GetBodyId("test_robot_upper_arm_link")]);
}
TEST_F(UrdfReaderTests, testNegative1DofJointAxes)
{
std::string file_path = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm.urdf";
std::string file_path_2 = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/test_robot_arm_neg_jnt_axes.urdf";
RobotDynamics::Model model, model_neg_axes;
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), &model, false, false));
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path_2.c_str(), &model_neg_axes, false, false));
RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model.q_size);
RobotDynamics::Math::VectorNd q_neg = RobotDynamics::Math::VectorNd::Zero(model.q_size);
RobotDynamics::Math::VectorNd qd = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd qd_neg = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd qdd = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd qdd_neg = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd tau = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::Math::VectorNd tau_neg = RobotDynamics::Math::VectorNd::Zero(model.qdot_size);
RobotDynamics::ModelPtr model(new RobotDynamics::Model()), model_neg_axes(new RobotDynamics::Model());
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path.c_str(), model, false, false));
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(file_path_2.c_str(), model_neg_axes, false, false));
RobotDynamics::Math::VectorNd q = RobotDynamics::Math::VectorNd::Zero(model->q_size);
RobotDynamics::Math::VectorNd q_neg = RobotDynamics::Math::VectorNd::Zero(model->q_size);
RobotDynamics::Math::VectorNd qd = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
RobotDynamics::Math::VectorNd qd_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
RobotDynamics::Math::VectorNd qdd = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
RobotDynamics::Math::VectorNd qdd_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
RobotDynamics::Math::VectorNd tau = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
RobotDynamics::Math::VectorNd tau_neg = RobotDynamics::Math::VectorNd::Zero(model->qdot_size);
for (int i = 0; i < q.size(); i++)
{
......@@ -141,22 +141,22 @@ TEST_F(UrdfReaderTests, testNegative1DofJointAxes)
qdd_neg = -qdd;
tau_neg = -tau;
RobotDynamics::updateKinematics(model, q, qd, qdd);
RobotDynamics::updateKinematics(model_neg_axes, q_neg, qd_neg, qdd_neg);
RobotDynamics::updateKinematics(*model, q, qd, qdd);
RobotDynamics::updateKinematics(*model_neg_axes, q_neg, qd_neg, qdd_neg);
for (unsigned int i = 0; i < model.mBodies.size(); i++)
for (unsigned int i = 0; i < model->mBodies.size(); i++)
{
EXPECT_TRUE(checkSpatialVectorsEpsilonClose(model.v[i], model_neg_axes.v[i], 1e-14));
EXPECT_TRUE(checkSpatialVectorsEpsilonClose(model.a[i], model_neg_axes.a[i], 1e-14));
EXPECT_TRUE(checkSpatialVectorsEpsilonClose(model->v[i], model_neg_axes->v[i], 1e-14));
EXPECT_TRUE(checkSpatialVectorsEpsilonClose(model->a[i], model_neg_axes->a[i], 1e-14));
}
RobotDynamics::forwardDynamics(model, q, qd, tau, qdd);
RobotDynamics::forwardDynamics(model_neg_axes, q_neg, qd_neg, tau_neg, qdd_neg);
RobotDynamics::forwardDynamics(*model, q, qd, tau, qdd);
RobotDynamics::forwardDynamics(*model_neg_axes, q_neg, qd_neg, tau_neg, qdd_neg);
EXPECT_TRUE(qdd.isApprox(-qdd_neg, 1e-14));
RobotDynamics::inverseDynamics(model, q, qd, qdd, tau);
RobotDynamics::inverseDynamics(model_neg_axes, q_neg, qd_neg, qdd_neg, tau_neg);
RobotDynamics::inverseDynamics(*model, q, qd, qdd, tau);
RobotDynamics::inverseDynamics(*model_neg_axes, q_neg, qd_neg, qdd_neg, tau_neg);
EXPECT_TRUE(tau.isApprox(-tau_neg, 1e-14));
}
......@@ -175,27 +175,27 @@ TEST_F(UrdfReaderTests, testJointBodyMap)
TEST_F(UrdfReaderTests, testFloatingBaseRobot)
{
RobotDynamics::Model model;
RobotDynamics::ModelPtr model(new RobotDynamics::Model());
std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), &model, true, false));
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
EXPECT_NEAR(model.mBodies[2].mMass, 4., 1e-14);
EXPECT_TRUE(model.mBodies[2].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0.3, 0.2, 0.1), 1e-14));
EXPECT_NEAR(model->mBodies[2].mMass, 4., 1e-14);
EXPECT_TRUE(model->mBodies[2].mCenterOfMass.isApprox(RobotDynamics::Math::Vector3d(0.3, 0.2, 0.1), 1e-14));
EXPECT_NEAR(model.Ib_c[2].Ixx, 0.1, 1e-14);
EXPECT_NEAR(model.Ib_c[2].Iyx, 0., 1e-14);
EXPECT_NEAR(model.Ib_c[2].Izx, 0., 1e-14);
EXPECT_NEAR(model.Ib_c[2].Iyy, 0.2, 1e-14);
EXPECT_NEAR(model.Ib_c[2].Izy, 0., 1e-14);
EXPECT_NEAR(model.Ib_c[2].Izz, 0.3, 1e-14);
EXPECT_NEAR(model->Ib_c[2].Ixx, 0.1, 1e-14);
EXPECT_NEAR(model->Ib_c[2].Iyx, 0., 1e-14);
EXPECT_NEAR(model->Ib_c[2].Izx, 0., 1e-14);
EXPECT_NEAR(model->Ib_c[2].Iyy, 0.2, 1e-14);
EXPECT_NEAR(model->Ib_c[2].Izy, 0., 1e-14);
EXPECT_NEAR(model->Ib_c[2].Izz, 0.3, 1e-14);
EXPECT_EQ(model.mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
EXPECT_EQ(model.mJoints[2].mJointType, RobotDynamics::JointTypeSpherical);
EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeSpherical);
}
TEST_F(UrdfReaderTests, testFloatingBaseRobotDeduceFloatingBase)
{
RobotDynamics::RdlModelPtr model(new RobotDynamics::Model());
RobotDynamics::ModelPtr model(new RobotDynamics::Model());
std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_base_robot.urdf";
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf, model));
......@@ -215,19 +215,19 @@ TEST_F(UrdfReaderTests, testFloatingBaseRobotDeduceFloatingBase)
TEST_F(UrdfReaderTests, testRobotWithFloatingJoint)
{
RobotDynamics::Model model;
RobotDynamics::ModelPtr model(new RobotDynamics::Model());
std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/floating_joint_robot.urdf";
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), &model, true, false));
EXPECT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
EXPECT_EQ(model.mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
EXPECT_EQ(model.mJoints[2].mJointType, RobotDynamics::JointTypeEulerXYZ);
EXPECT_EQ(model->mJoints[1].mJointType, RobotDynamics::JointTypeTranslationXYZ);
EXPECT_EQ(model->mJoints[2].mJointType, RobotDynamics::JointTypeEulerXYZ);
}
TEST_F(UrdfReaderTests, testRobotSingleBodyFloatingBase)
{
RobotDynamics::Model model;
RobotDynamics::ModelPtr model(new RobotDynamics::Model());
std::string path_to_urdf = ros::package::getPath("rdl_urdfreader") + "/tests/urdf/single_body_floating_base.urdf";
ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), &model, true, false));
ASSERT_TRUE(RobotDynamics::Urdf::urdfReadFromFile(path_to_urdf.c_str(), model, true, false));
}
int main(int argc, char** argv)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment