def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = .1
obj1.shapes[0].dimensions[2] = 2.0
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .9
obj1.poses[0].position.y = -.5
obj1.poses[0].position.z = 1.0
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
self.move_arm_action_client = actionlib.SimpleActionClient("move_irp6p", MoveArmAction)
self.move_arm_action_client.wait_for_server()
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "world"
obj1.id = "obj2";
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 4.0
obj1.shapes[0].dimensions[2] = 0.8
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = 1.0
obj1.poses[0].position.y = -.5
obj1.poses[0].position.z = 0.25
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(1.0)
def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
self.traj_pub = rospy.Publisher('rosie_path',DisplayTrajectory,latch=True)
rospy.wait_for_service("ompl_planning/plan_kinematic_path")
self.motion_planning_service = rospy.ServiceProxy("ompl_planning/plan_kinematic_path", GetMotionPlan)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .9
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(5.0)
def test_add_convert_objects():
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_collision_objects')
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = [
'r_gripper_palm_link', 'r_gripper_r_finger_link',
'r_gripper_l_finger_link', 'r_gripper_r_finger_tip_link',
'r_gripper_l_finger_tip_link', 'r_wrist_roll_link',
'r_wrist_flex_link', 'r_forearm_link',
'r_gripper_motor_accelerometer_link'
]
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .2
obj2.shapes[0].dimensions[1] = .4
obj2.shapes[0].dimensions[2] = .2
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
att_obj.object = obj2
r = rospy.Rate(6)
sent_twice = 0
while (True):
sent_twice += 1
if sent_twice >= 4 and sent_twice % 2 == 0:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
elif sent_twice > 4 and sent_twice % 2 == 1:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
print 'sending'
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def test_add_convert_objects():
att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
rospy.init_node('test_collision_objects')
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2";
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .2
obj2.shapes[0].dimensions[1] = .4
obj2.shapes[0].dimensions[2] = .2
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
att_obj.object = obj2
r = rospy.Rate(6)
sent_twice = 0
while(True):
sent_twice += 1
if sent_twice >= 4 and sent_twice%2 == 0:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
elif sent_twice > 4 and sent_twice%2 == 1:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
print 'sending'
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def setUp(self):
rospy.init_node('test_get_base_state_validity')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
rospy.wait_for_service(default_prefix + '/get_robot_state')
self.get_robot_state_server = rospy.ServiceProxy(
default_prefix + '/get_robot_state', GetRobotState)
rospy.wait_for_service(default_prefix + '/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(
default_prefix + '/get_state_validity', GetStateValidity)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = 'odom_combined'
obj1.id = 'table'
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .1
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = 4.25
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(5.)
def setUp(self):
rospy.init_node('test_motion_execution_buffer')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.traj_pub = rospy.Publisher('rosie_path',
DisplayTrajectory,
latch=True)
rospy.wait_for_service("ompl_planning/plan_kinematic_path")
self.motion_planning_service = rospy.ServiceProxy(
"ompl_planning/plan_kinematic_path", GetMotionPlan)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.CYLINDER
obj1.shapes[0].dimensions = [float() for _ in range(2)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = 1.5
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .9
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(5.0)
def setUp(self):
rospy.init_node('test_get_base_state_validity')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
rospy.wait_for_service(default_prefix+'/get_robot_state')
self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
rospy.wait_for_service(default_prefix+'/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = 'odom_combined'
obj1.id = 'table'
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .1
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = 4.25
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = .75
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(5.)
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
att_pub = rospy.Publisher("attached_collision_object", AttachedCollisionObject)
obj_pub = rospy.Publisher("collision_object", CollisionObject)
rospy.init_node("test_motion_execution_buffer")
# let everything settle down
rospy.sleep(5.0)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(0.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 0.5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = 0.2
obj1.poses[0].position.x = 0.95
obj1.poses[0].position.y = -0.25
obj1.poses[0].position.z = 0.62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = 0.5
obj1.shapes[2].dimensions[1] = 0.1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = 0.95
obj1.poses[2].position.y = -0.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = 0.5
obj1.shapes[1].dimensions[1] = 0.1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = 0.95
obj1.poses[1].position.y = 0.12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = 0.02
att_object.object.shapes[0].dimensions[1] = 0.1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -0.02
att_object.object.poses[0].position.y = 0.04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
def setUp(self):
rospy.init_node('test_allowed_collision_near_start')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
rospy.wait_for_service(default_prefix+'/get_robot_state')
self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
rospy.wait_for_service(default_prefix+'/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
rospy.wait_for_service(default_prefix+'/get_group_info')
self.get_group_info_server = rospy.ServiceProxy(default_prefix+'/get_group_info', GetGroupInfo)
rospy.sleep(3.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "r_gripper_palm_link"
obj1.id = "test_object"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .03
obj1.shapes[0].dimensions[1] = .03
obj1.shapes[0].dimensions[2] = .03
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = 0
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = 0
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.)
# now we put another object in collision with the base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
coll)
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self):
#adding object in collision with base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
# but we can still overwrite
coll = CollisionOperation()
coll.object1 = "base_link"
coll.object2 = coll.COLLISION_SET_OBJECTS
coll.operation = coll.ENABLE
goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break
#should still have succeedeed
final_state = self.move_arm_action_client.get_state()
self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def test_object_collisions(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix+'/'+set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix+'/'+get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix+'/'+revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
obj_pub = rospy.Publisher('collision_object',CollisionObject)
att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
rospy.init_node('test_allowed_collisions')
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(2)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .05
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = 1.0
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .05
obj1.poses = [Pose() for _ in range(2)]
obj1.poses[0].position.x = 1.0
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = .5
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.poses[1].position.x = 1.0
obj1.poses[1].position.y = 0
obj1.poses[1].position.z = .75
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
obj2 = CollisionObject();
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "obj2";
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = 1.0
obj2.shapes[0].dimensions[1] = 1.0
obj2.shapes[0].dimensions[2] = .05
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .15
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = .5
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
obj_pub.publish(obj2)
rospy.sleep(2.0)
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index("r_gripper_palm_link")
except ValueError:
self.fail("No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
obj1_index = get_res.matrix.link_names.index("obj1")
except ValueError:
self.fail("No obj1 in AllowedCollisionMatrix link names")
self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
self.failIf(len(get_res.matrix.entries) <= obj1_index)
self.failIf(len(get_res.matrix.entries[obj1_index].enabled) <= r_gripper_palm_link_index)
self.failIf(len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <= obj1_index)
self.assertEqual(False, get_res.matrix.entries[obj1_index].enabled[r_gripper_palm_link_index])
self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj1_index])
#now disable between all objects
#if enable just this link, it should be true
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest()
set_allowed_request.ord.collision_operations = [CollisionOperation() for _ in range(1)]
set_allowed_request.ord.collision_operations[0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[0].object2 = CollisionOperation.COLLISION_SET_OBJECTS
set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(True, get_res.matrix.entries[obj1_index].enabled[r_gripper_palm_link_index])
self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj1_index])
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
obj_pub = rospy.Publisher('collision_object',CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2";
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(4)]
obj1.poses = [Pose() for _ in range(4)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .2
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = -.25
obj1.poses[1].position.z = .92
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .2
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = .2
obj1.poses[2].position.x = .8
obj1.poses[2].position.y = -.5
obj1.poses[2].position.z = .78
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[3].type = Shape.BOX
obj1.shapes[3].dimensions = [float() for _ in range(3)]
obj1.shapes[3].dimensions[0] = .2
obj1.shapes[3].dimensions[1] = .1
obj1.shapes[3].dimensions[2] = .2
obj1.poses[3].position.x = .8
obj1.poses[3].position.y = .18
obj1.poses[3].position.z = .78
obj1.poses[3].orientation.x = 0
obj1.poses[3].orientation.y = 0
obj1.poses[3].orientation.z = 0
obj1.poses[3].orientation.w = 1
obj_pub.publish(obj1)
rospy.sleep(5.0)
def setUp(self):
rospy.init_node('test_allowed_collision_near_start')
self.tf = TransformListener()
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
rospy.wait_for_service(default_prefix + '/get_robot_state')
self.get_robot_state_server = rospy.ServiceProxy(
default_prefix + '/get_robot_state', GetRobotState)
rospy.wait_for_service(default_prefix + '/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(
default_prefix + '/get_state_validity', GetStateValidity)
rospy.wait_for_service(default_prefix + '/get_group_info')
self.get_group_info_server = rospy.ServiceProxy(
default_prefix + '/get_group_info', GetGroupInfo)
rospy.sleep(3.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "r_gripper_palm_link"
obj1.id = "test_object"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .03
obj1.shapes[0].dimensions[1] = .03
obj1.shapes[0].dimensions[2] = .03
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = 0
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = 0
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
self.obj_pub.publish(obj1)
rospy.sleep(2.)
# now we put another object in collision with the base
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "base_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .1
obj2.shapes[0].dimensions[1] = .1
obj2.shapes[0].dimensions[2] = .1
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = 0
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
self.obj_pub.publish(obj2)
rospy.sleep(5.)
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(4)]
obj1.poses = [Pose() for _ in range(4)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .2
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = -.25
obj1.poses[1].position.z = .92
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .2
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = .2
obj1.poses[2].position.x = .8
obj1.poses[2].position.y = -.5
obj1.poses[2].position.z = .78
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[3].type = Shape.BOX
obj1.shapes[3].dimensions = [float() for _ in range(3)]
obj1.shapes[3].dimensions[0] = .2
obj1.shapes[3].dimensions[1] = .1
obj1.shapes[3].dimensions[2] = .2
obj1.poses[3].position.x = .8
obj1.poses[3].position.y = .18
obj1.poses[3].position.z = .78
obj1.poses[3].orientation.x = 0
obj1.poses[3].orientation.y = 0
obj1.poses[3].orientation.z = 0
obj1.poses[3].orientation.w = 1
obj_pub.publish(obj1)
rospy.sleep(5.0)
def test_object_collisions(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix + '/' + set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix + '/' + get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix + '/' + revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, std_srvs.srv.Empty)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_allowed_collisions')
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1"
obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(2)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = 1.0
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .05
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = 1.0
obj1.shapes[1].dimensions[1] = 1.0
obj1.shapes[1].dimensions[2] = .05
obj1.poses = [Pose() for _ in range(2)]
obj1.poses[0].position.x = 1.0
obj1.poses[0].position.y = 0
obj1.poses[0].position.z = .5
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.poses[1].position.x = 1.0
obj1.poses[1].position.y = 0
obj1.poses[1].position.z = .75
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "obj2"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = 1.0
obj2.shapes[0].dimensions[1] = 1.0
obj2.shapes[0].dimensions[2] = .05
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .15
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = .5
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
obj_pub.publish(obj2)
rospy.sleep(2.0)
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index(
"r_gripper_palm_link")
except ValueError:
self.fail(
"No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
obj1_index = get_res.matrix.link_names.index("obj1")
except ValueError:
self.fail("No obj1 in AllowedCollisionMatrix link names")
self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
self.failIf(len(get_res.matrix.entries) <= obj1_index)
self.failIf(
len(get_res.matrix.entries[obj1_index].enabled) <=
r_gripper_palm_link_index)
self.failIf(
len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <=
obj1_index)
self.assertEqual(
False, get_res.matrix.entries[obj1_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
False, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj1_index])
#now disable between all objects
#if enable just this link, it should be true
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest(
)
set_allowed_request.ord.collision_operations = [
CollisionOperation() for _ in range(1)
]
set_allowed_request.ord.collision_operations[
0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].object2 = CollisionOperation.COLLISION_SET_OBJECTS
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
True, get_res.matrix.entries[obj1_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj1_index])