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])