6 from actionlib
import SimpleActionClient
7 import moveit_commander
8 from moveit_msgs.msg
import Constraints
9 from moveit_msgs.srv
import GetStateValidity
14 moveit_commander.roscpp_initialize(sys.argv)
20 "/check_state_validity", GetStateValidity
27 current_robot_state, self.
group_name, Constraints()
30 validity_report.contacts,
32 "In the default_robot_state, the robot should not collide with itself",
39 current_robot_state.joint_state.position = list(
40 current_robot_state.joint_state.position
42 current_robot_state.joint_state.position[
43 current_robot_state.joint_state.name.index(
"joint_3")
47 current_robot_state, self.
group_name, Constraints()
51 len(validity_report.contacts),
53 "When the robot collides with itself, it should have some contacts (with itself)",
57 if __name__ ==
"__main__":
60 rospy.init_node(
"check_state_validity_in_empty_scene")
62 "moveit_ros_move_group",
63 "test_check_state_validity_in_empty_scene",
64 TestCheckStateValidityInEmptyScene,