38#include <gtest/gtest.h>
40#include <urdf_parser/urdf_parser.h>
41#include <tf2_eigen/tf2_eigen.hpp>
42#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
53 geometry_msgs::msg::Pose origin;
54 origin.orientation.w = 1;
55 builder.
addChain(
"base_link->roll",
"revolute", { origin }, urdf::Vector3(1, 0, 0));
56 builder.
addChain(
"roll->pitch",
"revolute", { origin }, urdf::Vector3(0, 1, 0));
57 builder.
addChain(
"pitch->yaw",
"revolute", { origin }, urdf::Vector3(0, 0, 1));
62 std::map<std::string, double>
getJointValues(
const double roll,
const double pitch,
const double yaw)
64 std::map<std::string, double> jvals;
65 jvals[
"base_link-roll-joint"] = roll;
66 jvals[
"roll-pitch-joint"] = pitch;
67 jvals[
"pitch-yaw-joint"] = yaw;
101 robot.
addChain(
"base_link->ee",
"floating");
108 valid_euler_data_ << -0.1712092272140422, -0.2853625129991958, -0.1712092272140422, 0.9273311367620117,
109 -0.28536251299919585, -0.17120922721404216, 0.28536251299919585, 0.8987902273981057, 0.28536251299919585,
110 -0.17120922721404216, -0.28536251299919585, 0.8987902273981057, 0.1712092272140422, -0.2853625129991958,
111 0.1712092272140422, 0.9273311367620117, -0.28536251299919585, 0.17120922721404216, -0.28536251299919585,
112 0.8987902273981057, -0.1712092272140422, 0.2853625129991958, 0.1712092272140422, 0.9273311367620117,
113 0.1712092272140422, 0.2853625129991958, -0.1712092272140422, 0.9273311367620117, 0.28536251299919585,
114 0.17120922721404216, 0.28536251299919585, 0.8987902273981057;
115 valid_rotvec_data_ << -0.23771285949073287, -0.23771285949073287, -0.23771285949073287, 0.911305541132181,
116 -0.2401272988734743, -0.2401272988734743, 0.0, 0.9405731022474852, -0.23771285949073287, -0.23771285949073287,
117 0.23771285949073287, 0.911305541132181, 0.0, -0.24012729887347428, -0.24012729887347428, 0.9405731022474851,
118 0.0, -0.24012729887347428, 0.24012729887347428, 0.9405731022474851, 0.23771285949073287, -0.23771285949073287,
119 -0.23771285949073287, 0.911305541132181, 0.2401272988734743, -0.2401272988734743, 0.0, 0.9405731022474852,
120 0.23771285949073287, -0.23771285949073287, 0.23771285949073287, 0.911305541132181;
131 return Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
132 Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
138 Eigen::Vector3d v{ rx, ry, rz };
139 Eigen::Matrix3d m{ Eigen::AngleAxisd(v.norm(), v.normalized()) };
140 return Eigen::Quaterniond{ m };
146 Eigen::VectorXd joint_values(7);
147 joint_values << 0.0, 0.0, 0.0, quat.x(), quat.y(), quat.z(), quat.w();
157 moveit_msgs::msg::OrientationConstraint ocm;
159 ocm.link_name =
"yaw";
160 ocm.header.frame_id = robot_model_->getModelFrame();
161 ocm.orientation.x = 0.0;
162 ocm.orientation.y = 0.0;
163 ocm.orientation.z = 0.0;
164 ocm.orientation.w = 1.0;
165 ocm.absolute_x_axis_tolerance = 0.8;
166 ocm.absolute_y_axis_tolerance = 0.8;
167 ocm.absolute_z_axis_tolerance = 3.15;
184 moveit_msgs::msg::OrientationConstraint ocm;
186 ocm.link_name =
"yaw";
187 ocm.header.frame_id = robot_model_->getModelFrame();
188 ocm.orientation.x = 0.0;
189 ocm.orientation.y = 0.0;
190 ocm.orientation.z = 0.0;
191 ocm.orientation.w = 1.0;
192 ocm.absolute_x_axis_tolerance = 0.2;
193 ocm.absolute_y_axis_tolerance = 2.0;
194 ocm.absolute_z_axis_tolerance = 0.2;
229 moveit_msgs::msg::OrientationConstraint ocm;
231 ocm.link_name =
"yaw";
232 ocm.header.frame_id = robot_model_->getModelFrame();
233 ocm.orientation.x = 0.0;
234 ocm.orientation.y = 0.0;
235 ocm.orientation.z = 0.0;
236 ocm.orientation.w = 1.0;
237 ocm.absolute_x_axis_tolerance = 0.2;
238 ocm.absolute_y_axis_tolerance = 2.0;
239 ocm.absolute_z_axis_tolerance = 0.3;
256 ocm.absolute_x_axis_tolerance = 0.3;
257 ocm.absolute_y_axis_tolerance = 2.0;
258 ocm.absolute_z_axis_tolerance = 0.2;
277 moveit_msgs::msg::OrientationConstraint ocm;
279 ocm.link_name =
"yaw";
280 ocm.header.frame_id = robot_model_->getModelFrame();
281 ocm.orientation.x = 0.0;
282 ocm.orientation.y = 0.0;
283 ocm.orientation.z = 0.0;
284 ocm.orientation.w = 1.0;
285 ocm.absolute_x_axis_tolerance = 0.2;
286 ocm.absolute_y_axis_tolerance = 2.0;
287 ocm.absolute_z_axis_tolerance = 0.3;
304 ocm.absolute_x_axis_tolerance = 0.3;
305 ocm.absolute_y_axis_tolerance = 2.0;
306 ocm.absolute_z_axis_tolerance = 0.2;
331 moveit_msgs::msg::OrientationConstraint ocm;
337 ocm.link_name =
"yaw";
338 ocm.header.frame_id = robot_model_->getModelFrame();
339 ocm.orientation = tf2::toMsg(Eigen::Quaterniond(robot_state.
getGlobalLinkTransform(ocm.link_name).linear()));
340 ocm.absolute_x_axis_tolerance = 0.0;
341 ocm.absolute_y_axis_tolerance = 0.0;
342 ocm.absolute_z_axis_tolerance = 1.0;
358 moveit_msgs::msg::OrientationConstraint ocm;
359 ocm.link_name =
"ee";
360 ocm.header.frame_id = robot_model_->getModelFrame();
361 ocm.orientation.y = 0.0;
362 ocm.orientation.z = 0.0;
363 ocm.orientation.w = 1.0;
364 ocm.absolute_x_axis_tolerance = 0.5;
365 ocm.absolute_y_axis_tolerance = 0.5;
366 ocm.absolute_z_axis_tolerance = 0.5;
370 ocm.parameterization = 99;
394 moveit_msgs::msg::OrientationConstraint ocm;
395 ocm.link_name =
"ee";
396 ocm.header.frame_id = robot_model_->getModelFrame();
397 ocm.orientation = p.orientation;
398 ocm.absolute_x_axis_tolerance = 0.5;
399 ocm.absolute_y_axis_tolerance = 0.5;
400 ocm.absolute_z_axis_tolerance = 0.5;
405 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES;
406 EXPECT_TRUE(oc_euler.
configure(ocm, tf));
410 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
411 EXPECT_TRUE(oc_rotvec.
configure(ocm, tf));
431 Eigen::VectorXd joint_values(7);
432 joint_values << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
433 for (Eigen::Index i_row{ 0 }; i_row < valid_euler_data_.rows(); ++i_row)
435 joint_values[3] = valid_euler_data_(i_row, 0);
436 joint_values[4] = valid_euler_data_(i_row, 1);
437 joint_values[5] = valid_euler_data_(i_row, 2);
438 joint_values[6] = valid_euler_data_(i_row, 3);
444 joint_values[3] = valid_rotvec_data_(i_row, 0);
445 joint_values[4] = valid_rotvec_data_(i_row, 1);
446 joint_values[5] = valid_rotvec_data_(i_row, 2);
447 joint_values[6] = valid_rotvec_data_(i_row, 3);
459 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES;
461 EXPECT_TRUE(oc_euler.
configure(ocm, tf));
465 EXPECT_TRUE(oc_euler.
configure(ocm, tf));
468 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
470 EXPECT_TRUE(oc_rotvec.
configure(ocm, tf));
474 EXPECT_TRUE(oc_rotvec.
configure(ocm, tf));
480 testing::InitGoogleTest(&argc, argv);
481 return RUN_ALL_TESTS();
Eigen::Matrix< double, 8, 4 > valid_rotvec_data_
moveit::core::RobotModelPtr robot_model_
Eigen::Matrix< double, 8, 4 > valid_euler_data_
moveit::core::RobotModelPtr robot_model_
std::map< std::string, double > getJointValues(const double roll, const double pitch, const double yaw)
Class for constraints on the orientation of a link.
bool configure(const moveit_msgs::msg::OrientationConstraint &oc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::OrientationConstraint.
int getParameterizationType() const
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
void addChain(const std::string §ion, const std::string &type, const std::vector< geometry_msgs::msg::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
bool isValid()
Returns true if the building process so far has been valid.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void addGroupChain(const std::string &base_link, const std::string &tip_link, const std::string &name="")
Adds a new group using a chain of links. The group is the parent joint of each link in the chain.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
bool satisfied
Whether or not the constraint or constraints were satisfied.
TEST_F(SphericalRobot, Test1)
int main(int argc, char **argv)
Eigen::Quaterniond rotationVectorToQuaternion(double rx, double ry, double rz)
Eigen::Quaterniond xyzIntrinsixToQuaternion(double rx, double ry, double rz)
void setRobotEndEffectorOrientation(moveit::core::RobotState &robot_state, const Eigen::Quaterniond &quat)