Commit bb34428c authored by Jordan Lack's avatar Jordan Lack

rework FrameObject to extend Quaternion instead of having a quaternion

parent 6ee3afa9
Pipeline #55542854 canceled with stages
in 6 minutes
......@@ -36,21 +36,6 @@ build_and_test:
- $WS_NAME/
expire_in: 1 day
coverage_rdl_dynamics:
stage: analyze
dependencies:
- build_and_test
script:
- cd $WS_NAME && rosdep install --from-paths src -i -y --as-root apt:false
- catkin build
- catkin run_tests --no-deps rdl_dynamics -DCMAKE_BUILD_TYPE=Coverage --make-args run_coverage_rdl_dynamics
- gcovr -r .
- mv build/rdl_dynamics/coverage $CI_PROJECT_DIR/rdl_dynamics_coverage
artifacts:
name: rdl_dynamics_coverage
paths:
- $CI_PROJECT_DIR/rdl_dynamics_coverage/
coverage_rdl_urdfreader:
stage: analyze
dependencies:
......@@ -81,6 +66,21 @@ coverage_rdl_ros_tools:
paths:
- $CI_PROJECT_DIR/rdl_ros_tools_coverage/
coverage_rdl_dynamics:
stage: analyze
dependencies:
- build_and_test
script:
- cd $WS_NAME && rosdep install --from-paths src -i -y --as-root apt:false
- catkin build
- catkin run_tests --no-deps rdl_dynamics -DCMAKE_BUILD_TYPE=Coverage --make-args run_coverage_rdl_dynamics
- gcovr -r .
- mv build/rdl_dynamics/coverage $CI_PROJECT_DIR/rdl_dynamics_coverage
artifacts:
name: rdl_dynamics_coverage
paths:
- $CI_PROJECT_DIR/rdl_dynamics_coverage/
pages:
stage: pages
dependencies:
......
......@@ -27,7 +27,7 @@ namespace Math
* @ingroup reference_frame
* @brief A Frame object that represents an orientation(quaternion) relative to a reference frame
*/
class FrameOrientation : public FrameObject, Quaternion
class FrameOrientation : public FrameObject, public Quaternion
{
public:
FrameOrientation() : FrameObject(nullptr), Quaternion(0., 0., 0., 1.)
......@@ -45,6 +45,9 @@ class FrameOrientation : public FrameObject, Quaternion
FrameOrientation(ReferenceFramePtr referenceFrame, double x, double y, double z, double w) : FrameObject(referenceFrame), Quaternion(x,y,z,w)
{}
FrameOrientation(ReferenceFrame* referenceFrame, double x, double y, double z, double w) : FrameObject(referenceFrame), Quaternion(x,y,z,w)
{}
/**
*
* @param referenceFrame
......@@ -56,6 +59,11 @@ class FrameOrientation : public FrameObject, Quaternion
{
}
Math::TransformableGeometricObject* getTransformableGeometricObject()
{
return this;
}
void setIncludingFrame(const Quaternion& q, ReferenceFrame* referenceFrame)
{
this->referenceFrame = referenceFrame;
......@@ -84,11 +92,6 @@ class FrameOrientation : public FrameObject, Quaternion
{
changeFrameAndCopy(referenceFrame.get(), frameOrientation);
}
Math::TransformableGeometricObject* getTransformableGeometricObject()
{
return this;
}
};
} // namespace Math
} // namespace RobotDynamics
......
......@@ -20,6 +20,7 @@ namespace RobotDynamics
{
namespace Math
{
/**
* @class Point3
* @brief A generic 3D point
......@@ -51,17 +52,6 @@ class Point3d : public Math::TransformableGeometricObject
{
}
/**
* @brief Create a skew symmetric matrix, m, from a 3d vector such that, given two vectors \f$v_1\f$ and \f$v_2\f$,
* a 3rd vector which is the cross product of the first two is given by, \f$v_3=\tilde{v_1}v_2\f$. The \f$\sim\f$
* operator is referred to in Featherstones RBDA as the 3d vector cross(\f$\times\f$) operator.
* @return A skew symmetric matrix
*/
Matrix3d toTildeForm()
{
return Matrix3d(0., -point[2], point[1], point[2], 0., -point[0], -point[1], point[0], 0.);
}
/**
* @brief Performs in place point transform. Given a point, \f$p\f$, this performs \f$ p = -X.E X.r + X.E p \f$
* @param X
......@@ -350,6 +340,11 @@ inline std::ostream& operator<<(std::ostream& os, const Point3d& point)
os << "x: " << point.x() << '\n' << "y: " << point.y() << '\n' << "z: " << point.z() << "\n";
return os;
}
static inline Matrix3d toTildeForm(const Point3d& p)
{
return Matrix3d(0., -p.z(), p.y(), p.z(), 0., -p.x(), -p.y(), p.x(), 0.);
}
} // namespace Math
} // namespace RobotDynamics
#endif // ifndef __RDL_POINT_3_HPP__
......@@ -22,7 +22,7 @@ namespace Math
*
* order: x,y,z,w
*/
class Quaternion : public Vector4d, TransformableGeometricObject
class Quaternion : public Vector4d, public TransformableGeometricObject
{
public:
/**
......
......@@ -30,11 +30,16 @@ namespace Math
* @param vector
* @return A skew symmetric matrix
*/
inline Matrix3d toTildeForm(const Vector3d& vector)
static inline Matrix3d toTildeForm(const Vector3d& vector)
{
return Matrix3d(0., -vector[2], vector[1], vector[2], 0., -vector[0], -vector[1], vector[0], 0.);
}
static inline Matrix3d toTildeForm(const double x, const double y, const double z)
{
return Matrix3d(0., -z, y, z, 0., -x, -y, z, 0.);
}
inline std::ostream& operator<<(std::ostream& output, const SpatialTransform& X)
{
output << "X.E = " << std::endl << X.E << std::endl;
......
......@@ -366,7 +366,7 @@ namespace RobotDynamics
Math::SpatialMatrix K;
for (unsigned int i = 1; i < model.mBodies.size(); i++)
{
K.block<3,3>(3, 0) = -calcSubtreeCenterOfMassScaledByMass(model, i, q, false).toTildeForm();
K.block<3,3>(3, 0) = -toTildeForm(calcSubtreeCenterOfMassScaledByMass(model, i, q, false));
K.block<3,3>(3, 3) = Math::Matrix3dIdentity * calcSubtreeMass(model, i);
if (model.mJoints[i].mJointType != JointTypeCustom)
{
......
......@@ -31,21 +31,21 @@ TEST_F(FrameOrientationTest, setters)
Quaternion q(1., 2., 3., 4.);
orientation.setOrientation(q);
EXPECT_EQ(orientation.getOrientation().x(), 1.);
EXPECT_EQ(orientation.getOrientation().y(), 2.);
EXPECT_EQ(orientation.getOrientation().z(), 3.);
EXPECT_EQ(orientation.getOrientation().w(), 4.);
orientation.set(q);
EXPECT_EQ(orientation.x(), 1.);
EXPECT_EQ(orientation.y(), 2.);
EXPECT_EQ(orientation.z(), 3.);
EXPECT_EQ(orientation.w(), 4.);
SpatialTransform X = Xrotz(0.1);
Quaternion q2(Quaternion::fromMatrix(X.E));
orientation.setOrientation(X.E);
orientation.set(X.E);
EXPECT_EQ(orientation.getOrientation().x(), Quaternion::fromMatrix(X.E).x());
EXPECT_EQ(orientation.getOrientation().y(), Quaternion::fromMatrix(X.E).y());
EXPECT_EQ(orientation.getOrientation().z(), Quaternion::fromMatrix(X.E).z());
EXPECT_EQ(orientation.getOrientation().w(), Quaternion::fromMatrix(X.E).w());
EXPECT_EQ(orientation.x(), Quaternion::fromMatrix(X.E).x());
EXPECT_EQ(orientation.y(), Quaternion::fromMatrix(X.E).y());
EXPECT_EQ(orientation.z(), Quaternion::fromMatrix(X.E).z());
EXPECT_EQ(orientation.w(), Quaternion::fromMatrix(X.E).w());
}
TEST_F(FrameOrientationTest, changeFrame)
......@@ -61,18 +61,18 @@ TEST_F(FrameOrientationTest, changeFrame)
orientation.changeFrame(&f1);
Quaternion expected(Quaternion::fromMatrix(X2.E));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation.getOrientation(), expected, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f1");
orientation.changeFrame(ReferenceFrame::getWorldFrame());
expected = Quaternion(0., 0., 0., 1.);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation.getOrientation(), expected, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "World");
orientation.changeFrame(&f3);
expected = Quaternion::fromMatrix(Xrotz(M_PI_2).E);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation.getOrientation(), expected, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f3");
}
......@@ -89,7 +89,7 @@ TEST_F(FrameOrientationTest, changeFrameAndCopy)
FrameOrientation orientation_copy = orientation.changeFrameAndCopy(f1);
Quaternion expected(Quaternion::fromMatrix(X2.E));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy.getOrientation(), expected, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f2");
EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "f1");
......@@ -97,14 +97,14 @@ TEST_F(FrameOrientationTest, changeFrameAndCopy)
orientation.changeFrameAndCopy(ReferenceFrame::getWorldFrame(), orientation_copy);
expected = Quaternion(0., 0., 0., 1.);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy.getOrientation(), expected, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "f1");
EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "World");
orientation = orientation_copy;
orientation.changeFrameAndCopy(f3, orientation_copy);
expected = Quaternion::fromMatrix(Xrotz(M_PI_2).E);
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy.getOrientation(), expected, unit_test_utils::TEST_PREC));
EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(orientation_copy, expected, unit_test_utils::TEST_PREC));
EXPECT_STREQ(orientation.getReferenceFrame()->getName().c_str(), "World");
EXPECT_STREQ(orientation_copy.getReferenceFrame()->getName().c_str(), "f3");
}
......@@ -138,10 +138,10 @@ TEST_F(FrameOrientationTest, model)
RobotDynamics::Math::Quaternion q = RobotDynamics::Math::Quaternion::fromMatrix(model.worldFrame->getTransformToDesiredFrame(fixed_body2_frame).E);
EXPECT_NEAR(q.x(), f.getOrientation().x(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.y(), f.getOrientation().y(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.z(), f.getOrientation().z(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.w(), f.getOrientation().w(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.x(), f.x(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.y(), f.y(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.z(), f.z(), unit_test_utils::TEST_PREC);
EXPECT_NEAR(q.w(), f.w(), unit_test_utils::TEST_PREC);
}
int main(int argc, char** argv)
......
......@@ -98,7 +98,7 @@ TEST_F(QuaternionFixture, element_accessors)
EXPECT_EQ(4.1, q1.w());
EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(q1.vec(), Vector3d(1.1, 2.1, 3.1), unit_test_utils::TEST_PREC));
EXPECT_EQ(q1.getScalarPart(), 4.1);
EXPECT_EQ(q1.w(), 4.1);
}
TEST_F(QuaternionFixture, multiplication)
......
......@@ -159,10 +159,10 @@ TEST_F(KinematicsInterfaceTest, change_pose_frame)
rdl_msgs::ChangePoseFrameResponse pose_msg_resp;
pose_msg_req.new_reference_frame = "world";
pose_msg_req.pose_in.header.frame_id = "test_link2";
pose_msg_req.pose_in.pose.orientation.x = o.getOrientation().x();
pose_msg_req.pose_in.pose.orientation.y = o.getOrientation().y();
pose_msg_req.pose_in.pose.orientation.z = o.getOrientation().z();
pose_msg_req.pose_in.pose.orientation.w = o.getOrientation().w();
pose_msg_req.pose_in.pose.orientation.x = o.x();
pose_msg_req.pose_in.pose.orientation.y = o.y();
pose_msg_req.pose_in.pose.orientation.z = o.z();
pose_msg_req.pose_in.pose.orientation.w = o.w();
pose_msg_req.pose_in.pose.position.x = p.x();
pose_msg_req.pose_in.pose.position.y = p.y();
pose_msg_req.pose_in.pose.position.z = p.z();
......@@ -179,10 +179,10 @@ TEST_F(KinematicsInterfaceTest, change_pose_frame)
o.changeFrame(model->worldFrame);
p.changeFrame(model->worldFrame);
EXPECT_NEAR(o.getOrientation().x(), pose_msg_resp.pose_out.pose.orientation.x, 1.e-10);
EXPECT_NEAR(o.getOrientation().y(), pose_msg_resp.pose_out.pose.orientation.y, 1.e-10);
EXPECT_NEAR(o.getOrientation().z(), pose_msg_resp.pose_out.pose.orientation.z, 1.e-10);
EXPECT_NEAR(o.getOrientation().w(), pose_msg_resp.pose_out.pose.orientation.w, 1.e-10);
EXPECT_NEAR(o.x(), pose_msg_resp.pose_out.pose.orientation.x, 1.e-10);
EXPECT_NEAR(o.y(), pose_msg_resp.pose_out.pose.orientation.y, 1.e-10);
EXPECT_NEAR(o.z(), pose_msg_resp.pose_out.pose.orientation.z, 1.e-10);
EXPECT_NEAR(o.w(), pose_msg_resp.pose_out.pose.orientation.w, 1.e-10);
EXPECT_NEAR(p.x(), pose_msg_resp.pose_out.pose.position.x, 1.e-10);
EXPECT_NEAR(p.y(), pose_msg_resp.pose_out.pose.position.y, 1.e-10);
EXPECT_NEAR(p.z(), pose_msg_resp.pose_out.pose.position.z, 1.e-10);
......@@ -268,10 +268,10 @@ TEST_F(KinematicsInterfaceTest, change_orientation_frame)
rdl_msgs::ChangeOrientationFrameResponse o_msg_resp;
o_msg_req.new_reference_frame = "world";
o_msg_req.orientation_in.header.frame_id = "test_link2";
o_msg_req.orientation_in.quaternion.x = o.getOrientation().x();
o_msg_req.orientation_in.quaternion.y = o.getOrientation().y();
o_msg_req.orientation_in.quaternion.z = o.getOrientation().z();
o_msg_req.orientation_in.quaternion.w = o.getOrientation().w();
o_msg_req.orientation_in.quaternion.x = o.x();
o_msg_req.orientation_in.quaternion.y = o.y();
o_msg_req.orientation_in.quaternion.z = o.z();
o_msg_req.orientation_in.quaternion.w = o.w();
ros::ServiceClient o_client = nh.serviceClient<rdl_msgs::ChangeOrientationFrame>("change_frame_orientation");
EXPECT_TRUE(o_client.call(o_msg_req, o_msg_resp));
......@@ -284,10 +284,10 @@ TEST_F(KinematicsInterfaceTest, change_orientation_frame)
o.changeFrame(model->worldFrame);
EXPECT_NEAR(o.getOrientation().x(), o_msg_resp.orientation_out.quaternion.x, 1.e-10);
EXPECT_NEAR(o.getOrientation().y(), o_msg_resp.orientation_out.quaternion.y, 1.e-10);
EXPECT_NEAR(o.getOrientation().z(), o_msg_resp.orientation_out.quaternion.z, 1.e-10);
EXPECT_NEAR(o.getOrientation().w(), o_msg_resp.orientation_out.quaternion.w, 1.e-10);
EXPECT_NEAR(o.x(), o_msg_resp.orientation_out.quaternion.x, 1.e-10);
EXPECT_NEAR(o.y(), o_msg_resp.orientation_out.quaternion.y, 1.e-10);
EXPECT_NEAR(o.z(), o_msg_resp.orientation_out.quaternion.z, 1.e-10);
EXPECT_NEAR(o.w(), o_msg_resp.orientation_out.quaternion.w, 1.e-10);
EXPECT_STREQ(o.getReferenceFrame()->getName().c_str(), "World");
}
......
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