Beispiel #1
0
def make_wheel_assembly(prefix, xs, ys, zs, flip=1.0):
    motor_mass = 0.2
    motor_thickness = 0.1
    motor_radius = 0.2
    motor_y = 0.8
    motor = make_rigid_cylinder(prefix + "_motor", motor_mass,
                                xs, ys - motor_y * flip, zs,
                                motor_radius, motor_thickness,
                                0, 0, 0)

    hinge = Constraint()
    hinge.name = prefix + "_motor_hinge"
    hinge.body_a = "chassis"
    hinge.body_b = prefix + "_motor"
    hinge.type = Constraint.HINGE
    hinge.pivot_in_a.x = xs
    hinge.pivot_in_a.y = -motor_y * flip
    hinge.pivot_in_a.z = 0
    hinge.pivot_in_b.x = 0
    hinge.pivot_in_b.y = 0
    hinge.pivot_in_b.z = 0
    hinge.axis_in_a.x = 0
    hinge.axis_in_a.y = 1.0
    hinge.axis_in_a.z = 0
    hinge.axis_in_b.x = 0
    hinge.axis_in_b.y = 1.0
    hinge.axis_in_b.z = 0
    # These have to be < -pi and > pi to be unlimited
    hinge.lower_ang_lim = -3.2  # -math.pi
    hinge.upper_ang_lim = 3.2  # math.pi
    hinge.max_motor_impulse = 25000.0
    hinge.enable_pos_pub = True
    hinge.enable_motor_sub = True

    wheel_offset = 1.5
    nx = 4
    ny = 4
    nz = 4
    soft_length = 0.4
    node_mass = 0.1
    wheel = make_soft_cube(prefix + "wheel", node_mass,
                           xs, ys - wheel_offset * flip, zs, soft_length,
                           nx, ny, nz,
                           flip)

    # attach the rigid motor wheel to the soft wheel
    for i in range(2):
        for j in range(3):
            for k in range(2):
                xi = i + 1  # int(nx/2) - 1
                yi = j + 1  # int(ny/2) - 1
                zi = k + 1  # int(nz/2) - 1
                ind = xi * ny * nz + yi * nz + zi
                if ind < len(wheel.node):
                    anchor = Anchor()
                    anchor.disable_collision_between_linked_bodies = False
                    # the weaker the influence, the longer the spring between
                    # the local_pivot and the node
                    # TODO(lucasw) probably should have more anchors,
                    # but weaken the influence with distance from the wheel
                    # axle
                    anchor.influence = 1.0

                    anchor.node_index = ind
                    anchor.rigid_body_name = prefix + "_motor"
                    anchor.local_pivot.x = wheel.node[ind].position.x - xs
                    anchor.local_pivot.y = wheel.node[ind].position.y - ys + 0.8 * flip
                    anchor.local_pivot.z = wheel.node[ind].position.z - zs
                    wheel.anchor.append(anchor)

    return (motor, hinge, wheel)
Beispiel #2
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)
Beispiel #3
0
    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)

        obstacle = Body()
        obstacle.name = "obstacle_1"
        obstacle.type = Body.BOX
        obstacle.mass = 0.0
        obstacle.pose.orientation.w = 1.0
        obstacle.pose.position.x = -2.0
        obstacle.scale.x = 1.0
        obstacle.scale.y = 1.0
        obstacle.scale.z = 0.07
        add_compound_request.body.append(obstacle)

        num_tracks = rospy.get_param("~num_tracks", 25)
        track_width = rospy.get_param("~track_width", 0.1)
        track_length = rospy.get_param("~track_length", 0.04)
        track_height = rospy.get_param("~track_height", 0.02)
        track_mass = rospy.get_param("~track_mass", 0.1)

        gap = rospy.get_param("~gap", 0.04)
        # spawn in a circle for now
        radius = (track_width + gap) * num_tracks / (2.0 * math.pi)

        track = Body()
        track.type = Body.BOX
        track.mass = track_mass
        track.scale.x = track_length
        track.scale.y = track_width
        track.scale.z = track_height

        for k in range(2):

            add_track = True
            if add_track:
                for i in range(num_tracks):
                    trk = copy.deepcopy(track)
                    trk.name = "track_" + str(k) + "_" + str(i)
                    fr = float(i) / float(num_tracks)
                    angle = 2.0 * math.pi * fr
                    rot = tf.transformations.quaternion_from_euler(
                        0, -angle + math.pi / 2.0, 0)
                    trk.pose.orientation.x = rot[0]
                    trk.pose.orientation.y = rot[1]
                    trk.pose.orientation.z = rot[2]
                    trk.pose.orientation.w = rot[3]

                    trk.pose.position.x = 1.5 * radius * math.cos(angle)
                    trk.pose.position.y = -0.5 + k * 1.0
                    trk.pose.position.z = 1.2 * radius + 0.75 * radius * math.sin(
                        angle)
                    add_compound_request.body.append(trk)

                # add hinges between tracks
                for i in range(num_tracks):
                    constraint = Constraint()
                    constraint.name = "hinge_" + str(k) + "_" + str(i)
                    constraint.body_a = "track_" + str(k) + "_" + str(i)
                    constraint.body_b = "track_" + str(k) + "_" + str(
                        (i + 1) % num_tracks)
                    constraint.type = Constraint.HINGE
                    constraint.pivot_in_a.x = -(track_length / 2.0 + gap)
                    constraint.pivot_in_b.x = (track_length / 2.0 + gap)
                    constraint.axis_in_a.y = 1.0
                    constraint.axis_in_b.y = 1.0
                    add_compound_request.constraint.append(constraint)

            wheel_spacing = rospy.get_param("~wheel_spacing", 1.1)
            chassis = Body()
            chassis.name = "chassis"
            chassis.type = Body.BOX
            chassis.mass = 2.0
            chassis.pose.orientation.w = 1.0
            chassis.scale.x = wheel_spacing * 0.55
            chassis.scale.y = 0.3
            chassis.scale.z = 0.07
            chassis.pose.position.z = radius
            add_compound_request.body.append(chassis)

            tracks_per_circumference = 10
            wheel_circumference = (track_length * 2.0 +
                                   gap) * tracks_per_circumference
            wheel_radius = wheel_circumference / (2.0 * math.pi)
            for i in range(2):
                wheel = Body()
                wheel.type = Body.CYLINDER
                wheel.name = "wheel_" + str(k) + "_" + str(i)
                wheel.mass = 1.0
                wheel.pose.orientation.w = 1.0
                wheel.scale.x = wheel_radius
                wheel.scale.y = track_width * 1.1
                wheel.scale.z = wheel_radius
                wheel.pose.position.x = -wheel_spacing / 2.0 + i * wheel_spacing
                wheel.pose.position.y = -0.5 + 1.0 * k
                wheel.pose.position.z = radius
                add_compound_request.body.append(wheel)

                for j in range(tracks_per_circumference):
                    fr = float(j) / float(tracks_per_circumference)
                    angle = fr * 2.0 * math.pi
                    tooth = Body()
                    tooth.name = "tooth_" + str(k) + "_" + str(i) + "_" + str(
                        j)
                    tooth.type = Body.BOX
                    tooth.mass = 0.1
                    tooth_angle = angle
                    rot = tf.transformations.quaternion_from_euler(
                        0, tooth_angle - math.pi / 2.0, 0)
                    tooth.pose.orientation.x = rot[0]
                    tooth.pose.orientation.y = rot[1]
                    tooth.pose.orientation.z = rot[2]
                    tooth.pose.orientation.w = rot[3]
                    tooth.scale.x = gap * 0.8
                    tooth.scale.y = track_width
                    tooth.scale.z = gap * 0.35
                    x = wheel_radius * math.cos(angle)
                    z = wheel_radius * math.sin(angle)
                    tooth.pose.position.x = wheel.pose.position.x + x
                    tooth.pose.position.y = wheel.pose.position.y
                    tooth.pose.position.z = wheel.pose.position.z + z
                    add_compound_request.body.append(tooth)

                    if True:
                        fixed = Constraint()
                        fixed.name = "attach_" + tooth.name
                        fixed.body_a = wheel.name
                        fixed.body_b = tooth.name
                        fixed.type = Constraint.HINGE
                        # TODO(lucasw) this doesn't match what rot does above- as the sim
                        # start the tooth rotates to this constraint
                        fixed.lower_ang_lim = tooth_angle - 0.01
                        fixed.upper_ang_lim = tooth_angle + 0.01
                        # TODO(lucasw) Something is broken with fixed
                        # fixed.type = Constraint.FIXED
                        fixed.pivot_in_a.x = x
                        fixed.pivot_in_a.y = 0
                        fixed.pivot_in_a.z = z
                        fixed.axis_in_a.y = 1.0
                        fixed.axis_in_b.y = 1.0
                        add_compound_request.constraint.append(fixed)

                axle = Constraint()
                axle.name = "wheel_motor_" + str(k) + "_" + str(i)
                axle.body_a = "chassis"
                axle.body_b = wheel.name
                axle.type = Constraint.HINGE
                axle.lower_ang_lim = -4.0
                axle.upper_ang_lim = 4.0
                axle.max_motor_impulse = 25000.0
                axle.pivot_in_a.x = wheel.pose.position.x
                axle.pivot_in_a.y = -0.5 + 1.0 * k
                axle.pivot_in_b.y = 0.0
                axle.axis_in_a.y = 1.0
                axle.axis_in_b.y = 1.0
                add_compound_request.constraint.append(axle)

                # try to constraint the track- could also try two cones
                cover_offset = track_width * 1.15
                # Outer cover
                cover = Body()
                cover.type = Body.CYLINDER
                cover.name = "outer_cover_" + str(k) + "_" + str(i)
                cover.mass = 1.0
                cover.pose.orientation.w = 1.0
                cover.scale.x = wheel_radius * 1.1
                cover.scale.y = track_width * 0.1
                cover.scale.z = wheel_radius * 1.1
                cover.pose.position.x = -wheel_spacing / 2.0 + i * wheel_spacing
                cover.pose.position.y = -0.5 - cover_offset + (
                    1.0 + 2.0 * cover_offset) * k
                cover.pose.position.z = radius
                add_compound_request.body.append(cover)

                axle = Constraint()
                axle.name = "inner_cover_constraint_" + str(k) + "_" + str(i)
                axle.body_a = wheel.name
                axle.body_b = cover.name
                axle.type = Constraint.HINGE
                axle.lower_ang_lim = -0.01
                axle.upper_ang_lim = 0.01
                axle.max_motor_impulse = 0.0
                axle.pivot_in_a.x = 0.0
                axle.pivot_in_a.y = cover.pose.position.y - wheel.pose.position.y
                axle.pivot_in_b.y = 0.0
                axle.axis_in_a.y = 1.0
                axle.axis_in_b.y = 1.0
                add_compound_request.constraint.append(axle)

                # Inner cover
                cover = Body()
                cover.type = Body.CYLINDER
                cover.name = "inner_cover_" + str(k) + "_" + str(i)
                cover.mass = 1.0
                cover.pose.orientation.w = 1.0
                cover.scale.x = wheel_radius * 1.1
                cover.scale.y = track_width * 0.1
                cover.scale.z = wheel_radius * 1.1
                cover.pose.position.x = -wheel_spacing / 2.0 + i * wheel_spacing
                cover.pose.position.y = -0.5 + cover_offset + (
                    1.0 - 2.0 * cover_offset) * k
                cover.pose.position.z = radius
                add_compound_request.body.append(cover)

                axle = Constraint()
                axle.name = "outer_cover_constraint_" + str(k) + "_" + str(i)
                axle.body_a = wheel.name
                axle.body_b = cover.name
                axle.type = Constraint.HINGE
                axle.lower_ang_lim = -0.01
                axle.upper_ang_lim = 0.01
                axle.max_motor_impulse = 0.0
                axle.pivot_in_a.x = 0.0
                axle.pivot_in_a.y = cover.pose.position.y - wheel.pose.position.y
                axle.pivot_in_b.y = 0.0
                axle.axis_in_a.y = 1.0
                axle.axis_in_b.y = 1.0
                add_compound_request.constraint.append(axle)

        # print add_compound_request
        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)
    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)
Beispiel #6
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)
Beispiel #7
0
    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)

        obstacle = Body()
        obstacle.name = "obstacle_1"
        obstacle.type = Body.BOX
        obstacle.mass = 0.0
        obstacle.pose.orientation.w = 1.0
        obstacle.pose.position.x = -2.0
        obstacle.scale.x = 1.0
        obstacle.scale.y = 1.0
        obstacle.scale.z = 0.07
        add_compound_request.body.append(obstacle)

        num_tracks = rospy.get_param("~num_tracks", 25)
        track_width = rospy.get_param("~track_width", 0.1)
        track_length = rospy.get_param("~track_length", 0.04)
        track_height = rospy.get_param("~track_height", 0.02)
        track_mass = rospy.get_param("~track_mass", 0.1)

        gap = rospy.get_param("~gap", 0.04)
        # spawn in a circle for now
        radius = (track_width + gap) * num_tracks / (2.0 * math.pi)

        track = Body()
        track.type = Body.BOX
        track.mass = track_mass
        track.scale.x = track_length
        track.scale.y = track_width
        track.scale.z = track_height

        for k in range(2):

            add_track = True
            if add_track:
                for i in range(num_tracks):
                    trk = copy.deepcopy(track)
                    trk.name = "track_" + str(k) + "_" + str(i)
                    fr = float(i) / float(num_tracks)
                    angle = 2.0 * math.pi * fr
                    rot = tf.transformations.quaternion_from_euler(0, -angle + math.pi/2.0, 0)
                    trk.pose.orientation.x = rot[0]
                    trk.pose.orientation.y = rot[1]
                    trk.pose.orientation.z = rot[2]
                    trk.pose.orientation.w = rot[3]

                    trk.pose.position.x = 1.5 * radius * math.cos(angle)
                    trk.pose.position.y = -0.5 + k * 1.0
                    trk.pose.position.z = 1.2 * radius + 0.75 * radius * math.sin(angle)
                    add_compound_request.body.append(trk)

                # add hinges between tracks
                for i in range(num_tracks):
                    constraint = Constraint()
                    constraint.name = "hinge_" + str(k) + "_" + str(i)
                    constraint.body_a = "track_" + str(k) + "_" + str(i)
                    constraint.body_b = "track_" + str(k) + "_" + str((i + 1) % num_tracks)
                    constraint.type = Constraint.HINGE
                    constraint.pivot_in_a.x = -(track_length / 2.0 + gap)
                    constraint.pivot_in_b.x = (track_length / 2.0 + gap)
                    constraint.axis_in_a.y = 1.0
                    constraint.axis_in_b.y = 1.0
                    constraint.enable_pos_pub = False
                    constraint.enable_motor_sub = False
                    add_compound_request.constraint.append(constraint)

            wheel_spacing = rospy.get_param("~wheel_spacing", 1.1)
            chassis = Body()
            chassis.name = "chassis"
            chassis.type = Body.BOX
            chassis.mass = 2.0
            chassis.pose.orientation.w = 1.0
            chassis.scale.x = wheel_spacing * 0.55
            chassis.scale.y = 0.3
            chassis.scale.z = 0.07
            chassis.pose.position.z = radius
            add_compound_request.body.append(chassis)

            tracks_per_circumference = 10
            wheel_circumference = (track_length * 2.0 + gap) * tracks_per_circumference
            wheel_radius = wheel_circumference / (2.0 * math.pi)
            for i in range(2):
                wheel = Body()
                wheel.type = Body.CYLINDER
                wheel.name = "wheel_" + str(k) + "_" + str(i)
                wheel.mass = 1.0
                wheel.pose.orientation.w = 1.0
                wheel.scale.x = wheel_radius
                wheel.scale.y = track_width * 1.1
                wheel.scale.z = wheel_radius
                wheel.pose.position.x = -wheel_spacing / 2.0 + i * wheel_spacing
                wheel.pose.position.y = -0.5 + 1.0 * k
                wheel.pose.position.z = radius
                add_compound_request.body.append(wheel)

                for j in range(tracks_per_circumference):
                    fr = float(j) / float(tracks_per_circumference)
                    angle = fr * 2.0 * math.pi
                    tooth = Body()
                    tooth.name = "tooth_" + str(k) + "_" + str(i) + "_" + str(j)
                    tooth.type = Body.BOX
                    tooth.mass = 0.1
                    tooth_angle = angle
                    rot = tf.transformations.quaternion_from_euler(0, tooth_angle - math.pi / 2.0, 0)
                    tooth.pose.orientation.x = rot[0]
                    tooth.pose.orientation.y = rot[1]
                    tooth.pose.orientation.z = rot[2]
                    tooth.pose.orientation.w = rot[3]
                    tooth.scale.x = gap * 0.8
                    tooth.scale.y = track_width
                    tooth.scale.z = gap * 0.35
                    x = wheel_radius * math.cos(angle)
                    z = wheel_radius * math.sin(angle)
                    tooth.pose.position.x = wheel.pose.position.x + x
                    tooth.pose.position.y = wheel.pose.position.y
                    tooth.pose.position.z = wheel.pose.position.z + z
                    add_compound_request.body.append(tooth)

                    if True:
                        fixed = Constraint()
                        fixed.name = "attach_" + tooth.name
                        fixed.body_a = wheel.name
                        fixed.body_b = tooth.name
                        fixed.type = Constraint.HINGE
                        # TODO(lucasw) this doesn't match what rot does above- as the sim
                        # start the tooth rotates to this constraint
                        fixed.lower_ang_lim = tooth_angle - 0.01
                        fixed.upper_ang_lim = tooth_angle + 0.01
                        # TODO(lucasw) Something is broken with fixed
                        # fixed.type = Constraint.FIXED
                        fixed.pivot_in_a.x = x
                        fixed.pivot_in_a.y = 0
                        fixed.pivot_in_a.z = z
                        fixed.axis_in_a.y = 1.0
                        fixed.axis_in_b.y = 1.0
                        fixed.enable_pos_pub = False
                        fixed.enable_motor_sub = False
                        add_compound_request.constraint.append(fixed)

                axle = Constraint()
                axle.name = "wheel_motor_" + str(k) + "_" + str(i)
                axle.body_a = "chassis"
                axle.body_b = wheel.name
                axle.type = Constraint.HINGE
                axle.lower_ang_lim = -4.0
                axle.upper_ang_lim = 4.0
                axle.max_motor_impulse = 25000.0
                axle.pivot_in_a.x = wheel.pose.position.x
                axle.pivot_in_a.y = -0.5 + 1.0 * k
                axle.pivot_in_b.y = 0.0
                axle.axis_in_a.y = 1.0
                axle.axis_in_b.y = 1.0
                axle.enable_pos_pub = True
                axle.enable_motor_sub = True
                add_compound_request.constraint.append(axle)

                # try to constraint the track- could also try two cones
                cover_offset = track_width * 1.15
                # Outer cover
                cover = Body()
                cover.type = Body.CYLINDER
                cover.name = "outer_cover_" + str(k) + "_" + str(i)
                cover.mass = 1.0
                cover.pose.orientation.w = 1.0
                cover.scale.x = wheel_radius * 1.1
                cover.scale.y = track_width * 0.1
                cover.scale.z = wheel_radius * 1.1
                cover.pose.position.x = -wheel_spacing / 2.0 + i * wheel_spacing
                cover.pose.position.y = -0.5 - cover_offset + (1.0 + 2.0 * cover_offset) * k
                cover.pose.position.z = radius
                add_compound_request.body.append(cover)

                axle = Constraint()
                axle.name = "inner_cover_constraint_" + str(k) + "_" + str(i)
                axle.body_a = wheel.name
                axle.body_b = cover.name
                axle.type = Constraint.HINGE
                axle.lower_ang_lim = -0.01
                axle.upper_ang_lim = 0.01
                axle.max_motor_impulse = 0.0
                axle.pivot_in_a.x = 0.0
                axle.pivot_in_a.y = cover.pose.position.y - wheel.pose.position.y
                axle.pivot_in_b.y = 0.0
                axle.axis_in_a.y = 1.0
                axle.axis_in_b.y = 1.0
                axle.enable_pos_pub = False
                axle.enable_motor_sub = False
                add_compound_request.constraint.append(axle)

                # Inner cover
                cover = Body()
                cover.type = Body.CYLINDER
                cover.name = "inner_cover_" + str(k) + "_" + str(i)
                cover.mass = 1.0
                cover.pose.orientation.w = 1.0
                cover.scale.x = wheel_radius * 1.1
                cover.scale.y = track_width * 0.1
                cover.scale.z = wheel_radius * 1.1
                cover.pose.position.x = -wheel_spacing / 2.0 + i * wheel_spacing
                cover.pose.position.y = -0.5 + cover_offset + (1.0 - 2.0 * cover_offset) * k
                cover.pose.position.z = radius
                add_compound_request.body.append(cover)

                axle = Constraint()
                axle.name = "outer_cover_constraint_" + str(k) + "_" + str(i)
                axle.body_a = wheel.name
                axle.body_b = cover.name
                axle.type = Constraint.HINGE
                axle.lower_ang_lim = -0.01
                axle.upper_ang_lim = 0.01
                axle.max_motor_impulse = 0.0
                axle.pivot_in_a.x = 0.0
                axle.pivot_in_a.y = cover.pose.position.y - wheel.pose.position.y
                axle.pivot_in_b.y = 0.0
                axle.axis_in_a.y = 1.0
                axle.axis_in_b.y = 1.0
                axle.enable_pos_pub = False
                axle.enable_motor_sub = False
                add_compound_request.constraint.append(axle)

        # print add_compound_request
        try:
            add_compound_response = self.add_compound(add_compound_request)
            rospy.loginfo(add_compound_response)
        except rospy.service.ServiceException as e:
            rospy.logerr(e)
Beispiel #8
0
def make_wheel_assembly(prefix, xs, ys, zs, flip=1.0):
    motor_mass = 0.2
    motor_thickness = 0.1
    motor_radius = 0.2
    motor_y = 0.8
    motor = make_rigid_cylinder(prefix + "_motor", motor_mass,
                                xs, ys - motor_y * flip, zs,
                                motor_radius, motor_thickness,
                                0, 0, 0)

    hinge = Constraint()
    hinge.name = prefix + "_motor_hinge"
    hinge.body_a = "chassis"
    hinge.body_b = prefix + "_motor"
    hinge.type = Constraint.HINGE
    hinge.pivot_in_a.x = xs
    hinge.pivot_in_a.y = -motor_y * flip
    hinge.pivot_in_a.z = 0
    hinge.pivot_in_b.x = 0
    hinge.pivot_in_b.y = 0
    hinge.pivot_in_b.z = 0
    hinge.axis_in_a.x = 0
    hinge.axis_in_a.y = 1.0
    hinge.axis_in_a.z = 0
    hinge.axis_in_b.x = 0
    hinge.axis_in_b.y = 1.0
    hinge.axis_in_b.z = 0
    # These have to be < -pi and > pi to be unlimited
    hinge.lower_ang_lim = -3.2  # -math.pi
    hinge.upper_ang_lim = 3.2  # math.pi
    hinge.max_motor_impulse = 25000.0
    hinge.enable_pos_pub = True
    hinge.enable_motor_sub = True

    wheel_offset = 1.5
    nx = 4
    ny = 4
    nz = 4
    soft_length = 0.4
    node_mass = 0.1
    wheel = make_soft_cube(prefix + "wheel", node_mass,
                           xs, ys - wheel_offset * flip, zs, soft_length,
                           nx, ny, nz,
                           flip)

    # attach the rigid motor wheel to the soft wheel
    for i in range(2):
        for j in range(3):
            for k in range(2):
                xi = i + 1  # int(nx/2) - 1
                yi = j + 1  # int(ny/2) - 1
                zi = k + 1  # int(nz/2) - 1
                ind = xi * ny * nz + yi * nz + zi
                if ind < len(wheel.node):
                    anchor = Anchor()
                    anchor.disable_collision_between_linked_bodies = False
                    # the weaker the influence, the longer the spring between
                    # the local_pivot and the node
                    # TODO(lucasw) probably should have more anchors,
                    # but weaken the influence with distance from the wheel
                    # axle
                    anchor.influence = 1.0

                    anchor.node_index = ind
                    anchor.rigid_body_name = prefix + "_motor"
                    anchor.local_pivot.x = wheel.node[ind].position.x - xs
                    anchor.local_pivot.y = wheel.node[ind].position.y - ys + 0.8 * flip
                    anchor.local_pivot.z = wheel.node[ind].position.z - zs
                    wheel.anchor.append(anchor)

    return (motor, hinge, wheel)