Exemple #1
0
    def __init__(self):
        rospy.loginfo("waiting for server add_compound")
        rospy.wait_for_service('add_compound')
        self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
        rospy.loginfo("connected to add_compound service")
        add_compound_request = AddCompoundRequest()
        add_compound_request.remove = rospy.get_param('~remove', False)

        # scale everything by this factor
        scale = rospy.get_param("~scale", 10.0)

        # make a table
        table_height = 0.0
        if rospy.get_param("~ground", False):
            rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
            table_thickness = 0.1
            table = Body()
            table.name = "table"
            # make the floor static
            table.mass = 0.0
            table.pose.orientation.x = rot90[0]
            table.pose.orientation.y = rot90[1]
            table.pose.orientation.z = rot90[2]
            table.pose.orientation.w = rot90[3]
            table.pose.position.z = (table_height - table_thickness / 2) * scale
            table.type = Body.CYLINDER
            table.scale.x = 1.0 * scale
            table.scale.y = table_thickness * scale
            table.scale.z = 1.0 * scale
            add_compound_request.body.append(table)

        # make a sphere to grasp
        if rospy.get_param("~rigid_ball", False):
            ball_radius = 0.04 * scale
            ball = Body()
            ball.name = "ball"
            ball.mass = 0.3 * scale
            ball.pose.orientation.x = rot90[0]
            ball.pose.orientation.y = rot90[1]
            ball.pose.orientation.z = rot90[2]
            ball.pose.orientation.w = rot90[3]
            ball.pose.position.z = (table_height + ball_radius + 0.01) * scale
            ball.type = Body.SPHERE
            ball.scale.x = ball_radius
            ball.scale.y = ball_radius
            ball.scale.z = ball_radius
            add_compound_request.body.append(ball)

        # Platform arm is attached to
        arm_base_height = table_height + 0.93
        rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
        arm_base_thickness = 0.02
        arm_base = Body()
        arm_base.name = "arm_base"
        # make the platform static
        arm_base.mass = 0.0
        arm_base.pose.orientation.x = rot90[0]
        arm_base.pose.orientation.y = rot90[1]
        arm_base.pose.orientation.z = rot90[2]
        arm_base.pose.orientation.w = rot90[3]
        arm_base.pose.position.z = arm_base_height * scale
        arm_base.type = Body.CYLINDER
        arm_base.scale.x = 0.1 * scale
        arm_base.scale.y = arm_base_thickness * scale
        arm_base.scale.z = 0.1 * scale
        add_compound_request.body.append(arm_base)

        if True:
            thickness = 0.08
            cyl_length = 0.5
            arm_upper = Body()
            # arm_upper = copy.deepcopy(arm_fore)
            arm_upper.name = "arm_upper"
            arm_upper.mass = 1.0
            arm_upper.pose.position.x = 0.0
            arm_upper.pose.position.y = 0.0
            rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
            arm_upper.pose.position.z = (arm_base_height - cyl_length / 2.0) * scale
            arm_upper.pose.orientation.x = rot90[0]
            arm_upper.pose.orientation.y = rot90[1]
            arm_upper.pose.orientation.z = rot90[2]
            arm_upper.pose.orientation.w = rot90[3]
            arm_upper.type = Body.CYLINDER
            arm_upper.scale.x = thickness / 2.0 * scale
            arm_upper.scale.y = cyl_length / 2.0 * scale
            arm_upper.scale.z = thickness / 2.0 * scale
            arm_upper.friction = 0.7
            add_compound_request.body.append(arm_upper)

            # connect the cylinders to the bottom plate with p2p ball socket
            # joints.
            constraint = Constraint()
            constraint.name = "arm_upper_fixed"
            constraint.body_a = "arm_base"
            constraint.body_b = arm_upper.name
            constraint.type = Constraint.FIXED
            # Need to transform arm_fore.pose into bot_plate frame
            # to get these
            constraint.pivot_in_a.x = 0.0
            constraint.pivot_in_a.y = 0.0
            constraint.pivot_in_a.z = 0.0
            constraint.pivot_in_b.x = 0
            constraint.pivot_in_b.y = cyl_length / 2.0 * scale
            constraint.pivot_in_b.z = 0
            constraint.disable_collisions_between_linked_bodies = True
            add_compound_request.constraint.append(constraint)

            arm_fore = Body()
            arm_fore.name = "arm_fore"
            arm_fore.mass = 1.0
            arm_fore.pose.orientation.x = rot90[0]
            arm_fore.pose.orientation.y = rot90[1]
            arm_fore.pose.orientation.z = rot90[2]
            arm_fore.pose.orientation.w = rot90[3]
            arm_fore.pose.position.x = 0.0
            arm_fore.pose.position.y = 0.0
            arm_fore.pose.position.z = (arm_base_height - cyl_length) * scale
            arm_fore.type = Body.CYLINDER
            arm_fore.scale.x = thickness * 0.4 * scale
            arm_fore.scale.y = cyl_length / 2.0 * scale
            arm_fore.scale.z = thickness * 0.4 * scale
            arm_fore.friction = 0.7
            add_compound_request.body.append(arm_fore)

            # connect each top cylinder to paired bottom cylinder with slider constraint
            prismatic = Constraint()
            prismatic.name = "prismatic_upper_fore"
            prismatic.body_a = arm_upper.name
            prismatic.body_b = arm_fore.name
            prismatic.type = Constraint.SLIDER
            prismatic.enable_pos_pub = True
            prismatic.enable_motor_sub = True
            prismatic.pivot_in_a.x = 0.0
            prismatic.pivot_in_a.y = -cyl_length / 4.0 * scale
            prismatic.pivot_in_a.z = 0.0
            prismatic.pivot_in_b.x = 0.0
            prismatic.pivot_in_b.y = 0.0  # cyl_length / 4.0
            prismatic.pivot_in_b.z = 0.0
            prismatic.lower_lin_lim = 0.0
            prismatic.upper_lin_lim = cyl_length * scale
            # TODO(lucasw) is this an absolute angle or rate?
            prismatic.lower_ang_lim = -0.1
            prismatic.upper_ang_lim = 0.1
            prismatic.max_motor_impulse = 2800.0
            prismatic.disable_collisions_between_linked_bodies = True
            add_compound_request.constraint.append(prismatic)

        # fingers gotta fing
        num_fingers = 3
        for i in range(num_fingers):
            finger_thickness = thickness / 4.0
            finger_length = 0.05
            finger_upper = Body()
            finger_upper.name = "finger_upper_" + str(i)
            finger_upper.mass = 0.5
            finger_upper.pose.orientation.x = rot90[0]
            finger_upper.pose.orientation.y = rot90[1]
            finger_upper.pose.orientation.z = rot90[2]
            finger_upper.pose.orientation.w = rot90[3]
            angle = float(i) / num_fingers * 2.0 * math.pi
            finger_upper.pose.position.x = thickness / 2.0 * math.cos(angle) * scale
            finger_upper.pose.position.y = thickness / 2.0 * math.sin(angle) * scale
            finger_upper.pose.position.z = (arm_base_height - cyl_length * 1.56) * scale
            finger_upper.type = Body.BOX
            finger_upper.scale.x = finger_thickness / 2.0 * scale
            finger_upper.scale.y = finger_length / 2.0 * scale
            finger_upper.scale.z = finger_thickness / 2.0 * scale
            finger_upper.friction = 0.7
            add_compound_request.body.append(finger_upper)

	    finger_joint = Constraint()
	    finger_joint.name = "finger_joint_" + str(i)
	    finger_joint.body_a = "arm_fore"
	    finger_joint.body_b = finger_upper.name
	    finger_joint.type = Constraint.HINGE
	    finger_joint.lower_ang_lim = -0.9
	    finger_joint.upper_ang_lim = 0.6
	    finger_joint.max_motor_impulse = 2000.0
	    finger_joint.pivot_in_a.x = (thickness / 2.0 * math.cos(angle)) * scale
	    finger_joint.pivot_in_a.z = (-thickness / 2.0 * math.sin(angle)) * scale
	    finger_joint.pivot_in_a.y = -cyl_length * 0.52 * scale
	    finger_joint.axis_in_a.x = -math.cos(angle + math.pi / 2.0)
	    finger_joint.axis_in_a.z = math.sin(angle + math.pi / 2.0)
	    finger_joint.axis_in_a.y = 0.0
	    finger_joint.pivot_in_b.x = 0.0
	    finger_joint.pivot_in_b.y = finger_length * 0.5 * scale
	    finger_joint.pivot_in_b.z = 0.0
	    finger_joint.axis_in_b.x = 1.0
	    finger_joint.axis_in_b.y = 0.0
	    finger_joint.axis_in_b.z = 0.0
	    finger_joint.enable_pos_pub = True
	    finger_joint.enable_motor_sub = True
            finger_joint.disable_collisions_between_linked_bodies = True
	    add_compound_request.constraint.append(finger_joint)

            finger_lower = copy.deepcopy(finger_upper)
            finger_lower.name = "finger_lower_" + str(i)
            finger_lower.mass = 0.5
            finger_lower.pose.position.z = (arm_base_height - cyl_length * 1.56 - finger_length) * scale
            finger_lower.scale.y = finger_length * 0.4 * scale
            finger_lower.scale.z = finger_thickness / 3.0 * scale
            finger_lower.friction = 1.9
            add_compound_request.body.append(finger_lower)

	    finger_lower_joint = copy.deepcopy(finger_joint)
	    finger_lower_joint.name = "finger_lower_joint_" + str(i)
	    finger_lower_joint.body_a = finger_upper.name
	    finger_lower_joint.body_b = finger_lower.name
	    finger_lower_joint.type = Constraint.HINGE
	    finger_lower_joint.lower_ang_lim = -0.5
	    finger_lower_joint.upper_ang_lim = 0.9
	    finger_lower_joint.max_motor_impulse = 2000.0
	    finger_lower_joint.pivot_in_a.x = 0.0
	    finger_lower_joint.pivot_in_a.z = 0.0
	    finger_lower_joint.pivot_in_a.y = -finger_length * 0.5 * scale
	    finger_lower_joint.axis_in_a.x = 1.0
	    finger_lower_joint.axis_in_a.y = 0.0
	    finger_lower_joint.axis_in_a.z = 0.0
	    finger_lower_joint.axis_in_a.y = 0.0
	    finger_joint.pivot_in_b.y = finger_length * 0.4 * scale
	    finger_lower_joint.enable_pos_pub = True
	    finger_lower_joint.enable_motor_sub = True
	    add_compound_request.constraint.append(finger_lower_joint)

        try:
            add_compound_response = self.add_compound(add_compound_request)
            rospy.loginfo(add_compound_response)
        except rospy.service.ServiceException as e:
            rospy.logerr(e)
    def __init__(self):
        rospy.loginfo("waiting for server add_compound")
        rospy.wait_for_service('add_compound')
        self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
        rospy.loginfo("connected to service")
        add_compound_request = AddCompoundRequest()
        add_compound_request.remove = rospy.get_param('~remove', False)

        floor = 0.0
        radius = 1.0
        height = radius * 2
        rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
        thickness = 0.1
        # make the bottom cylinder plate
        bot_plate = Body()
        bot_plate.name = "bottom_plate"
        # make the plate static
        bot_plate.mass = 0.0
        bot_plate.pose.orientation.x = rot90[0]
        bot_plate.pose.orientation.y = rot90[1]
        bot_plate.pose.orientation.z = rot90[2]
        bot_plate.pose.orientation.w = rot90[3]
        bot_plate.pose.position.z = floor + 0.2
        bot_plate.type = Body.CYLINDER
        bot_plate.scale.x = radius
        bot_plate.scale.y = thickness
        bot_plate.scale.z = radius
        add_compound_request.body.append(bot_plate)

        # make the top cylinder plate
        top_plate = Body()
        top_plate.name = "top_plate"
        top_plate.mass = 0.5
        top_plate.pose.orientation.x = rot90[0]
        top_plate.pose.orientation.y = rot90[1]
        top_plate.pose.orientation.z = rot90[2]
        top_plate.pose.orientation.w = rot90[3]
        top_plate.pose.position.z = floor + 1.7
        top_plate.type = Body.CYLINDER
        top_plate.scale.x = radius
        top_plate.scale.y = thickness
        top_plate.scale.z = radius
        add_compound_request.body.append(top_plate)

        # make six actuator cylinder bottoms with TBD spacing in a circle
        for i in range(6):
            bot_cylinder = Body()
            bot_cylinder.name = "bot_cylinder_" + str(i)
            bot_cylinder.mass = 0.3
            rot90 = tf.transformations.quaternion_from_euler(-math.pi/2.0, 0, 0)
            bot_cylinder.pose.orientation.x = rot90[0]
            bot_cylinder.pose.orientation.y = rot90[1]
            bot_cylinder.pose.orientation.z = rot90[2]
            bot_cylinder.pose.orientation.w = rot90[3]
            if i % 2 == 0:
                angle = i / 6.0 * 2.0 * math.pi - 0.15
            else:
                angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
            bot_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
            bot_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
            bot_cylinder.pose.position.z = floor + 0.2 + height/6.0 * 0.5 + 0.3
            bot_cylinder.type = Body.CYLINDER
            bot_cylinder.scale.x = thickness / 2.0
            bot_cylinder.scale.y = height / 4.0
            bot_cylinder.scale.z = thickness / 2.0
            add_compound_request.body.append(bot_cylinder)

            # connect the cylinders to the bottom plate with p2p ball socket
            # joints.
            constraint = Constraint()
            constraint.name = "bot_cylinder_p2p_" + str(i)
            constraint.body_a = "bottom_plate"
            constraint.body_b = bot_cylinder.name
            constraint.type = Constraint.POINT2POINT
            # Need to transform bot_cylinder.pose into bot_plate frame
            # to get these
            constraint.pivot_in_a.x = bot_cylinder.pose.position.x
            constraint.pivot_in_a.z = -bot_cylinder.pose.position.y
            constraint.pivot_in_a.y = bot_cylinder.pose.position.z - 0.5
            constraint.pivot_in_b.x = 0
            constraint.pivot_in_b.y = bot_cylinder.scale.y + 0.05
            constraint.pivot_in_b.z = 0
            add_compound_request.constraint.append(constraint)

            # make six actuator cylinder tops
            top_cylinder = copy.deepcopy(bot_cylinder)
            top_cylinder.name = "top_cylinder_" + str(i)
            top_cylinder.mass = 0.3
            if i % 2 == 0:
                angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
            else:
                angle = (i) / 6.0 * 2.0 * math.pi - 0.15
            top_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
            top_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
            top_cylinder.pose.position.z = bot_cylinder.pose.position.z + 0.6
            top_cylinder.scale.x = thickness / 2.3
            top_cylinder.scale.y = height / 7.0
            top_cylinder.scale.z = thickness / 2.3
            add_compound_request.body.append(top_cylinder)

            # connect each top cylinder to paired bottom cylinder with slider constraint
            prismatic = Constraint()
            prismatic.name = "prismatic_" + str(i)
            prismatic.body_a = bot_cylinder.name
            prismatic.body_b = top_cylinder.name
            prismatic.type = Constraint.SLIDER
            prismatic.enable_pos_pub = True
            prismatic.enable_motor_sub = True
            prismatic.pivot_in_a.x = 0
            prismatic.pivot_in_a.y = -0.1
            prismatic.pivot_in_a.z = 0
            prismatic.pivot_in_b.x = 0
            prismatic.pivot_in_b.y = 0.1
            prismatic.pivot_in_b.z = 0
            prismatic.lower_lin_lim = 0.0
            prismatic.upper_lin_lim = 0.3
            # TODO(lucasw) is this an absolute angle or rate?
            prismatic.lower_ang_lim = -0.1
            prismatic.upper_ang_lim = 0.1
            prismatic.max_motor_impulse = 5000.0
            add_compound_request.constraint.append(prismatic)

            # connect the top cylinders with p2p joints to top plate
            constraint = Constraint()
            constraint.name = "tot_cylinder_p2p_" + str(i)
            constraint.body_a = "top_plate"
            constraint.body_b = top_cylinder.name
            constraint.type = Constraint.POINT2POINT
            # Need to transform bot_cylinder.pose into bot_plate frame
            # to get these
            constraint.pivot_in_a.x = top_cylinder.pose.position.x
            constraint.pivot_in_a.z = -top_cylinder.pose.position.y
            constraint.pivot_in_a.y = -0.2
            constraint.pivot_in_b.x = 0
            constraint.pivot_in_b.y = -0.2
            constraint.pivot_in_b.z = 0
            add_compound_request.constraint.append(constraint)

        try:
            add_compound_response = self.add_compound(add_compound_request)
            rospy.loginfo(add_compound_response)
        except rospy.service.ServiceException as e:
            rospy.logerr(e)
    def __init__(self):
        rospy.wait_for_service('add_compound')
        self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
        add_compound_request = AddCompoundRequest()
        add_compound_request.remove = rospy.get_param('~remove', False)

        floor = 0.0
        radius = 1.0
        height = radius * 2
        rot90 = tf.transformations.quaternion_from_euler(math.pi / 2.0, 0, 0)
        thickness = 0.1
        # make the bottom cylinder plate
        bot_plate = Body()
        bot_plate.name = "bottom_plate"
        # make the plate static
        bot_plate.mass = 0.0
        bot_plate.pose.orientation.x = rot90[0]
        bot_plate.pose.orientation.y = rot90[1]
        bot_plate.pose.orientation.z = rot90[2]
        bot_plate.pose.orientation.w = rot90[3]
        bot_plate.pose.position.z = floor + 0.2
        bot_plate.type = Body.CYLINDER
        bot_plate.scale.x = radius
        bot_plate.scale.y = thickness
        bot_plate.scale.z = radius
        add_compound_request.body.append(bot_plate)

        # make the top cylinder plate
        top_plate = Body()
        top_plate.name = "top_plate"
        top_plate.mass = 0.5
        top_plate.pose.orientation.x = rot90[0]
        top_plate.pose.orientation.y = rot90[1]
        top_plate.pose.orientation.z = rot90[2]
        top_plate.pose.orientation.w = rot90[3]
        top_plate.pose.position.z = floor + 1.7
        top_plate.type = Body.CYLINDER
        top_plate.scale.x = radius
        top_plate.scale.y = thickness
        top_plate.scale.z = radius
        add_compound_request.body.append(top_plate)

        # make six actuator cylinder bottoms with TBD spacing in a circle
        for i in range(6):
            bot_cylinder = Body()
            bot_cylinder.name = "bot_cylinder_" + str(i)
            bot_cylinder.mass = 0.3
            rot90 = tf.transformations.quaternion_from_euler(
                -math.pi / 2.0, 0, 0)
            bot_cylinder.pose.orientation.x = rot90[0]
            bot_cylinder.pose.orientation.y = rot90[1]
            bot_cylinder.pose.orientation.z = rot90[2]
            bot_cylinder.pose.orientation.w = rot90[3]
            if i % 2 == 0:
                angle = i / 6.0 * 2.0 * math.pi - 0.15
            else:
                angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
            bot_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
            bot_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
            bot_cylinder.pose.position.z = floor + 0.2 + height / 6.0 * 0.5 + 0.3
            bot_cylinder.type = Body.CYLINDER
            bot_cylinder.scale.x = thickness / 2.0
            bot_cylinder.scale.y = height / 4.0
            bot_cylinder.scale.z = thickness / 2.0
            add_compound_request.body.append(bot_cylinder)

            # connect the cylinders to the bottom plate with p2p ball socket
            # joints.
            constraint = Constraint()
            constraint.name = "bot_cylinder_p2p_" + str(i)
            constraint.body_a = "bottom_plate"
            constraint.body_b = bot_cylinder.name
            constraint.type = Constraint.POINT2POINT
            # Need to transform bot_cylinder.pose into bot_plate frame
            # to get these
            constraint.pivot_in_a.x = bot_cylinder.pose.position.x
            constraint.pivot_in_a.z = -bot_cylinder.pose.position.y
            constraint.pivot_in_a.y = bot_cylinder.pose.position.z - 0.5
            constraint.pivot_in_b.x = 0
            constraint.pivot_in_b.y = bot_cylinder.scale.y + 0.05
            constraint.pivot_in_b.z = 0
            add_compound_request.constraint.append(constraint)

            # make six actuator cylinder tops
            top_cylinder = copy.deepcopy(bot_cylinder)
            top_cylinder.name = "top_cylinder_" + str(i)
            top_cylinder.mass = 0.3
            if i % 2 == 0:
                angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
            else:
                angle = (i) / 6.0 * 2.0 * math.pi - 0.15
            top_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
            top_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
            top_cylinder.pose.position.z = bot_cylinder.pose.position.z + 0.6
            top_cylinder.scale.x = thickness / 2.3
            top_cylinder.scale.y = height / 7.0
            top_cylinder.scale.z = thickness / 2.3
            add_compound_request.body.append(top_cylinder)

            # connect each top cylinder to paired bottom cylinder with slider constraint
            prismatic = Constraint()
            prismatic.name = "prismatic_" + str(i)
            prismatic.body_a = bot_cylinder.name
            prismatic.body_b = top_cylinder.name
            prismatic.type = Constraint.SLIDER
            prismatic.pivot_in_a.x = 0
            prismatic.pivot_in_a.y = -0.1
            prismatic.pivot_in_a.z = 0
            prismatic.pivot_in_b.x = 0
            prismatic.pivot_in_b.y = 0.1
            prismatic.pivot_in_b.z = 0
            prismatic.lower_lin_lim = 0.0
            prismatic.upper_lin_lim = 0.3
            # TODO(lucasw) is this an absolute angle or rate?
            prismatic.lower_ang_lim = -0.1
            prismatic.upper_ang_lim = 0.1
            prismatic.max_motor_impulse = 5000.0
            add_compound_request.constraint.append(prismatic)

            # connect the top cylinders with p2p joints to top plate
            constraint = Constraint()
            constraint.name = "tot_cylinder_p2p_" + str(i)
            constraint.body_a = "top_plate"
            constraint.body_b = top_cylinder.name
            constraint.type = Constraint.POINT2POINT
            # Need to transform bot_cylinder.pose into bot_plate frame
            # to get these
            constraint.pivot_in_a.x = top_cylinder.pose.position.x
            constraint.pivot_in_a.z = -top_cylinder.pose.position.y
            constraint.pivot_in_a.y = -0.2
            constraint.pivot_in_b.x = 0
            constraint.pivot_in_b.y = -0.2
            constraint.pivot_in_b.z = 0
            add_compound_request.constraint.append(constraint)

        try:
            add_compound_response = self.add_compound(add_compound_request)
            rospy.loginfo(add_compound_response)
        except rospy.service.ServiceException as e:
            rospy.logerr(e)
Exemple #4
0
    def __init__(self):
        rospy.loginfo("waiting for server add_compound")
        rospy.wait_for_service('add_compound')
        self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
        rospy.loginfo("connected to add_compound service")
        add_compound_request = AddCompoundRequest()
        add_compound_request.remove = rospy.get_param('~remove', False)

        # scale everything by this factor
        scale = rospy.get_param("~scale", 10.0)

        # make a table
        table_height = 0.0
        if rospy.get_param("~ground", False):
            rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
            table_thickness = 0.1
            table = Body()
            table.name = "table"
            # make the floor static
            table.mass = 0.0
            table.pose.orientation.x = rot90[0]
            table.pose.orientation.y = rot90[1]
            table.pose.orientation.z = rot90[2]
            table.pose.orientation.w = rot90[3]
            table.pose.position.z = (table_height - table_thickness / 2) * scale
            table.type = Body.CYLINDER
            table.scale.x = 1.0 * scale
            table.scale.y = table_thickness * scale
            table.scale.z = 1.0 * scale
            add_compound_request.body.append(table)

        # make a sphere to grasp
        if rospy.get_param("~rigid_ball", False):
            ball_radius = 0.04 * scale
            ball = Body()
            ball.name = "ball"
            ball.mass = 0.3 * scale
            ball.pose.orientation.x = rot90[0]
            ball.pose.orientation.y = rot90[1]
            ball.pose.orientation.z = rot90[2]
            ball.pose.orientation.w = rot90[3]
            ball.pose.position.z = (table_height + ball_radius + 0.01) * scale
            ball.type = Body.SPHERE
            ball.scale.x = ball_radius
            ball.scale.y = ball_radius
            ball.scale.z = ball_radius
            add_compound_request.body.append(ball)

        # Platform arm is attached to
        arm_base_height = table_height + 0.93
        rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
        arm_base_thickness = 0.02
        arm_base = Body()
        arm_base.name = "arm_base"
        # make the platform static
        arm_base.mass = 0.0
        arm_base.pose.orientation.x = rot90[0]
        arm_base.pose.orientation.y = rot90[1]
        arm_base.pose.orientation.z = rot90[2]
        arm_base.pose.orientation.w = rot90[3]
        arm_base.pose.position.z = arm_base_height * scale
        arm_base.type = Body.CYLINDER
        arm_base.scale.x = 0.1 * scale
        arm_base.scale.y = arm_base_thickness * scale
        arm_base.scale.z = 0.1 * scale
        add_compound_request.body.append(arm_base)

        if True:
            thickness = 0.08
            cyl_length = 0.5
            arm_upper = Body()
            # arm_upper = copy.deepcopy(arm_fore)
            arm_upper.name = "arm_upper"
            arm_upper.mass = 1.0
            arm_upper.pose.position.x = 0.0
            arm_upper.pose.position.y = 0.0
            rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
            arm_upper.pose.position.z = (arm_base_height - cyl_length / 2.0) * scale
            arm_upper.pose.orientation.x = rot90[0]
            arm_upper.pose.orientation.y = rot90[1]
            arm_upper.pose.orientation.z = rot90[2]
            arm_upper.pose.orientation.w = rot90[3]
            arm_upper.type = Body.CYLINDER
            arm_upper.scale.x = thickness / 2.0 * scale
            arm_upper.scale.y = cyl_length / 2.0 * scale
            arm_upper.scale.z = thickness / 2.0 * scale
            arm_upper.friction = 0.7
            add_compound_request.body.append(arm_upper)

            # connect the cylinders to the bottom plate with p2p ball socket
            # joints.
            constraint = Constraint()
            constraint.name = "arm_upper_fixed"
            constraint.body_a = "arm_base"
            constraint.body_b = arm_upper.name
            constraint.type = Constraint.FIXED
            # Need to transform arm_fore.pose into bot_plate frame
            # to get these
            constraint.pivot_in_a.x = 0.0
            constraint.pivot_in_a.y = 0.0
            constraint.pivot_in_a.z = 0.0
            constraint.pivot_in_b.x = 0
            constraint.pivot_in_b.y = cyl_length / 2.0 * scale
            constraint.pivot_in_b.z = 0
            constraint.disable_collisions_between_linked_bodies = True
            add_compound_request.constraint.append(constraint)

            arm_fore = Body()
            arm_fore.name = "arm_fore"
            arm_fore.mass = 1.0
            arm_fore.pose.orientation.x = rot90[0]
            arm_fore.pose.orientation.y = rot90[1]
            arm_fore.pose.orientation.z = rot90[2]
            arm_fore.pose.orientation.w = rot90[3]
            arm_fore.pose.position.x = 0.0
            arm_fore.pose.position.y = 0.0
            arm_fore.pose.position.z = (arm_base_height - cyl_length) * scale
            arm_fore.type = Body.CYLINDER
            arm_fore.scale.x = thickness * 0.4 * scale
            arm_fore.scale.y = cyl_length / 2.0 * scale
            arm_fore.scale.z = thickness * 0.4 * scale
            arm_fore.friction = 0.7
            add_compound_request.body.append(arm_fore)

            # connect each top cylinder to paired bottom cylinder with slider constraint
            prismatic = Constraint()
            prismatic.name = "prismatic_upper_fore"
            prismatic.body_a = arm_upper.name
            prismatic.body_b = arm_fore.name
            prismatic.type = Constraint.SLIDER
            prismatic.enable_pos_pub = True
            prismatic.enable_motor_sub = True
            prismatic.pivot_in_a.x = 0.0
            prismatic.pivot_in_a.y = -cyl_length / 4.0 * scale
            prismatic.pivot_in_a.z = 0.0
            prismatic.pivot_in_b.x = 0.0
            prismatic.pivot_in_b.y = 0.0  # cyl_length / 4.0
            prismatic.pivot_in_b.z = 0.0
            prismatic.lower_lin_lim = 0.0
            prismatic.upper_lin_lim = cyl_length * scale
            # TODO(lucasw) is this an absolute angle or rate?
            prismatic.lower_ang_lim = -0.1
            prismatic.upper_ang_lim = 0.1
            prismatic.max_motor_impulse = 2800.0
            prismatic.disable_collisions_between_linked_bodies = True
            add_compound_request.constraint.append(prismatic)

        # fingers gotta fing
        num_fingers = 3
        for i in range(num_fingers):
            finger_thickness = thickness / 4.0
            finger_length = 0.05
            finger_upper = Body()
            finger_upper.name = "finger_upper_" + str(i)
            finger_upper.mass = 0.5
            finger_upper.pose.orientation.x = rot90[0]
            finger_upper.pose.orientation.y = rot90[1]
            finger_upper.pose.orientation.z = rot90[2]
            finger_upper.pose.orientation.w = rot90[3]
            angle = float(i) / num_fingers * 2.0 * math.pi
            finger_upper.pose.position.x = thickness / 2.0 * math.cos(angle) * scale
            finger_upper.pose.position.y = thickness / 2.0 * math.sin(angle) * scale
            finger_upper.pose.position.z = (arm_base_height - cyl_length * 1.56) * scale
            finger_upper.type = Body.BOX
            finger_upper.scale.x = finger_thickness / 2.0 * scale
            finger_upper.scale.y = finger_length / 2.0 * scale
            finger_upper.scale.z = finger_thickness / 2.0 * scale
            finger_upper.friction = 0.7
            add_compound_request.body.append(finger_upper)

            finger_joint = Constraint()
            finger_joint.name = "finger_joint_" + str(i)
            finger_joint.body_a = "arm_fore"
            finger_joint.body_b = finger_upper.name
            finger_joint.type = Constraint.HINGE
            finger_joint.lower_ang_lim = -0.9
            finger_joint.upper_ang_lim = 0.6
            finger_joint.max_motor_impulse = 2000.0
            finger_joint.pivot_in_a.x = (thickness / 2.0 * math.cos(angle)) * scale
            finger_joint.pivot_in_a.z = (-thickness / 2.0 * math.sin(angle)) * scale
            finger_joint.pivot_in_a.y = -cyl_length * 0.52 * scale
            finger_joint.axis_in_a.x = -math.cos(angle + math.pi / 2.0)
            finger_joint.axis_in_a.z = math.sin(angle + math.pi / 2.0)
            finger_joint.axis_in_a.y = 0.0
            finger_joint.pivot_in_b.x = 0.0
            finger_joint.pivot_in_b.y = finger_length * 0.5 * scale
            finger_joint.pivot_in_b.z = 0.0
            finger_joint.axis_in_b.x = 1.0
            finger_joint.axis_in_b.y = 0.0
            finger_joint.axis_in_b.z = 0.0
            finger_joint.enable_pos_pub = True
            finger_joint.enable_motor_sub = True
            finger_joint.disable_collisions_between_linked_bodies = True
            add_compound_request.constraint.append(finger_joint)

            finger_lower = copy.deepcopy(finger_upper)
            finger_lower.name = "finger_lower_" + str(i)
            finger_lower.mass = 0.5
            finger_lower.pose.position.z = (arm_base_height - cyl_length * 1.56 - finger_length) * scale
            finger_lower.scale.y = finger_length * 0.4 * scale
            finger_lower.scale.z = finger_thickness / 3.0 * scale
            finger_lower.friction = 1.9
            add_compound_request.body.append(finger_lower)

            finger_lower_joint = copy.deepcopy(finger_joint)
            finger_lower_joint.name = "finger_lower_joint_" + str(i)
            finger_lower_joint.body_a = finger_upper.name
            finger_lower_joint.body_b = finger_lower.name
            finger_lower_joint.type = Constraint.HINGE
            finger_lower_joint.lower_ang_lim = -0.5
            finger_lower_joint.upper_ang_lim = 0.9
            finger_lower_joint.max_motor_impulse = 2000.0
            finger_lower_joint.pivot_in_a.x = 0.0
            finger_lower_joint.pivot_in_a.z = 0.0
            finger_lower_joint.pivot_in_a.y = -finger_length * 0.5 * scale
            finger_lower_joint.axis_in_a.x = 1.0
            finger_lower_joint.axis_in_a.y = 0.0
            finger_lower_joint.axis_in_a.z = 0.0
            finger_lower_joint.axis_in_a.y = 0.0
            finger_joint.pivot_in_b.y = finger_length * 0.4 * scale
            finger_lower_joint.enable_pos_pub = True
            finger_lower_joint.enable_motor_sub = True
            add_compound_request.constraint.append(finger_lower_joint)

        try:
            add_compound_response = self.add_compound(add_compound_request)
            rospy.loginfo(add_compound_response)
        except rospy.service.ServiceException as e:
            rospy.logerr(e)