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