6 from actionlib
import SimpleActionClient
7 import moveit_commander
8 from moveit_msgs.msg
import Constraints
9 from moveit_msgs.srv
import GetStateValidity
15 moveit_commander.roscpp_initialize(sys.argv)
17 self.
group_name = self.robot_commander.get_group_names()[0]
23 current_robot_state = self.robot_commander.get_current_state()
26 self.assertListEqual(validity_report.contacts, [],
"In the default_robot_state, the robot should not collide with itself")
30 current_robot_state = self.robot_commander.get_current_state()
33 current_robot_state.joint_state.position = list(current_robot_state.joint_state.position)
34 current_robot_state.joint_state.position[current_robot_state.joint_state.name.index(
"joint_3")] = -2
38 self.assertNotEqual(len(validity_report.contacts), 0,
"When the robot collides with itself, it should have some contacts (with itself)")
41 if __name__ ==
'__main__':
43 rospy.init_node(
'check_state_validity_in_empty_scene')
44 rostest.rosrun(
'moveit_ros_move_group',
'test_check_state_validity_in_empty_scene', TestCheckStateValidityInEmptyScene)
def test_check_colliding_state_validity_in_empty_scene(self)
def test_check_collision_free_state_validity_in_empty_scene(self)