Skip to content

Commit

Permalink
Improve performance by calling Eigen::Transform::linear() instead of …
Browse files Browse the repository at this point in the history 
…Eigen::Transform::rotation().
  • Loading branch information
@peci1
peci1 committed Apr 27, 2020
1 parent ad88e18 commit 0d27388
Show file tree
Hide file tree
Showing 24 changed files with 71 additions and 72 deletions.
2 changes: 1 addition & 1 deletion ...it_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -319,7 +319,7 @@ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f)
#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
f = b.matrix();
#else
Eigen::Quaterniond q(b.rotation());
Eigen::Quaterniond q(b.linear());
f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z()));
f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
#endif
Expand Down
12 changes: 6 additions & 6 deletions moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -479,16 +479,16 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q
Eigen::Isometry3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
quat = Eigen::Quaterniond(reqr.rotation());
Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
quat = Eigen::Quaterniond(reqr.linear());

// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
// model
if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
{
const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
Eigen::Isometry3d rt(t.rotation() * quat);
quat = Eigen::Quaterniond(rt.rotation());
Eigen::Isometry3d rt(t.linear() * quat);
quat = Eigen::Quaterniond(rt.linear());
}
}
else
Expand Down Expand Up @@ -565,7 +565,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq;
point = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
quat = Eigen::Quaterniond(ikq.linear());
}

if (need_eef_to_ik_tip_transform_)
Expand All @@ -574,7 +574,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
ikq = ikq * eef_to_ik_tip_transform_;
point = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
quat = Eigen::Quaterniond(ikq.linear());
}

geometry_msgs::Pose ik_query;
Expand Down
8 changes: 4 additions & 4 deletions moveit_core/distance_field/src/distance_field.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,10 +172,10 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance,
// double angle = -gradient.angle(unitX);
// Eigen::AngleAxisd rotation(angle, axis);

// marker.pose.orientation.x = rotation.rotation().x();
// marker.pose.orientation.y = rotation.rotation().y();
// marker.pose.orientation.z = rotation.rotation().z();
// marker.pose.orientation.w = rotation.rotation().w();
// marker.pose.orientation.x = rotation.linear().x();
// marker.pose.orientation.y = rotation.linear().y();
// marker.pose.orientation.z = rotation.linear().z();
// marker.pose.orientation.w = rotation.linear().w();

marker.scale.x = getResolution();
marker.scale.y = getResolution();
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/dynamics_solver/src/dynamics_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform
{
Eigen::Vector3d p;
p = Eigen::Vector3d(vector.x, vector.y, vector.z);
p = transform.rotation() * p;
p = transform.linear() * p;

geometry_msgs::Vector3 result;
result.x = p.x();
Expand Down
27 changes: 13 additions & 14 deletions moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -391,7 +391,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin)
for (std::size_t j = 0; j < o.constraint_region_.size(); ++j)
{
Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j];
if (diff.translation().norm() < margin && diff.rotation().isIdentity(margin) &&
if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) &&
constraint_region_[i]->getType() == o.constraint_region_[j]->getType() &&
fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin)
{
Expand Down Expand Up @@ -597,16 +597,15 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob
Eigen::Vector3d xyz;
if (mobile_frame_)
{
Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).rotation() * desired_rotation_matrix_;
Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).rotation());
xyz = diff.rotation().eulerAngles(0, 1, 2);
Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_;
Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear());
xyz = diff.linear().eulerAngles(0, 1, 2);
// 0,1,2 corresponds to XYZ, the convention used in sampling constraints
}
else
{
Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).rotation());
xyz =
diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear());
xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
}

xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
Expand All @@ -618,7 +617,7 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob

if (verbose)
{
Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation());
Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear());
Eigen::Quaterniond q_des(desired_rotation_matrix_);
ROS_INFO_NAMED("kinematic_constraints",
"Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion "
Expand Down Expand Up @@ -756,12 +755,12 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin
Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_;
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
if (!diff.linear().isIdentity(margin))
return false;
diff = target_pose_.inverse() * o.target_pose_;
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
if (!diff.linear().isIdentity(margin))
return false;
return true;
}
Expand Down Expand Up @@ -895,7 +894,7 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state,
mka.scale.y = .15;
mka.scale.z = 0.0;
mka.points.resize(2);
Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5;
Eigen::Vector3d d = tp.translation() + tp.linear().col(2) * 0.5;
mka.points[0].x = tp.translation().x();
mka.points[0].y = tp.translation().y();
mka.points[0].z = tp.translation().z();
Expand All @@ -908,7 +907,7 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state,
mka.color.b = 1.0;
mka.color.r = 0.0;

d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5;
d = sp.translation() + sp.linear().col(2 - sensor_view_direction_) * 0.5;
mka.points[0].x = sp.translation().x();
mka.points[0].y = sp.translation().y();
mka.points[0].z = sp.translation().z();
Expand All @@ -932,11 +931,11 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo
mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_;

// necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2
const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_);
const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_);

if (max_view_angle_ > 0.0)
{
const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0; // along Z axis and inverted
const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted
double dp = normal2.dot(normal1);
double ang = acos(dp);
if (dp < 0.0)
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/kinematic_constraints/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -567,7 +567,7 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta
if (c.link_name != robot_link->getName())
{
c.link_name = robot_link->getName();
Eigen::Quaterniond link_name_to_robot_link(transform.linear().inverse() *
Eigen::Quaterniond link_name_to_robot_link(transform.inverse().linear() *
state.getGlobalLinkTransform(robot_link).linear());
Eigen::Quaterniond quat_target;
tf::quaternionMsgToEigen(c.orientation, quat_target);
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/planning_scene/src/planning_scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -966,7 +966,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const
shapes::saveAsText(obj->shapes_[j].get(), out);
out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " "
<< obj->shape_poses_[j].translation().z() << std::endl;
Eigen::Quaterniond r(obj->shape_poses_[j].rotation());
Eigen::Quaterniond r(obj->shape_poses_[j].linear());
out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl;
if (hasObjectColor(id))
{
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/robot_model/src/aabb.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& trans
//
// Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/

const Eigen::Matrix3d& r = transform.rotation();
const Eigen::Matrix3d& r = transform.linear();
const Eigen::Vector3d& t = transform.translation();

double x_range = 0.5 * (fabs(r(0, 0) * box[0]) + fabs(r(0, 1) * box[1]) + fabs(r(0, 2) * box[2]));
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/robot_model/src/floating_joint_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,7 @@ void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& trans
joint_values[0] = transf.translation().x();
joint_values[1] = transf.translation().y();
joint_values[2] = transf.translation().z();
Eigen::Quaterniond q(transf.rotation());
Eigen::Quaterniond q(transf.linear());
joint_values[3] = q.x();
joint_values[4] = q.y();
joint_values[5] = q.z();
Expand Down
4 changes: 2 additions & 2 deletions moveit_core/robot_model/src/link_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ void LinkModel::setJointOriginTransform(const Eigen::Isometry3d& transform)
{
joint_origin_transform_ = transform;
joint_origin_transform_is_identity_ =
joint_origin_transform_.rotation().isIdentity() &&
joint_origin_transform_.linear().isIdentity() &&
joint_origin_transform_.translation().norm() < std::numeric_limits<double>::epsilon();
}

Expand All @@ -83,7 +83,7 @@ void LinkModel::setGeometry(const std::vector<shapes::ShapeConstPtr>& shapes,
for (std::size_t i = 0; i < shapes_.size(); ++i)
{
collision_origin_transform_is_identity_[i] =
(collision_origin_transform_[i].rotation().isIdentity() &&
(collision_origin_transform_[i].linear().isIdentity() &&
collision_origin_transform_[i].translation().norm() < std::numeric_limits<double>::epsilon()) ?
1 :
0;
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/robot_model/src/planar_joint_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,7 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf,
joint_values[0] = transf.translation().x();
joint_values[1] = transf.translation().y();

Eigen::Quaterniond q(transf.rotation());
Eigen::Quaterniond q(transf.linear());
// taken from Bullet
double s_squared = 1.0 - (q.w() * q.w());
if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/robot_model/src/revolute_joint_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -268,7 +268,7 @@ void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Iso

void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
{
Eigen::Quaterniond q(transf.rotation());
Eigen::Quaterniond q(transf.linear());
q.normalize();
size_t max_idx;
axis_.array().abs().maxCoeff(&max_idx);
Expand Down
6 changes: 3 additions & 3 deletions moveit_core/robot_state/src/cartesian_interpolator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link);

// the direction can be in the local reference frame (in which case we rotate it)
const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction;
const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction;

// The target pose is built by applying a translation to the start pose for the desired direction and distance
Eigen::Isometry3d target_pose = start_pose;
Expand Down Expand Up @@ -91,8 +91,8 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
// the target can be in the local reference frame (in which case we rotate it)
Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target;

Eigen::Quaterniond start_quaternion(start_pose.rotation());
Eigen::Quaterniond target_quaternion(rotated_target.rotation());
Eigen::Quaterniond start_quaternion(start_pose.linear());
Eigen::Quaterniond target_quaternion(rotated_target.linear());

if (max_step.translation <= 0.0 && max_step.rotation <= 0.0)
{
Expand Down
10 changes: 5 additions & 5 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1242,15 +1242,15 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
if (pjm->getType() == moveit::core::JointModel::REVOLUTE)
{
joint_transform = reference_transform * getGlobalLinkTransform(link);
joint_axis = joint_transform.rotation() * static_cast<const moveit::core::RevoluteJointModel*>(pjm)->getAxis();
joint_axis = joint_transform.linear() * static_cast<const moveit::core::RevoluteJointModel*>(pjm)->getAxis();
jacobian.block<3, 1>(0, joint_index) =
jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation());
jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis;
}
else if (pjm->getType() == moveit::core::JointModel::PRISMATIC)
{
joint_transform = reference_transform * getGlobalLinkTransform(link);
joint_axis = joint_transform.rotation() * static_cast<const moveit::core::PrismaticJointModel*>(pjm)->getAxis();
joint_axis = joint_transform.linear() * static_cast<const moveit::core::PrismaticJointModel*>(pjm)->getAxis();
jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis;
}
else if (pjm->getType() == moveit::core::JointModel::PLANAR)
Expand Down Expand Up @@ -1279,7 +1279,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
// [x] [ w -z y ] [ omega_2 ]
// [y] [ z w -x ] [ omega_3 ]
// [z] [ -y x w ]
Eigen::Quaterniond q(link_transform.rotation());
Eigen::Quaterniond q(link_transform.linear());
double w = q.w(), x = q.x(), y = q.y(), z = q.z();
Eigen::MatrixXd quaternion_update_matrix(4, 3);
quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w;
Expand Down Expand Up @@ -1836,7 +1836,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::

for (std::size_t i = 0; i < transformed_poses.size(); ++i)
{
Eigen::Quaterniond quat(transformed_poses[i].rotation());
Eigen::Quaterniond quat(transformed_poses[i].linear());
Eigen::Vector3d point(transformed_poses[i].translation());
ik_queries[i].position.x = point.x();
ik_queries[i].position.y = point.y();
Expand Down Expand Up @@ -2101,7 +2101,7 @@ void RobotState::printStateInfo(std::ostream& out) const

void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const
{
Eigen::Quaterniond q(transform.rotation());
Eigen::Quaterniond q(transform.linear());
out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
<< transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
<< "]" << std::endl;
Expand Down
32 changes: 16 additions & 16 deletions moveit_core/robot_state/test/robot_state_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -450,28 +450,28 @@ TEST_F(OneRobot, FK)
state.setVariablePositions(joint_values);

EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0));
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5);
EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5);
EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5);
EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5);
EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5);

EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0));
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5);
EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5);
EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5);

EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0));
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5);
EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5);
EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5);
EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5);
EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5);

EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0));
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5);
EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5);
EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5);
EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5);

EXPECT_TRUE(state.satisfiesBounds());

Expand Down
Loading

0 comments on commit 0d27388

Please sign in to comment.