def __init__(self):
# self.param_name = rospy.get_param('~robot_description', 'robot_description')
# TODO(lucasw) get xyz to spawn the root link at.
self.robot = URDF.from_parameter_server()
# rospy.loginfo(self.robot)
try:
rospy.wait_for_service('add_compound', 5)
except ROSException as e:
rospy.logerr("no service available")
rospy.logerr(e)
return
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
# self.body_pub = rospy.Publisher("add_body", Body, queue_size=50)
# self.constraint_pub = rospy.Publisher("add_constraint", Constraint, queue_size=50)
bodies = {}
for link in self.robot.links:
if rospy.is_shutdown():
break
# try:
# rospy.loginfo(link.visual.origin)
# except:
# pass
if link.collision is not None:
body = Body()
# TODO(lucasw) tf_prefix needed here?
body.name = link.name
body.pose.position.z = 2.0
# TODO(lwalter) transformations.py rpy to quaternion
body.pose.orientation.w = 1.0
# TODO(lwalter) is there a better way to get type?
if str(type(link.collision.geometry)
) == "<class 'urdf_parser_py.urdf.Box'>":
body.type = Body.BOX
body.scale.x = link.collision.geometry.size[0]
body.scale.y = link.collision.geometry.size[1]
body.scale.z = link.collision.geometry.size[2]
if str(type(link.collision.geometry)
) == "<class 'urdf_parser_py.urdf.Cylinder'>":
body.type = Body.CYLINDER
body.scale.x = link.collision.geometry.radius
body.scale.y = link.collision.geometry.radius
body.scale.z = link.collision.geometry.length
bodies[body.name] = body
# self.body_pub.publish(body)
constraints = {}
for joint in self.robot.joints:
print ''
print joint
constraint = Constraint()
constraint.body_a = joint.parent
constraint.body_b = joint.child
if joint.type == 'fixed':
constraint.type = Constraint.FIXED
constraints[joint.name] = constraint
add_compound_request = AddCompoundRequest()
for key in bodies.keys():
# rospy.loginfo(bodies.name)
add_compound_request.body.append(bodies[key])
for key in constraints.keys():
# rospy.loginfo(bodies.name)
add_compound_request.constraint.append(constraints[key])
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)
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
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)
# TODO(lucasw) make a random rotation
wheel.pose.orientation.x = 0.0 # 0.707
wheel.pose.orientation.w = random.uniform(0.9, 1.0) # 0.707
wheel.scale.x = random.uniform(0.5, 2.4)
wheel.scale.y = random.uniform(0.5, 2.4)
wheel.scale.z = random.uniform(0.5, 2.4)
# republish wheel because usually the first one isn't received
body_pub.publish(wheel)
rospy.sleep(sleep_time)
count += 1
# TODO(lucasw) create random joints between existing links
# store the names of all the existing bodies
min_count = 8
if count > min_count and random.random() > 0.5:
axel = Constraint()
axel.name = "constraint_" + postfix + "_" + str(count)
axel.type = Constraint.POINT2POINT
axel.body_a = bodies[random.randrange(count - min_count, count)]
axel.body_b = bodies[random.randrange(count - min_count, count)]
if (axel.body_a == axel.body_b):
continue
axel.pivot_in_a.x = random.uniform(2.5, 3.0)
axel.pivot_in_a.y = random.uniform(-3.0, 3.0)
axel.pivot_in_a.z = random.uniform(-3.0, 3.0)
axel.pivot_in_b.x = random.uniform(-2.5, -3.0)
axel.pivot_in_b.y = random.uniform(-3.0, 3.0)
axel.pivot_in_b.z = random.uniform(-3.0, 3.0)
constraint_pub.publish(axel)
rospy.loginfo(axel)
rospy.sleep(sleep_time)
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):
# self.param_name = rospy.get_param('~robot_description', 'robot_description')
# TODO(lucasw) get xyz to spawn the root link at.
self.robot = URDF.from_parameter_server()
# rospy.loginfo(self.robot)
try:
rospy.wait_for_service('add_compound', 5)
except ROSException as e:
rospy.logerr("no service available")
rospy.logerr(e)
return
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
# self.body_pub = rospy.Publisher("add_body", Body, queue_size=50)
# self.constraint_pub = rospy.Publisher("add_constraint", Constraint, queue_size=50)
bodies = {}
for link in self.robot.links:
if rospy.is_shutdown():
break
# try:
# rospy.loginfo(link.visual.origin)
# except:
# pass
if link.collision is not None:
body = Body()
# TODO(lucasw) tf_prefix needed here?
body.name = link.name
body.pose.position.z = 2.0
# TODO(lwalter) transformations.py rpy to quaternion
body.pose.orientation.w = 1.0
# TODO(lwalter) is there a better way to get type?
if str(type(link.collision.geometry)) == "<class 'urdf_parser_py.urdf.Box'>":
body.type = Body.BOX
body.scale.x = link.collision.geometry.size[0]
body.scale.y = link.collision.geometry.size[1]
body.scale.z = link.collision.geometry.size[2]
if str(type(link.collision.geometry)) == "<class 'urdf_parser_py.urdf.Cylinder'>":
body.type = Body.CYLINDER
body.scale.x = link.collision.geometry.radius
body.scale.y = link.collision.geometry.radius
body.scale.z = link.collision.geometry.length
bodies[body.name] = body
# self.body_pub.publish(body)
constraints = {}
for joint in self.robot.joints:
print ''
print joint
constraint = Constraint()
constraint.body_a = joint.parent
constraint.body_b = joint.child
if joint.type == 'fixed':
constraint.type = Constraint.FIXED
constraints[joint.name] = constraint
add_compound_request = AddCompoundRequest()
for key in bodies.keys():
# rospy.loginfo(bodies.name)
add_compound_request.body.append(bodies[key])
for key in constraints.keys():
# rospy.loginfo(bodies.name)
add_compound_request.constraint.append(constraints[key])
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
chassis.type = Body.BOX chassis.pose.position.x = 0.0 chassis.pose.position.y = 0.0 chassis.pose.position.z = z_height chassis.pose.orientation.x = 0.0 # 0.707 chassis.pose.orientation.w = 1.0 # 0.707 chassis.scale.x = 1.2 chassis.scale.y = 0.5 chassis.scale.z = 0.25 body_pub.publish(chassis) rospy.sleep(sleep_time) # exit() axel_y = 0.75 axel = Constraint() axel.name = "axel0" axel.type = Constraint.POINT2POINT axel.body_a = "chassis0" axel.body_b = "wheel0" axel.pivot_in_a.x = 1.0 # wheel0.pose.position.x axel.pivot_in_a.y = axel_y axel.pivot_in_a.z = -0.1 axel.pivot_in_b.y = -0.2 constraint_pub.publish(axel) rospy.sleep(sleep_time) axel.name = "axel1" axel.body_b = "wheel1" axel.pivot_in_a.y = -axel_y axel.pivot_in_b.y = 0.2
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)
chassis.type = Body.BOX chassis.pose.position.x = 0.0 chassis.pose.position.y = 0.0 chassis.pose.position.z = z_height chassis.pose.orientation.x = 0.0 # 0.707 chassis.pose.orientation.w = 1.0 # 0.707 chassis.scale.x = 1.2 chassis.scale.y = 0.5 chassis.scale.z = 0.25 body_pub.publish(chassis) rospy.sleep(sleep_time) # exit() axle_y = 0.75 axle = Constraint() axle.name = "axle0" axle.type = Constraint.POINT2POINT axle.body_a = "chassis0" axle.body_b = "wheel0" axle.pivot_in_a.x = 1.0 # wheel0.pose.position.x axle.pivot_in_a.y = axle_y axle.pivot_in_a.z = -0.1 axle.pivot_in_b.y = -0.2 constraint_pub.publish(axle) rospy.sleep(sleep_time) axle.name = "axle1" axle.body_b = "wheel1" axle.pivot_in_a.y = -axle_y axle.pivot_in_b.y = 0.2
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)