test_check_state_validity_in_empty_scene.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import sys
4 import rospy
5 import unittest
6 from actionlib import SimpleActionClient
7 import moveit_commander
8 from moveit_msgs.msg import Constraints
9 from moveit_msgs.srv import GetStateValidity
10 
11 
12 class TestCheckStateValidityInEmptyScene(unittest.TestCase):
13  def setUp(self):
14  moveit_commander.roscpp_initialize(sys.argv)
15  self.robot_commander = moveit_commander.RobotCommander()
16  self.group_name = self.robot_commander.get_group_names()[0]
17  self.group_commander = moveit_commander.MoveGroupCommander(self.group_name)
18 
19  self._check_state_validity = rospy.ServiceProxy(
20  "/check_state_validity", GetStateValidity
21  )
22 
24  current_robot_state = self.robot_commander.get_current_state()
25 
26  validity_report = self._check_state_validity(
27  current_robot_state, self.group_name, Constraints()
28  )
29  self.assertListEqual(
30  validity_report.contacts,
31  [],
32  "In the default_robot_state, the robot should not collide with itself",
33  )
34 
36  current_robot_state = self.robot_commander.get_current_state()
37 
38  # force a colliding state with the Fanuc M-10iA
39  current_robot_state.joint_state.position = list(
40  current_robot_state.joint_state.position
41  )
42  current_robot_state.joint_state.position[
43  current_robot_state.joint_state.name.index("joint_3")
44  ] = -2
45 
46  validity_report = self._check_state_validity(
47  current_robot_state, self.group_name, Constraints()
48  )
49 
50  self.assertNotEqual(
51  len(validity_report.contacts),
52  0,
53  "When the robot collides with itself, it should have some contacts (with itself)",
54  )
55 
56 
57 if __name__ == "__main__":
58  import rostest
59 
60  rospy.init_node("check_state_validity_in_empty_scene")
61  rostest.rosrun(
62  "moveit_ros_move_group",
63  "test_check_state_validity_in_empty_scene",
64  TestCheckStateValidityInEmptyScene,
65  )
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene.group_commander
group_commander
Definition: test_check_state_validity_in_empty_scene.py:17
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene.test_check_collision_free_state_validity_in_empty_scene
def test_check_collision_free_state_validity_in_empty_scene(self)
Definition: test_check_state_validity_in_empty_scene.py:23
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene.group_name
group_name
Definition: test_check_state_validity_in_empty_scene.py:16
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene.robot_commander
robot_commander
Definition: test_check_state_validity_in_empty_scene.py:15
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene._check_state_validity
_check_state_validity
Definition: test_check_state_validity_in_empty_scene.py:19
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene.test_check_colliding_state_validity_in_empty_scene
def test_check_colliding_state_validity_in_empty_scene(self)
Definition: test_check_state_validity_in_empty_scene.py:35
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene
Definition: test_check_state_validity_in_empty_scene.py:12
test_check_state_validity_in_empty_scene.TestCheckStateValidityInEmptyScene.setUp
def setUp(self)
Definition: test_check_state_validity_in_empty_scene.py:13


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon May 27 2024 02:27:46