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 = arm_navigation_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 = .6
        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(2.0)
예제 #2
0
 def run (self):
  
   coll_obj = CollisionObject()
   coll_obj.operation.operation = CollisionObjectOperation.ADD
   coll_obj.id = self.name_of_the_target_object
   coll_obj.header = rospy.Header()
   coll_obj.header.frame_id = self.frame_id
   coll_obj.poses =  [self.pose_of_the_target_object]
   coll_obj.shapes = [self.shape]
   coll_obj.padding = self.padding
   pub = rospy.Publisher('/collision_object',CollisionObject,latch=True)
   
   rospy.loginfo('Thread publishing on /collision_object topic (%s)',self.name_of_the_target_object)
   
   while (rospy.is_shutdown() == False) and (self.end == False):
     
     coll_obj.header.stamp = rospy.Time.now()
     try:
       pub.publish(coll_obj)
     except Exception, e:
       
       rospy.logerr("Error on publishing to /collision_object topic: %s", e)
       return 0
       
     rospy.logdebug('Thread is looping')
     rospy.sleep(2)
def get_virtual_table(height = 0.42):
    table_msg = CollisionObject()

    table_msg.id = "table"
    table_msg.operation.operation = CollisionObjectOperation.ADD    

    table_msg.header.stamp = rospy.get_rostime()
    table_msg.header.frame_id = "base_footprint"
    
    side_box = Shape()
    side_box.type = Shape.BOX
    side_box.dimensions = [ 3.0, 1.0, height ]

    front_box = Shape()
    front_box.type = Shape.BOX
    front_box.dimensions = [ 1.0, 3.0, height ]

    pose = Pose()
    pose.position.x = 0.0
    pose.position.y = 0.0
    pose.position.z = height / 2
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
    pose.orientation.w = 1

    l_side_pose = copy.deepcopy(pose)
    l_side_pose.position.y = 0.85

    r_side_pose = copy.deepcopy(pose)
    r_side_pose.position.y = -0.85

    front_pose = copy.deepcopy(pose)
    front_pose.position.x = 0.85

    table_msg.shapes = [ side_box, side_box, front_box ]
    table_msg.poses = [ l_side_pose, r_side_pose, front_pose ]

    return table_msg
    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 = arm_navigation_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 = .6
        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(2.0)
    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 = arm_navigation_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 = arm_navigation_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 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 = arm_navigation_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 = arm_navigation_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)
예제 #8
0
def test_add_convert_objects():

    obj_pub = rospy.Publisher('collision_object',CollisionObject)
    att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)

    rospy.init_node('test_collision_objects')

    #let everything settle down
    rospy.sleep(5.)
      
    obj1 = CollisionObject()
    
    obj1.header.stamp = rospy.Time.now()
    obj1.header.frame_id = "base_link"
    obj1.id = "obj1";
    obj1.operation.operation = arm_navigation_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] = .75
    obj1.poses = [Pose() for _ in range(1)]
    obj1.poses[0].position.x = .6
    obj1.poses[0].position.y = -.6
    obj1.poses[0].position.z = .375
    obj1.poses[0].orientation.x = 0
    obj1.poses[0].orientation.y = 0
    obj1.poses[0].orientation.z = 0
    obj1.poses[0].orientation.w = 1

    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 = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
    obj2.shapes = [Shape() for _ in range(1)]
    obj2.shapes[0].type = Shape.CYLINDER
    obj2.shapes[0].dimensions = [float() for _ in range(2)]
    obj2.shapes[0].dimensions[0] = .025
    obj2.shapes[0].dimensions[1] = .5
    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(.1)

    while(True):
        obj1.header.stamp = rospy.Time.now()
        obj_pub.publish(obj1)
        att_obj.object.header.stamp = rospy.Time.now()
        att_pub.publish(att_obj)
        r.sleep()
예제 #9
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 = arm_navigation_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)
예제 #10
0
    def add_spatula(self, arm):
        spatula = CollisionObject()
        spatula.id = "spatula"
        spatula.header.frame_id = self.wi.world_frame
        spatula.operation.operation = spatula.operation.ADD

        paddle = Shape()
        handle = Shape()
        paddle.type = paddle.BOX
        paddle.dimensions = [0.11, 0.12, 0.005]
        handle.type = handle.CYLINDER
        handle.dimensions = [0.02, 0.24]

        paddle_pose = Pose()
        handle_pose = Pose()
        paddle_pose.position.y = paddle.dimensions[1] / 2.0
        paddle_pose.orientation.w = 1.0

        angle = np.pi / 5.0
        handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
            np.pi / 2.0 - angle)
        handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
            np.pi / 2.0 - angle)
        handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
        handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)

        spatula.shapes = [paddle, handle]
        spatula.poses = [paddle_pose, handle_pose]

        #this is the grasp transformation
        pos_on_handle = handle.dimensions[1] - 0.1
        inv_grasp = Transform()
        grasp = RigidGrasp()
        #really should be calculating this...
        inv_grasp.translation.y = GRIPPER_LENGTH
        inv_grasp.translation.z = pos_on_handle / 2.0
        #flip 90 degrees
        inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
        inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
        g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
        origin = Pose()
        origin.orientation.w = 1.0
        grasp.transform = pose_to_transform(
            gt.inverse_transform_pose(origin, g))
        grasp.touch_links = [arm[0] + '_end_effector']
        grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
        grasp.min_approach_distance = 0
        grasp.desired_approach_distance = 0.15
        grasp.min_distance_from_surface = -1

        spat_p = copy.deepcopy(spatula)

        wtrans = Pose()
        wtrans.orientation.x = np.sin(angle / 2.0)
        wtrans.orientation.w = np.cos(angle / 2.0)
        if self.world == -1:
            wtrans.position.x = 3
            wtrans.position.y = -2.8
            wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['far_corner']
        elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
            wtrans.position.x = -1.7
            wtrans.position.y = 2
            wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['door_table']
        else:
            wtrans.position.x = 0.6
            wtrans.position.y = -0.3
            wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
                0]
            ss = ['center_table']
            if self.world == -4 or self.world == -5:
                wtrans.position.y = 0
                rot = Quaternion()
                rot.z = np.sin(np.pi / 2.0)
                rot.w = np.cos(np.pi / 2.0)
                wtrans.orientation = gt.multiply_quaternions(
                    rot, wtrans.orientation)
                if self.world == -5:
                    wtrans.position.x = 0

        for i in range(len(spat_p.poses)):
            spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)

        self.wi.add_object(spat_p)
        return ObjectType(type="SpatulaObject",
                          collision_object=spatula,
                          parameters=ss,
                          numeric_parameters=paddle.dimensions +
                          handle.dimensions + [angle]), [grasp]
    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 = arm_navigation_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 = arm_navigation_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)        
예제 #12
0
def get_obj():

    attached_obj = AttachedCollisionObject()
    obj = CollisionObject()
    shape = Shape()
    pose = Pose()
    vert = Point()
    verts = []

    in_verts = True
    in_position = True

    f = open('test.txt', 'r')
    for line in f:
        fields = line.split(':')
        fields = [fields[i].strip() for i in range(len(fields))]
        #        print fields
        if fields[0] == "frame_id":
            obj.header.frame_id = fields[1]
        elif fields[0] == "id":
            obj.id = fields[1]
        elif fields[0] == "operation" and fields[1] != "":
            obj.operation.operation = int(fields[1])
        elif fields[0] == "type":
            shape.type = int(fields[1])
        elif fields[0] == "triangles":
            array = fields[1][1:-2]
            ind = array.split(',')
            inds = [int(i) for i in ind]
            shape.triangles = inds
        elif fields[0] == "x" and in_verts:
            vert = Point()
            vert.x = float(fields[1])
        elif fields[0] == "y" and in_verts:
            vert.y = float(fields[1])
        elif fields[0] == "z" and in_verts:
            vert.z = float(fields[1])
            verts.append(vert)
        elif fields[0] == "poses":
            in_verts = False
        elif fields[0] == "x" and in_position:
            pose.position.x = float(fields[1])
        elif fields[0] == "y" and in_position:
            pose.position.y = float(fields[1])
        elif fields[0] == "z" and in_position:
            pose.position.z = float(fields[1])
            in_position = False
        elif fields[0] == "x" and not in_position:
            pose.orientation.x = float(fields[1])
        elif fields[0] == "y" and not in_position:
            pose.orientation.y = float(fields[1])
        elif fields[0] == "z" and not in_position:
            pose.orientation.z = float(fields[1])
        elif fields[0] == "w":
            pose.orientation.w = float(fields[1])

        obj.id = "graspable_object_1001"
        shape.vertices = verts
        obj.shapes = [shape]
        obj.poses = [pose]

    attached_obj.object = obj
    return attached_obj
    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 = arm_navigation_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)