43 #include <moveit_msgs/DisplayTrajectory.h> 45 #include <moveit_resources/config.h> 48 #include <visualization_msgs/MarkerArray.h> 50 #include <gtest/gtest.h> 51 #include <urdf_parser/urdf_parser.h> 53 #include <boost/bind.hpp> 54 #include <boost/filesystem/path.hpp> 77 boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
81 std::fstream xml_file((res_path /
"pr2_description/urdf/robot.xml").
string().c_str(), std::fstream::in);
82 if (xml_file.is_open())
84 while (xml_file.good())
87 std::getline(xml_file, line);
88 xml_string += (line +
"\n");
109 std::map<std::string, robot_model::SolverAllocatorFn> allocators;
115 kmodel->setKinematicsAllocators(allocators);
128 planning_scene::PlanningScenePtr
ps;
144 moveit_msgs::JointConstraint jcm1;
146 jcm1.position = 0.42;
147 jcm1.tolerance_above = 0.01;
148 jcm1.tolerance_below = 0.05;
152 std::vector<kinematic_constraints::JointConstraint> js;
160 jcm1.joint_name =
"r_shoulder_pan_joint";
168 for (
int t = 0; t < 100; ++t)
180 moveit_msgs::JointConstraint jcm2;
181 jcm2.joint_name =
"r_shoulder_pan_joint";
182 jcm2.position = 0.54;
183 jcm2.tolerance_above = 0.01;
184 jcm2.tolerance_below = 0.01;
206 jcm1.tolerance_above = .05;
207 jcm1.tolerance_below = .05;
209 jcm2.tolerance_above = .1;
210 jcm2.tolerance_below = .1;
219 for (
int t = 0; t < 100; ++t)
228 jcm1.position = -3.1;
229 jcm1.tolerance_above = .05;
230 jcm1.tolerance_below = .05;
240 jcm1.tolerance_above = .05;
241 jcm1.tolerance_below = .05;
243 jcm2.tolerance_above = .05;
244 jcm2.tolerance_below = .05;
252 for (
int t = 0; t < 100; ++t)
255 std::map<std::string, double> var_values;
269 for (
int t = 0; t < 100; ++t)
282 moveit_msgs::PositionConstraint pcm;
284 pcm.link_name =
"l_wrist_roll_link";
285 pcm.target_point_offset.x = 0;
286 pcm.target_point_offset.y = 0;
287 pcm.target_point_offset.z = 0;
288 pcm.constraint_region.primitives.resize(1);
289 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
290 pcm.constraint_region.primitives[0].dimensions.resize(1);
291 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
293 pcm.constraint_region.primitive_poses.resize(1);
294 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
295 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
296 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
297 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
298 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
299 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
300 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
314 pcm.header.frame_id =
kmodel->getModelFrame();
324 pcm.link_name =
"l_shoulder_pan_link";
330 pcm.link_name =
"l_wrist_roll_link";
352 moveit_msgs::OrientationConstraint ocm;
354 ocm.link_name =
"r_wrist_roll_link";
355 ocm.header.frame_id = ocm.link_name;
356 ocm.orientation.x = 0.5;
357 ocm.orientation.y = 0.5;
358 ocm.orientation.z = 0.5;
359 ocm.orientation.w = 0.5;
360 ocm.absolute_x_axis_tolerance = 0.01;
361 ocm.absolute_y_axis_tolerance = 0.01;
362 ocm.absolute_z_axis_tolerance = 0.01;
370 ocm.header.frame_id =
kmodel->getModelFrame();
375 for (
int t = 0; t < 100; ++t)
395 moveit_msgs::PositionConstraint pcm;
397 pcm.link_name =
"l_wrist_roll_link";
398 pcm.target_point_offset.x = 0;
399 pcm.target_point_offset.y = 0;
400 pcm.target_point_offset.z = 0;
401 pcm.constraint_region.primitives.resize(1);
402 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
403 pcm.constraint_region.primitives[0].dimensions.resize(1);
404 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
406 pcm.header.frame_id =
kmodel->getModelFrame();
408 pcm.constraint_region.primitive_poses.resize(1);
409 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
410 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
411 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
412 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
413 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
414 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
415 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
421 moveit_msgs::OrientationConstraint ocm;
423 ocm.link_name =
"l_wrist_roll_link";
424 ocm.header.frame_id =
kmodel->getModelFrame();
425 ocm.orientation.x = 0.0;
426 ocm.orientation.y = 0.0;
427 ocm.orientation.z = 0.0;
428 ocm.orientation.w = 1.0;
429 ocm.absolute_x_axis_tolerance = 0.2;
430 ocm.absolute_y_axis_tolerance = 0.1;
431 ocm.absolute_z_axis_tolerance = 0.4;
438 for (
int t = 0; t < 100; ++t)
447 for (
int t = 0; t < 100; ++t)
455 for (
int t = 0; t < 100; ++t)
476 std::map<std::string, double> state_values;
478 moveit_msgs::JointConstraint torso_constraint;
479 torso_constraint.joint_name =
"torso_lift_joint";
481 torso_constraint.tolerance_above = 0.01;
482 torso_constraint.tolerance_below = 0.01;
483 torso_constraint.weight = 1.0;
487 moveit_msgs::JointConstraint jcm2;
488 jcm2.joint_name =
"r_elbow_flex_joint";
490 jcm2.tolerance_above = 0.01;
491 jcm2.tolerance_below = 0.01;
495 moveit_msgs::PositionConstraint pcm;
497 pcm.link_name =
"l_wrist_roll_link";
498 pcm.target_point_offset.x = 0;
499 pcm.target_point_offset.y = 0;
500 pcm.target_point_offset.z = 0;
501 pcm.constraint_region.primitives.resize(1);
502 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
503 pcm.constraint_region.primitives[0].dimensions.resize(1);
504 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
506 pcm.constraint_region.primitive_poses.resize(1);
507 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
508 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
509 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
510 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
511 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
512 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
513 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
516 pcm.header.frame_id =
kmodel->getModelFrame();
518 moveit_msgs::OrientationConstraint ocm;
520 ocm.link_name =
"l_wrist_roll_link";
521 ocm.header.frame_id =
kmodel->getModelFrame();
522 ocm.orientation.x = 0.0;
523 ocm.orientation.y = 0.0;
524 ocm.orientation.z = 0.0;
525 ocm.orientation.w = 1.0;
526 ocm.absolute_x_axis_tolerance = 0.2;
527 ocm.absolute_y_axis_tolerance = 0.1;
528 ocm.absolute_z_axis_tolerance = 0.4;
531 std::vector<kinematic_constraints::JointConstraint> js;
534 constraint_samplers::JointConstraintSamplerPtr jcsp(
538 std::vector<kinematic_constraints::JointConstraint> js2;
554 std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
555 cspv.push_back(jcsp2);
556 cspv.push_back(iksp);
557 cspv.push_back(jcsp);
570 EXPECT_EQ(jcs2->getJointModelGroup()->getName(),
"arms");
572 for (
int t = 0; t < 100; ++t)
584 pcm.link_name =
"r_wrist_roll_link";
585 ocm.link_name =
"r_wrist_roll_link";
600 cspv.push_back(iksp2);
601 cspv.push_back(iksp);
607 ASSERT_TRUE(ikcs_test);
611 pcm.link_name =
"l_wrist_roll_link";
612 ocm.link_name =
"l_wrist_roll_link";
613 pcm.header.frame_id =
"r_wrist_roll_link";
614 ocm.header.frame_id =
"r_wrist_roll_link";
620 cspv.push_back(iksp2);
621 cspv.push_back(iksp);
641 moveit_msgs::PositionConstraint pcm;
642 pcm.link_name =
"l_wrist_roll_link";
643 pcm.target_point_offset.x = 0;
644 pcm.target_point_offset.y = 0;
645 pcm.target_point_offset.z = 0;
646 pcm.constraint_region.primitives.resize(1);
647 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
648 pcm.constraint_region.primitives[0].dimensions.resize(1);
649 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
651 pcm.header.frame_id =
kmodel->getModelFrame();
653 pcm.constraint_region.primitive_poses.resize(1);
654 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
655 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
656 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
657 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
658 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
659 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
660 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
663 moveit_msgs::OrientationConstraint ocm;
665 ocm.link_name =
"l_wrist_roll_link";
666 ocm.header.frame_id =
kmodel->getModelFrame();
667 ocm.orientation.x = 0.0;
668 ocm.orientation.y = 0.0;
669 ocm.orientation.z = 0.0;
670 ocm.orientation.w = 1.0;
671 ocm.absolute_x_axis_tolerance = 0.2;
672 ocm.absolute_y_axis_tolerance = 0.1;
673 ocm.absolute_z_axis_tolerance = 0.4;
677 moveit_msgs::Constraints c;
678 c.position_constraints.push_back(pcm);
679 c.orientation_constraints.push_back(ocm);
681 constraint_samplers::ConstraintSamplerPtr
s =
689 static const int NT = 100;
691 for (
int t = 0; t < NT; ++t)
696 if (s->sample(ks, ks_const, 1))
699 ROS_INFO(
"Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf",
700 (
double)succ / (
double)NT);
703 ocm.absolute_x_axis_tolerance = 0.1;
705 c.orientation_constraints.push_back(ocm);
722 moveit_msgs::Constraints con;
723 con.joint_constraints.resize(1);
725 con.joint_constraints[0].joint_name =
"l_shoulder_pan_joint";
726 con.joint_constraints[0].position = 0.54;
727 con.joint_constraints[0].tolerance_above = 0.01;
728 con.joint_constraints[0].tolerance_below = 0.01;
729 con.joint_constraints[0].weight = 1.0;
731 constraint_samplers::ConstraintSamplerPtr
s =
737 con.joint_constraints.resize(7);
739 con.joint_constraints[1].joint_name =
"l_shoulder_lift_joint";
740 con.joint_constraints[1].position = 0.54;
741 con.joint_constraints[1].tolerance_above = 0.01;
742 con.joint_constraints[1].tolerance_below = 0.01;
743 con.joint_constraints[1].weight = 1.0;
745 con.joint_constraints[2].joint_name =
"l_upper_arm_roll_joint";
746 con.joint_constraints[2].position = 0.54;
747 con.joint_constraints[2].tolerance_above = 0.01;
748 con.joint_constraints[2].tolerance_below = 0.01;
749 con.joint_constraints[2].weight = 1.0;
751 con.joint_constraints[3].joint_name =
"l_elbow_flex_joint";
752 con.joint_constraints[3].position = -0.54;
753 con.joint_constraints[3].tolerance_above = 0.01;
754 con.joint_constraints[3].tolerance_below = 0.01;
755 con.joint_constraints[3].weight = 1.0;
757 con.joint_constraints[4].joint_name =
"l_forearm_roll_joint";
758 con.joint_constraints[4].position = 0.54;
759 con.joint_constraints[4].tolerance_above = 0.01;
760 con.joint_constraints[4].tolerance_below = 0.01;
761 con.joint_constraints[4].weight = 1.0;
763 con.joint_constraints[5].joint_name =
"l_wrist_flex_joint";
764 con.joint_constraints[5].position = -0.54;
765 con.joint_constraints[5].tolerance_above = 0.05;
766 con.joint_constraints[5].tolerance_below = 0.05;
767 con.joint_constraints[5].weight = 1.0;
770 con.joint_constraints[6].joint_name =
"l_wrist_flex_joint";
771 con.joint_constraints[6].position = -0.56;
772 con.joint_constraints[6].tolerance_above = 0.01;
773 con.joint_constraints[6].tolerance_below = 0.01;
774 con.joint_constraints[6].weight = 1.0;
779 con.position_constraints.resize(1);
782 con.position_constraints[0].link_name =
"r_wrist_roll_link";
783 con.position_constraints[0].target_point_offset.x = 0;
784 con.position_constraints[0].target_point_offset.y = 0;
785 con.position_constraints[0].target_point_offset.z = 0;
786 con.position_constraints[0].constraint_region.primitives.resize(1);
787 con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
788 con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
789 con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
791 con.position_constraints[0].header.frame_id =
kmodel->getModelFrame();
793 con.position_constraints[0].constraint_region.primitive_poses.resize(1);
794 con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
795 con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
796 con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
797 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
798 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
799 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
800 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
801 con.position_constraints[0].weight = 1.0;
810 con.position_constraints[0].link_name =
"l_wrist_roll_link";
823 con.orientation_constraints.resize(1);
826 con.orientation_constraints[0].link_name =
"r_wrist_roll_link";
827 con.orientation_constraints[0].header.frame_id =
kmodel->getModelFrame();
828 con.orientation_constraints[0].orientation.x = 0.0;
829 con.orientation_constraints[0].orientation.y = 0.0;
830 con.orientation_constraints[0].orientation.z = 0.0;
831 con.orientation_constraints[0].orientation.w = 1.0;
832 con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
833 con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
834 con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
835 con.orientation_constraints[0].weight = 1.0;
850 con.orientation_constraints[0].link_name =
"l_wrist_roll_link";
862 con.position_constraints[0].link_name =
"r_wrist_roll_link";
864 ASSERT_TRUE(static_cast<bool>(s));
872 con.joint_constraints.resize(8);
873 con.joint_constraints[7].joint_name =
"l_wrist_roll_joint";
874 con.joint_constraints[7].position = 0.54;
875 con.joint_constraints[7].tolerance_above = 0.01;
876 con.joint_constraints[7].tolerance_below = 0.01;
877 con.joint_constraints[7].weight = 1.0;
880 EXPECT_TRUE(static_cast<bool>(s));
894 moveit_msgs::Constraints con;
895 con.joint_constraints.resize(1);
897 con.joint_constraints[0].joint_name =
"torso_lift_joint";
899 con.joint_constraints[0].tolerance_above = 0.01;
900 con.joint_constraints[0].tolerance_below = 0.01;
901 con.joint_constraints[0].weight = 1.0;
906 con.position_constraints.resize(1);
908 con.position_constraints[0].link_name =
"l_wrist_roll_link";
909 con.position_constraints[0].target_point_offset.x = 0;
910 con.position_constraints[0].target_point_offset.y = 0;
911 con.position_constraints[0].target_point_offset.z = 0;
912 con.position_constraints[0].constraint_region.primitives.resize(1);
913 con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
914 con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
915 con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
917 con.position_constraints[0].constraint_region.primitive_poses.resize(1);
918 con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
919 con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
920 con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
921 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
922 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
923 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
924 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
925 con.position_constraints[0].weight = 1.0;
927 con.position_constraints[0].header.frame_id =
kmodel->getModelFrame();
929 con.orientation_constraints.resize(1);
930 con.orientation_constraints[0].link_name =
"l_wrist_roll_link";
931 con.orientation_constraints[0].header.frame_id =
kmodel->getModelFrame();
932 con.orientation_constraints[0].orientation.x = 0.0;
933 con.orientation_constraints[0].orientation.y = 0.0;
934 con.orientation_constraints[0].orientation.z = 0.0;
935 con.orientation_constraints[0].orientation.w = 1.0;
936 con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
937 con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
938 con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
939 con.orientation_constraints[0].weight = 1.0;
941 constraint_samplers::ConstraintSamplerPtr
s =
950 ASSERT_TRUE(ikcs_test);
952 for (
int t = 0; t < 1; ++t)
971 moveit_msgs::JointConstraint jcm1;
972 jcm1.joint_name =
"head_pan_joint";
973 jcm1.position = 0.42;
974 jcm1.tolerance_above = 0.01;
975 jcm1.tolerance_below = 0.05;
980 moveit_msgs::JointConstraint jcm2;
981 jcm2.joint_name =
"l_shoulder_pan_joint";
983 jcm2.tolerance_above = 0.1;
984 jcm2.tolerance_below = 0.05;
989 moveit_msgs::JointConstraint jcm3;
990 jcm3.joint_name =
"r_wrist_roll_joint";
992 jcm3.tolerance_above = 0.14;
993 jcm3.tolerance_below = 0.005;
998 moveit_msgs::JointConstraint jcm4;
999 jcm4.joint_name =
"torso_lift_joint";
1000 jcm4.position = 0.2;
1001 jcm4.tolerance_above = 0.09;
1002 jcm4.tolerance_below = 0.01;
1006 std::vector<kinematic_constraints::JointConstraint> js;
1017 for (
int t = 0; t < 100; ++t)
1025 moveit_msgs::Constraints c;
1028 constraint_samplers::ConstraintSamplerPtr s0 =
1033 c.joint_constraints.push_back(jcm1);
1034 c.joint_constraints.push_back(jcm2);
1035 c.joint_constraints.push_back(jcm3);
1036 c.joint_constraints.push_back(jcm4);
1038 constraint_samplers::ConstraintSamplerPtr
s =
1043 for (
int t = 0; t < 1000; ++t)
1053 moveit_msgs::Constraints c;
1055 moveit_msgs::PositionConstraint pcm;
1056 pcm.link_name =
"l_wrist_roll_link";
1057 pcm.target_point_offset.x = 0;
1058 pcm.target_point_offset.y = 0;
1059 pcm.target_point_offset.z = 0;
1061 pcm.constraint_region.primitives.resize(1);
1062 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
1063 pcm.constraint_region.primitives[0].dimensions.resize(1);
1064 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
1066 pcm.header.frame_id =
kmodel->getModelFrame();
1068 pcm.constraint_region.primitive_poses.resize(1);
1069 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
1070 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
1071 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
1072 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
1073 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
1074 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
1075 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
1077 c.position_constraints.push_back(pcm);
1079 moveit_msgs::OrientationConstraint ocm;
1080 ocm.link_name =
"l_wrist_roll_link";
1081 ocm.header.frame_id =
kmodel->getModelFrame();
1082 ocm.orientation.x = 0.0;
1083 ocm.orientation.y = 0.0;
1084 ocm.orientation.z = 0.0;
1085 ocm.orientation.w = 1.0;
1086 ocm.absolute_x_axis_tolerance = 0.2;
1087 ocm.absolute_y_axis_tolerance = 0.1;
1088 ocm.absolute_z_axis_tolerance = 0.4;
1090 c.orientation_constraints.push_back(ocm);
1092 ocm.link_name =
"r_wrist_roll_link";
1093 ocm.header.frame_id =
kmodel->getModelFrame();
1094 ocm.orientation.x = 0.0;
1095 ocm.orientation.y = 0.0;
1096 ocm.orientation.z = 0.0;
1097 ocm.orientation.w = 1.0;
1098 ocm.absolute_x_axis_tolerance = 0.01;
1099 ocm.absolute_y_axis_tolerance = 0.01;
1100 ocm.absolute_z_axis_tolerance = 0.01;
1102 c.orientation_constraints.push_back(ocm);
1105 constraint_samplers::ConstraintSamplerPtr
s =
1122 static const int NT = 100;
1124 for (
int t = 0; t < NT; ++t)
1128 if (s->sample(ks, ks_const, 1))
1132 "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf",
1133 (
double)succ / (
double)NT);
1138 testing::InitGoogleTest(&argc, argv);
1140 return RUN_ALL_TESTS();
planning_scene::PlanningScenePtr ps
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_
robot_model::RobotModelPtr kmodel
bool configure(const moveit_msgs::OrientationConstraint &oc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::OrientationConstraint.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
#define ROS_INFO_NAMED(name,...)
Class for constraints on the orientation of a link.
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
const std::string & getName() const
Get the name of the joint group.
Class for handling single DOF joint constraints.
A class that contains many different constraints, and can check RobotState *versus the full set...
int main(int argc, char **argv)
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces a sample from all configured samplers.
This class exists as a union of constraint samplers. It contains a vector of constraint samplers...
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
robot_model::SolverAllocatorFn func_left_arm
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
#define EXPECT_NEAR(a, b, prec)
kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup *jmg)
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
std::size_t getUnconstrainedJointCount() const
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits...
std::size_t getConstrainedJointCount() const
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_
bool satisfied
Whether or not the constraint or constraints were satisfied.
#define EXPECT_FALSE(args)
void update(bool force=false)
Update all transforms.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
const robot_model::JointModelGroup * getJointModelGroup() const
Gets the joint model group.
TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
urdf::ModelInterfaceSharedPtr urdf_model
boost::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> SolverAllocatorFn
Function type that allocates a kinematics solver for a particular group.
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
A class that allows the sampling of IK constraints.
robot_model::SolverAllocatorFn func_right_arm
def xml_string(rootXml, addHeader=True)
Class for constraints on the XYZ position of a link.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &ks, unsigned int max_attempts)
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
srdf::ModelSharedPtr srdf_model
static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::Constraints &constr)
Default logic to select a ConstraintSampler given a constraints message.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
#define EXPECT_TRUE(args)
kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup *jmg)
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
const std::vector< ConstraintSamplerPtr > & getSamplers() const
Gets the sorted internal list of constraint samplers.