def make_rigid_cylinder(name,
mass,
xs,
ys,
zs,
radius,
thickness,
roll=math.pi / 2.0,
pitch=0,
yaw=0):
# make the top cylinder plate
body = Body()
body.name = name
body.mass = mass
rot90 = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
body.pose.orientation.x = rot90[0]
body.pose.orientation.y = rot90[1]
body.pose.orientation.z = rot90[2]
body.pose.orientation.w = rot90[3]
body.pose.position.x = xs
body.pose.position.y = ys
body.pose.position.z = zs
body.type = Body.CYLINDER
body.scale.x = radius
body.scale.y = thickness
body.scale.z = radius
return body
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 = False
# make the top cylinder plate
ground = Body()
ground.name = "ground"
ground.mass = 0.0
rot90 = tf.transformations.quaternion_from_euler(0, 0, 0)
radius = 50
thickness = 1.0
ground.pose.orientation.x = rot90[0]
ground.pose.orientation.y = rot90[1]
ground.pose.orientation.z = rot90[2]
ground.pose.orientation.w = rot90[3]
ground.pose.position.z = -thickness
ground.type = Body.BOX
ground.scale.x = radius
ground.scale.y = radius
ground.scale.z = thickness
ground.friction = 0.7
add_compound_request.body.append(ground)
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 = False
# make the top cylinder plate
ground = Body()
ground.name = "ground"
ground.mass = 0.0
rot90 = tf.transformations.quaternion_from_euler(0, 0, 0)
radius = 50
thickness = 1.0
ground.pose.orientation.x = rot90[0]
ground.pose.orientation.y = rot90[1]
ground.pose.orientation.z = rot90[2]
ground.pose.orientation.w = rot90[3]
ground.pose.position.z = -thickness
ground.type = Body.BOX
ground.scale.x = radius
ground.scale.y = radius
ground.scale.z = thickness
add_compound_request.body.append(ground)
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_rigid_box(name, mass, xs, ys, zs, wd, ln, ht, roll=0, pitch=0, yaw=0):
body = Body()
body.name = name
body.type = Body.BOX
body.mass = mass
body.pose.position.x = xs
body.pose.position.y = ys
body.pose.position.z = zs
rot90 = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
body.pose.orientation.x = rot90[0]
body.pose.orientation.y = rot90[1]
body.pose.orientation.z = rot90[2]
body.pose.orientation.w = rot90[3]
body.scale.x = wd
body.scale.y = ln
body.scale.z = ht
return body
def make_rigid_box(name, mass, xs, ys, zs, wd, ln, ht,
roll=0, pitch=0, yaw=0):
body = Body()
body.name = name
body.type = Body.BOX
body.mass = mass
body.kinematic = False
body.pose.position.x = xs
body.pose.position.y = ys
body.pose.position.z = zs
rot90 = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
body.pose.orientation.x = rot90[0]
body.pose.orientation.y = rot90[1]
body.pose.orientation.z = rot90[2]
body.pose.orientation.w = rot90[3]
body.scale.x = wd
body.scale.y = ln
body.scale.z = ht
return body
def make_rigid_cylinder(name, mass, xs, ys, zs, radius, thickness,
roll=math.pi/2.0, pitch=0, yaw=0):
# make the top cylinder plate
body = Body()
body.name = name
body.mass = mass
body.kinematic = False
rot90 = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
body.pose.orientation.x = rot90[0]
body.pose.orientation.y = rot90[1]
body.pose.orientation.z = rot90[2]
body.pose.orientation.w = rot90[3]
body.pose.position.x = xs
body.pose.position.y = ys
body.pose.position.z = zs
body.type = Body.CYLINDER
body.scale.x = radius
body.scale.y = thickness
body.scale.z = radius
return body
size_x = rospy.get_param("~size_x", 0.4)
size_y = rospy.get_param("~size_y", 0.3)
size_z = rospy.get_param("~size_z", 0.15)
mass = rospy.get_param("~mass", 0.2)
margin_x = rospy.get_param("~margin_x", 0.00001)
margin_z = rospy.get_param("~margin_z", 0.01)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False
if False:
for j in range(num_y):
for i in range(num_x):
body = Body()
body.name = "wall_box_" + str(i) + "_" + str(j)
body.mass = mass
body.type = Body.BOX
body.scale.x = size_x / 2.0
body.scale.y = size_y / 2.0
body.scale.z = size_z / 2.0
x_offset = -num_x / 2.0 * size_x + (j % 2) * size_x / 3.0
body.pose.position.x = x + x_offset + i * (size_x + margin_x)
body.pose.position.y = y
body.pose.position.z = size_z / 2.0 + margin_z + j * (size_z +
margin_z)
body.pose.orientation.w = 1.0
add_compound_request.body.append(body)
else:
# make a cylinder wall
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)
postfix = str(random.randrange(0, 1000000000))
count = 0
bodies = {}
z_height = rospy.get_param("~z", 8.8)
single = rospy.get_param("~single", False)
while not rospy.is_shutdown():
body = Body()
body.name = "random_body_" + postfix + "_" + str(count)
bodies[count] = body.name
body.type = random.randint(0, 4) # Body.CYLINDER
if rospy.get_param("~kinematic", False):
body.kinematic = True
body.mass = 0.0
else:
body.mass = 0.4 # + random.uniform(-0.5, 1.5)
body.pose.position.x = random.uniform(-0.1, 0.1)
body.pose.position.y = random.uniform(-0.1, 0.1)
body.pose.position.z = z_height # + random.uniform(0, 4.0)
# TODO(lucasw) make a random rotation
body.pose.orientation.x = 0.0 # 0.707
body.pose.orientation.w = random.uniform(0.9, 1.0) # 0.707
body.scale.x = random.uniform(0.1, 0.4)
body.scale.y = random.uniform(0.1, 0.4)
body.scale.z = random.uniform(0.1, 0.4)
# republish body because usually the first one isn't received
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
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 process_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
# feedback.control_name == "spawn_menu":
if feedback.menu_entry_id == 1:
self.count += 1
# rospy.loginfo(feedback)
body = Body()
body.name = "imarker_spawned_body_" + str(self.count)
# TODO(lucasw)
body.mass = 1.0
try:
trans = self.tf_buffer.lookup_transform(
"map", self.spawn_frame, rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr("tf2_ros exception")
rospy.logerr(e)
return
body.pose.position.x = trans.transform.translation.x
body.pose.position.y = trans.transform.translation.y
body.pose.position.z = trans.transform.translation.z
body.pose.orientation = trans.transform.rotation
# TODO(lucasw) add twist linear with another interactive tf,
# and twist angular with a second interactive tf?
body.type = Body.BOX
body.scale.x = self.im.controls[0].markers[0].scale.x / 2.0
body.scale.y = self.im.controls[0].markers[0].scale.y / 2.0
body.scale.z = self.im.controls[0].markers[0].scale.z / 2.0
# can't get this to work
# http://answers.ros.org/question/249433/tf2_ros-buffer-transform-pointstamped/
if False:
feedback_pt = PointStamped()
feedback_pt.header = feedback.header
feedback_pt.point.x = self.linear_vel_pt[0]
feedback_pt.point.y = self.linear_vel_pt[1]
feedback_pt.point.z = self.linear_vel_pt[2]
self.tf_buffer.registration.print_me()
try:
pt_in_map = self.tf_buffer.transform(
feedback_pt, "map", rospy.Duration(2.0),
PointStamped)
except tf2_ros.TypeException as e:
# rospy.logerr(e)
print e
return
print 'output', pt_in_map
else:
quat = [
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w
]
mat = tf.transformations.quaternion_matrix(quat)
pt_in_map = numpy.dot(mat, self.linear_vel_pt)
body.twist.linear.x = pt_in_map[0]
body.twist.linear.y = pt_in_map[1]
body.twist.linear.z = pt_in_map[2]
# rospy.loginfo(body.twist.linear)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False
add_compound_request.body.append(body)
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)
xs = rospy.get_param("~x", 0.0)
ys = rospy.get_param("~y", 0.0)
zs = rospy.get_param("~z", 1.0)
body = SoftBody()
body.name = "pyramid"
body.config = make_soft_config()
mass = 0.5
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs + 1.0
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys + 1.0
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs + 1.0
body.node.append(n1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 1
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 2
l1.node_indices[1] = 3
body.link.append(l1)
mat = Material()
mat.kLST = 0.15
mat.kVST = 0.1
mat.kAST = 0.1
body.material.append(mat)
body.k_clusters = 8
body.margin = 0.05
add_compound_request.soft_body.append(body)
if False:
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(
math.pi / 2.0, 0, 0)
radius = 1.5
thickness = 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.x = 0
top_plate.pose.position.y = 0
top_plate.pose.position.z = 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)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
floor = 0.0
radius = 1.0
height = radius * 2
rot90 = tf.transformations.quaternion_from_euler(math.pi / 2.0, 0, 0)
thickness = 0.1
# make the bottom cylinder plate
bot_plate = Body()
bot_plate.name = "bottom_plate"
# make the plate static
bot_plate.mass = 0.0
bot_plate.pose.orientation.x = rot90[0]
bot_plate.pose.orientation.y = rot90[1]
bot_plate.pose.orientation.z = rot90[2]
bot_plate.pose.orientation.w = rot90[3]
bot_plate.pose.position.z = floor + 0.2
bot_plate.type = Body.CYLINDER
bot_plate.scale.x = radius
bot_plate.scale.y = thickness
bot_plate.scale.z = radius
add_compound_request.body.append(bot_plate)
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.5
top_plate.pose.orientation.x = rot90[0]
top_plate.pose.orientation.y = rot90[1]
top_plate.pose.orientation.z = rot90[2]
top_plate.pose.orientation.w = rot90[3]
top_plate.pose.position.z = floor + 1.7
top_plate.type = Body.CYLINDER
top_plate.scale.x = radius
top_plate.scale.y = thickness
top_plate.scale.z = radius
add_compound_request.body.append(top_plate)
# make six actuator cylinder bottoms with TBD spacing in a circle
for i in range(6):
bot_cylinder = Body()
bot_cylinder.name = "bot_cylinder_" + str(i)
bot_cylinder.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(
-math.pi / 2.0, 0, 0)
bot_cylinder.pose.orientation.x = rot90[0]
bot_cylinder.pose.orientation.y = rot90[1]
bot_cylinder.pose.orientation.z = rot90[2]
bot_cylinder.pose.orientation.w = rot90[3]
if i % 2 == 0:
angle = i / 6.0 * 2.0 * math.pi - 0.15
else:
angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
bot_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
bot_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
bot_cylinder.pose.position.z = floor + 0.2 + height / 6.0 * 0.5 + 0.3
bot_cylinder.type = Body.CYLINDER
bot_cylinder.scale.x = thickness / 2.0
bot_cylinder.scale.y = height / 4.0
bot_cylinder.scale.z = thickness / 2.0
add_compound_request.body.append(bot_cylinder)
# connect the cylinders to the bottom plate with p2p ball socket
# joints.
constraint = Constraint()
constraint.name = "bot_cylinder_p2p_" + str(i)
constraint.body_a = "bottom_plate"
constraint.body_b = bot_cylinder.name
constraint.type = Constraint.POINT2POINT
# Need to transform bot_cylinder.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = bot_cylinder.pose.position.x
constraint.pivot_in_a.z = -bot_cylinder.pose.position.y
constraint.pivot_in_a.y = bot_cylinder.pose.position.z - 0.5
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = bot_cylinder.scale.y + 0.05
constraint.pivot_in_b.z = 0
add_compound_request.constraint.append(constraint)
# make six actuator cylinder tops
top_cylinder = copy.deepcopy(bot_cylinder)
top_cylinder.name = "top_cylinder_" + str(i)
top_cylinder.mass = 0.3
if i % 2 == 0:
angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
else:
angle = (i) / 6.0 * 2.0 * math.pi - 0.15
top_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
top_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
top_cylinder.pose.position.z = bot_cylinder.pose.position.z + 0.6
top_cylinder.scale.x = thickness / 2.3
top_cylinder.scale.y = height / 7.0
top_cylinder.scale.z = thickness / 2.3
add_compound_request.body.append(top_cylinder)
# connect each top cylinder to paired bottom cylinder with slider constraint
prismatic = Constraint()
prismatic.name = "prismatic_" + str(i)
prismatic.body_a = bot_cylinder.name
prismatic.body_b = top_cylinder.name
prismatic.type = Constraint.SLIDER
prismatic.pivot_in_a.x = 0
prismatic.pivot_in_a.y = -0.1
prismatic.pivot_in_a.z = 0
prismatic.pivot_in_b.x = 0
prismatic.pivot_in_b.y = 0.1
prismatic.pivot_in_b.z = 0
prismatic.lower_lin_lim = 0.0
prismatic.upper_lin_lim = 0.3
# TODO(lucasw) is this an absolute angle or rate?
prismatic.lower_ang_lim = -0.1
prismatic.upper_ang_lim = 0.1
prismatic.max_motor_impulse = 5000.0
add_compound_request.constraint.append(prismatic)
# connect the top cylinders with p2p joints to top plate
constraint = Constraint()
constraint.name = "tot_cylinder_p2p_" + str(i)
constraint.body_a = "top_plate"
constraint.body_b = top_cylinder.name
constraint.type = Constraint.POINT2POINT
# Need to transform bot_cylinder.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = top_cylinder.pose.position.x
constraint.pivot_in_a.z = -top_cylinder.pose.position.y
constraint.pivot_in_a.y = -0.2
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = -0.2
constraint.pivot_in_b.z = 0
add_compound_request.constraint.append(constraint)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
rospy.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)
xs = rospy.get_param("~x", 0.0)
ys = rospy.get_param("~y", 0.0)
zs = rospy.get_param("~z", 1.0)
body = SoftBody()
body.name = "pyramid"
body.config = make_soft_config()
mass = 0.5
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs + 1.0
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys + 1.0
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs + 1.0
body.node.append(n1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 1
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 2
l1.node_indices[1] = 3
body.link.append(l1)
mat = Material()
mat.kLST = 0.15
mat.kVST = 0.1
mat.kAST = 0.1
body.material.append(mat)
body.k_clusters = 8
body.margin = 0.05
add_compound_request.soft_body.append(body)
if False:
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
radius = 1.5
thickness = 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.x = 0
top_plate.pose.position.y = 0
top_plate.pose.position.z = 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)
rospy.loginfo(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 process_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
# feedback.control_name == "spawn_menu":
if feedback.menu_entry_id == 1:
self.count += 1
# rospy.loginfo(feedback)
body = Body()
body.name = "imarker_spawned_body_" + str(self.count)
# TODO(lucasw)
body.mass = 1.0
try:
trans = self.tf_buffer.lookup_transform("map", self.spawn_frame,
rospy.Time())
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr("tf2_ros exception")
rospy.logerr(e)
return
body.pose.position.x = trans.transform.translation.x
body.pose.position.y = trans.transform.translation.y
body.pose.position.z = trans.transform.translation.z
body.pose.orientation = trans.transform.rotation
# TODO(lucasw) add twist linear with another interactive tf,
# and twist angular with a second interactive tf?
body.type = Body.BOX
body.scale.x = self.im.controls[0].markers[0].scale.x / 2.0
body.scale.y = self.im.controls[0].markers[0].scale.y / 2.0
body.scale.z = self.im.controls[0].markers[0].scale.z / 2.0
# can't get this to work
# http://answers.ros.org/question/249433/tf2_ros-buffer-transform-pointstamped/
if False:
feedback_pt = PointStamped()
feedback_pt.header = feedback.header
feedback_pt.point.x = self.linear_vel_pt[0]
feedback_pt.point.y = self.linear_vel_pt[1]
feedback_pt.point.z = self.linear_vel_pt[2]
self.tf_buffer.registration.print_me()
try:
pt_in_map = self.tf_buffer.transform(feedback_pt, "map",
rospy.Duration(2.0), PointStamped)
except tf2_ros.TypeException as e:
# rospy.logerr(e)
print e
return
print 'output', pt_in_map
else:
quat = [trans.transform.rotation.x,
trans.transform.rotation.y,
trans.transform.rotation.z,
trans.transform.rotation.w]
mat = tf.transformations.quaternion_matrix(quat)
pt_in_map = numpy.dot(mat, self.linear_vel_pt)
body.twist.linear.x = pt_in_map[0]
body.twist.linear.y = pt_in_map[1]
body.twist.linear.z = pt_in_map[2]
# rospy.loginfo(body.twist.linear)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False
add_compound_request.body.append(body)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
# impulse_pub = rospy.Publisher("add_impulse", Impulse, queue_size=1)
sleep_time = 2.0
count = 0
bodies = {}
z_height = rospy.get_param("~z", 0.8)
if True:
body = Body()
body.name = "kinematic_body"
bodies[count] = body.name
body.type = random.randint(0, 4) # Body.CYLINDER
body.kinematic = True
body.mass = 0.0
body.pose.position.x = random.uniform(-0.1, 0.1)
body.pose.position.y = random.uniform(-0.1, 0.1)
body.pose.position.z = z_height # + random.uniform(0, 4.0)
# TODO(lucasw) make a random rotation
body.pose.orientation.x = 0.0 # 0.707
body.pose.orientation.w = random.uniform(0.9, 1.0) # 0.707
body.scale.x = random.uniform(0.1, 0.4)
body.scale.y = random.uniform(0.1, 0.4)
body.scale.z = random.uniform(0.1, 0.4)
# republish body because usually the first one isn't received
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
add_compound_request.body.append(body)
body2 = copy.deepcopy(body)
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)
size_x = rospy.get_param("~size_x", 0.4)
size_y = rospy.get_param("~size_y", 0.3)
size_z = rospy.get_param("~size_z", 0.15)
mass = rospy.get_param("~mass", 0.2)
margin_x = rospy.get_param("~margin_x", 0.00001)
margin_z = rospy.get_param("~margin_z", 0.01)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False
if False:
for j in range(num_y):
for i in range(num_x):
body = Body()
body.name = "wall_box_" + str(i) + "_" + str(j)
body.mass = mass
body.type = Body.BOX
body.scale.x = size_x / 2.0
body.scale.y = size_y / 2.0
body.scale.z = size_z / 2.0
x_offset = -num_x / 2.0 * size_x + (j % 2) * size_x / 3.0
body.pose.position.x = x + x_offset + i * (size_x + margin_x)
body.pose.position.y = y
body.pose.position.z = size_z / 2.0 + margin_z + j * (size_z + margin_z)
body.pose.orientation.w = 1.0
add_compound_request.body.append(body)
else:
# make a cylinder wall
circumference = size_x * num_x
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)