def __init__(self):
rospy.loginfo("waiting for server add_compound")
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
rospy.loginfo("connected to add_compound service")
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
# scale everything by this factor
scale = rospy.get_param("~scale", 10.0)
# make a table
table_height = 0.0
if rospy.get_param("~ground", False):
rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
table_thickness = 0.1
table = Body()
table.name = "table"
# make the floor static
table.mass = 0.0
table.pose.orientation.x = rot90[0]
table.pose.orientation.y = rot90[1]
table.pose.orientation.z = rot90[2]
table.pose.orientation.w = rot90[3]
table.pose.position.z = (table_height - table_thickness / 2) * scale
table.type = Body.CYLINDER
table.scale.x = 1.0 * scale
table.scale.y = table_thickness * scale
table.scale.z = 1.0 * scale
add_compound_request.body.append(table)
# make a sphere to grasp
if rospy.get_param("~rigid_ball", False):
ball_radius = 0.04 * scale
ball = Body()
ball.name = "ball"
ball.mass = 0.3 * scale
ball.pose.orientation.x = rot90[0]
ball.pose.orientation.y = rot90[1]
ball.pose.orientation.z = rot90[2]
ball.pose.orientation.w = rot90[3]
ball.pose.position.z = (table_height + ball_radius + 0.01) * scale
ball.type = Body.SPHERE
ball.scale.x = ball_radius
ball.scale.y = ball_radius
ball.scale.z = ball_radius
add_compound_request.body.append(ball)
# Platform arm is attached to
arm_base_height = table_height + 0.93
rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
arm_base_thickness = 0.02
arm_base = Body()
arm_base.name = "arm_base"
# make the platform static
arm_base.mass = 0.0
arm_base.pose.orientation.x = rot90[0]
arm_base.pose.orientation.y = rot90[1]
arm_base.pose.orientation.z = rot90[2]
arm_base.pose.orientation.w = rot90[3]
arm_base.pose.position.z = arm_base_height * scale
arm_base.type = Body.CYLINDER
arm_base.scale.x = 0.1 * scale
arm_base.scale.y = arm_base_thickness * scale
arm_base.scale.z = 0.1 * scale
add_compound_request.body.append(arm_base)
if True:
thickness = 0.08
cyl_length = 0.5
arm_upper = Body()
# arm_upper = copy.deepcopy(arm_fore)
arm_upper.name = "arm_upper"
arm_upper.mass = 1.0
arm_upper.pose.position.x = 0.0
arm_upper.pose.position.y = 0.0
rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
arm_upper.pose.position.z = (arm_base_height - cyl_length / 2.0) * scale
arm_upper.pose.orientation.x = rot90[0]
arm_upper.pose.orientation.y = rot90[1]
arm_upper.pose.orientation.z = rot90[2]
arm_upper.pose.orientation.w = rot90[3]
arm_upper.type = Body.CYLINDER
arm_upper.scale.x = thickness / 2.0 * scale
arm_upper.scale.y = cyl_length / 2.0 * scale
arm_upper.scale.z = thickness / 2.0 * scale
arm_upper.friction = 0.7
add_compound_request.body.append(arm_upper)
# connect the cylinders to the bottom plate with p2p ball socket
# joints.
constraint = Constraint()
constraint.name = "arm_upper_fixed"
constraint.body_a = "arm_base"
constraint.body_b = arm_upper.name
constraint.type = Constraint.FIXED
# Need to transform arm_fore.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = 0.0
constraint.pivot_in_a.y = 0.0
constraint.pivot_in_a.z = 0.0
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = cyl_length / 2.0 * scale
constraint.pivot_in_b.z = 0
constraint.disable_collisions_between_linked_bodies = True
add_compound_request.constraint.append(constraint)
arm_fore = Body()
arm_fore.name = "arm_fore"
arm_fore.mass = 1.0
arm_fore.pose.orientation.x = rot90[0]
arm_fore.pose.orientation.y = rot90[1]
arm_fore.pose.orientation.z = rot90[2]
arm_fore.pose.orientation.w = rot90[3]
arm_fore.pose.position.x = 0.0
arm_fore.pose.position.y = 0.0
arm_fore.pose.position.z = (arm_base_height - cyl_length) * scale
arm_fore.type = Body.CYLINDER
arm_fore.scale.x = thickness * 0.4 * scale
arm_fore.scale.y = cyl_length / 2.0 * scale
arm_fore.scale.z = thickness * 0.4 * scale
arm_fore.friction = 0.7
add_compound_request.body.append(arm_fore)
# connect each top cylinder to paired bottom cylinder with slider constraint
prismatic = Constraint()
prismatic.name = "prismatic_upper_fore"
prismatic.body_a = arm_upper.name
prismatic.body_b = arm_fore.name
prismatic.type = Constraint.SLIDER
prismatic.enable_pos_pub = True
prismatic.enable_motor_sub = True
prismatic.pivot_in_a.x = 0.0
prismatic.pivot_in_a.y = -cyl_length / 4.0 * scale
prismatic.pivot_in_a.z = 0.0
prismatic.pivot_in_b.x = 0.0
prismatic.pivot_in_b.y = 0.0 # cyl_length / 4.0
prismatic.pivot_in_b.z = 0.0
prismatic.lower_lin_lim = 0.0
prismatic.upper_lin_lim = cyl_length * scale
# TODO(lucasw) is this an absolute angle or rate?
prismatic.lower_ang_lim = -0.1
prismatic.upper_ang_lim = 0.1
prismatic.max_motor_impulse = 2800.0
prismatic.disable_collisions_between_linked_bodies = True
add_compound_request.constraint.append(prismatic)
# fingers gotta fing
num_fingers = 3
for i in range(num_fingers):
finger_thickness = thickness / 4.0
finger_length = 0.05
finger_upper = Body()
finger_upper.name = "finger_upper_" + str(i)
finger_upper.mass = 0.5
finger_upper.pose.orientation.x = rot90[0]
finger_upper.pose.orientation.y = rot90[1]
finger_upper.pose.orientation.z = rot90[2]
finger_upper.pose.orientation.w = rot90[3]
angle = float(i) / num_fingers * 2.0 * math.pi
finger_upper.pose.position.x = thickness / 2.0 * math.cos(angle) * scale
finger_upper.pose.position.y = thickness / 2.0 * math.sin(angle) * scale
finger_upper.pose.position.z = (arm_base_height - cyl_length * 1.56) * scale
finger_upper.type = Body.BOX
finger_upper.scale.x = finger_thickness / 2.0 * scale
finger_upper.scale.y = finger_length / 2.0 * scale
finger_upper.scale.z = finger_thickness / 2.0 * scale
finger_upper.friction = 0.7
add_compound_request.body.append(finger_upper)
finger_joint = Constraint()
finger_joint.name = "finger_joint_" + str(i)
finger_joint.body_a = "arm_fore"
finger_joint.body_b = finger_upper.name
finger_joint.type = Constraint.HINGE
finger_joint.lower_ang_lim = -0.9
finger_joint.upper_ang_lim = 0.6
finger_joint.max_motor_impulse = 2000.0
finger_joint.pivot_in_a.x = (thickness / 2.0 * math.cos(angle)) * scale
finger_joint.pivot_in_a.z = (-thickness / 2.0 * math.sin(angle)) * scale
finger_joint.pivot_in_a.y = -cyl_length * 0.52 * scale
finger_joint.axis_in_a.x = -math.cos(angle + math.pi / 2.0)
finger_joint.axis_in_a.z = math.sin(angle + math.pi / 2.0)
finger_joint.axis_in_a.y = 0.0
finger_joint.pivot_in_b.x = 0.0
finger_joint.pivot_in_b.y = finger_length * 0.5 * scale
finger_joint.pivot_in_b.z = 0.0
finger_joint.axis_in_b.x = 1.0
finger_joint.axis_in_b.y = 0.0
finger_joint.axis_in_b.z = 0.0
finger_joint.enable_pos_pub = True
finger_joint.enable_motor_sub = True
finger_joint.disable_collisions_between_linked_bodies = True
add_compound_request.constraint.append(finger_joint)
finger_lower = copy.deepcopy(finger_upper)
finger_lower.name = "finger_lower_" + str(i)
finger_lower.mass = 0.5
finger_lower.pose.position.z = (arm_base_height - cyl_length * 1.56 - finger_length) * scale
finger_lower.scale.y = finger_length * 0.4 * scale
finger_lower.scale.z = finger_thickness / 3.0 * scale
finger_lower.friction = 1.9
add_compound_request.body.append(finger_lower)
finger_lower_joint = copy.deepcopy(finger_joint)
finger_lower_joint.name = "finger_lower_joint_" + str(i)
finger_lower_joint.body_a = finger_upper.name
finger_lower_joint.body_b = finger_lower.name
finger_lower_joint.type = Constraint.HINGE
finger_lower_joint.lower_ang_lim = -0.5
finger_lower_joint.upper_ang_lim = 0.9
finger_lower_joint.max_motor_impulse = 2000.0
finger_lower_joint.pivot_in_a.x = 0.0
finger_lower_joint.pivot_in_a.z = 0.0
finger_lower_joint.pivot_in_a.y = -finger_length * 0.5 * scale
finger_lower_joint.axis_in_a.x = 1.0
finger_lower_joint.axis_in_a.y = 0.0
finger_lower_joint.axis_in_a.z = 0.0
finger_lower_joint.axis_in_a.y = 0.0
finger_joint.pivot_in_b.y = finger_length * 0.4 * scale
finger_lower_joint.enable_pos_pub = True
finger_lower_joint.enable_motor_sub = True
add_compound_request.constraint.append(finger_lower_joint)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
rospy.loginfo("waiting for server add_compound")
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
rospy.loginfo("connected to service")
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
floor = 0.0
radius = 1.0
height = radius * 2
rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
thickness = 0.1
# make the bottom cylinder plate
bot_plate = Body()
bot_plate.name = "bottom_plate"
# make the plate static
bot_plate.mass = 0.0
bot_plate.pose.orientation.x = rot90[0]
bot_plate.pose.orientation.y = rot90[1]
bot_plate.pose.orientation.z = rot90[2]
bot_plate.pose.orientation.w = rot90[3]
bot_plate.pose.position.z = floor + 0.2
bot_plate.type = Body.CYLINDER
bot_plate.scale.x = radius
bot_plate.scale.y = thickness
bot_plate.scale.z = radius
add_compound_request.body.append(bot_plate)
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.5
top_plate.pose.orientation.x = rot90[0]
top_plate.pose.orientation.y = rot90[1]
top_plate.pose.orientation.z = rot90[2]
top_plate.pose.orientation.w = rot90[3]
top_plate.pose.position.z = floor + 1.7
top_plate.type = Body.CYLINDER
top_plate.scale.x = radius
top_plate.scale.y = thickness
top_plate.scale.z = radius
add_compound_request.body.append(top_plate)
# make six actuator cylinder bottoms with TBD spacing in a circle
for i in range(6):
bot_cylinder = Body()
bot_cylinder.name = "bot_cylinder_" + str(i)
bot_cylinder.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(-math.pi/2.0, 0, 0)
bot_cylinder.pose.orientation.x = rot90[0]
bot_cylinder.pose.orientation.y = rot90[1]
bot_cylinder.pose.orientation.z = rot90[2]
bot_cylinder.pose.orientation.w = rot90[3]
if i % 2 == 0:
angle = i / 6.0 * 2.0 * math.pi - 0.15
else:
angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
bot_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
bot_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
bot_cylinder.pose.position.z = floor + 0.2 + height/6.0 * 0.5 + 0.3
bot_cylinder.type = Body.CYLINDER
bot_cylinder.scale.x = thickness / 2.0
bot_cylinder.scale.y = height / 4.0
bot_cylinder.scale.z = thickness / 2.0
add_compound_request.body.append(bot_cylinder)
# connect the cylinders to the bottom plate with p2p ball socket
# joints.
constraint = Constraint()
constraint.name = "bot_cylinder_p2p_" + str(i)
constraint.body_a = "bottom_plate"
constraint.body_b = bot_cylinder.name
constraint.type = Constraint.POINT2POINT
# Need to transform bot_cylinder.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = bot_cylinder.pose.position.x
constraint.pivot_in_a.z = -bot_cylinder.pose.position.y
constraint.pivot_in_a.y = bot_cylinder.pose.position.z - 0.5
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = bot_cylinder.scale.y + 0.05
constraint.pivot_in_b.z = 0
add_compound_request.constraint.append(constraint)
# make six actuator cylinder tops
top_cylinder = copy.deepcopy(bot_cylinder)
top_cylinder.name = "top_cylinder_" + str(i)
top_cylinder.mass = 0.3
if i % 2 == 0:
angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
else:
angle = (i) / 6.0 * 2.0 * math.pi - 0.15
top_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
top_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
top_cylinder.pose.position.z = bot_cylinder.pose.position.z + 0.6
top_cylinder.scale.x = thickness / 2.3
top_cylinder.scale.y = height / 7.0
top_cylinder.scale.z = thickness / 2.3
add_compound_request.body.append(top_cylinder)
# connect each top cylinder to paired bottom cylinder with slider constraint
prismatic = Constraint()
prismatic.name = "prismatic_" + str(i)
prismatic.body_a = bot_cylinder.name
prismatic.body_b = top_cylinder.name
prismatic.type = Constraint.SLIDER
prismatic.enable_pos_pub = True
prismatic.enable_motor_sub = True
prismatic.pivot_in_a.x = 0
prismatic.pivot_in_a.y = -0.1
prismatic.pivot_in_a.z = 0
prismatic.pivot_in_b.x = 0
prismatic.pivot_in_b.y = 0.1
prismatic.pivot_in_b.z = 0
prismatic.lower_lin_lim = 0.0
prismatic.upper_lin_lim = 0.3
# TODO(lucasw) is this an absolute angle or rate?
prismatic.lower_ang_lim = -0.1
prismatic.upper_ang_lim = 0.1
prismatic.max_motor_impulse = 5000.0
add_compound_request.constraint.append(prismatic)
# connect the top cylinders with p2p joints to top plate
constraint = Constraint()
constraint.name = "tot_cylinder_p2p_" + str(i)
constraint.body_a = "top_plate"
constraint.body_b = top_cylinder.name
constraint.type = Constraint.POINT2POINT
# Need to transform bot_cylinder.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = top_cylinder.pose.position.x
constraint.pivot_in_a.z = -top_cylinder.pose.position.y
constraint.pivot_in_a.y = -0.2
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = -0.2
constraint.pivot_in_b.z = 0
add_compound_request.constraint.append(constraint)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
floor = 0.0
radius = 1.0
height = radius * 2
rot90 = tf.transformations.quaternion_from_euler(math.pi / 2.0, 0, 0)
thickness = 0.1
# make the bottom cylinder plate
bot_plate = Body()
bot_plate.name = "bottom_plate"
# make the plate static
bot_plate.mass = 0.0
bot_plate.pose.orientation.x = rot90[0]
bot_plate.pose.orientation.y = rot90[1]
bot_plate.pose.orientation.z = rot90[2]
bot_plate.pose.orientation.w = rot90[3]
bot_plate.pose.position.z = floor + 0.2
bot_plate.type = Body.CYLINDER
bot_plate.scale.x = radius
bot_plate.scale.y = thickness
bot_plate.scale.z = radius
add_compound_request.body.append(bot_plate)
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.5
top_plate.pose.orientation.x = rot90[0]
top_plate.pose.orientation.y = rot90[1]
top_plate.pose.orientation.z = rot90[2]
top_plate.pose.orientation.w = rot90[3]
top_plate.pose.position.z = floor + 1.7
top_plate.type = Body.CYLINDER
top_plate.scale.x = radius
top_plate.scale.y = thickness
top_plate.scale.z = radius
add_compound_request.body.append(top_plate)
# make six actuator cylinder bottoms with TBD spacing in a circle
for i in range(6):
bot_cylinder = Body()
bot_cylinder.name = "bot_cylinder_" + str(i)
bot_cylinder.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(
-math.pi / 2.0, 0, 0)
bot_cylinder.pose.orientation.x = rot90[0]
bot_cylinder.pose.orientation.y = rot90[1]
bot_cylinder.pose.orientation.z = rot90[2]
bot_cylinder.pose.orientation.w = rot90[3]
if i % 2 == 0:
angle = i / 6.0 * 2.0 * math.pi - 0.15
else:
angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
bot_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
bot_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
bot_cylinder.pose.position.z = floor + 0.2 + height / 6.0 * 0.5 + 0.3
bot_cylinder.type = Body.CYLINDER
bot_cylinder.scale.x = thickness / 2.0
bot_cylinder.scale.y = height / 4.0
bot_cylinder.scale.z = thickness / 2.0
add_compound_request.body.append(bot_cylinder)
# connect the cylinders to the bottom plate with p2p ball socket
# joints.
constraint = Constraint()
constraint.name = "bot_cylinder_p2p_" + str(i)
constraint.body_a = "bottom_plate"
constraint.body_b = bot_cylinder.name
constraint.type = Constraint.POINT2POINT
# Need to transform bot_cylinder.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = bot_cylinder.pose.position.x
constraint.pivot_in_a.z = -bot_cylinder.pose.position.y
constraint.pivot_in_a.y = bot_cylinder.pose.position.z - 0.5
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = bot_cylinder.scale.y + 0.05
constraint.pivot_in_b.z = 0
add_compound_request.constraint.append(constraint)
# make six actuator cylinder tops
top_cylinder = copy.deepcopy(bot_cylinder)
top_cylinder.name = "top_cylinder_" + str(i)
top_cylinder.mass = 0.3
if i % 2 == 0:
angle = (i - 1) / 6.0 * 2.0 * math.pi + 0.15
else:
angle = (i) / 6.0 * 2.0 * math.pi - 0.15
top_cylinder.pose.position.x = 0.7 * radius * math.cos(angle)
top_cylinder.pose.position.y = 0.7 * radius * math.sin(angle)
top_cylinder.pose.position.z = bot_cylinder.pose.position.z + 0.6
top_cylinder.scale.x = thickness / 2.3
top_cylinder.scale.y = height / 7.0
top_cylinder.scale.z = thickness / 2.3
add_compound_request.body.append(top_cylinder)
# connect each top cylinder to paired bottom cylinder with slider constraint
prismatic = Constraint()
prismatic.name = "prismatic_" + str(i)
prismatic.body_a = bot_cylinder.name
prismatic.body_b = top_cylinder.name
prismatic.type = Constraint.SLIDER
prismatic.pivot_in_a.x = 0
prismatic.pivot_in_a.y = -0.1
prismatic.pivot_in_a.z = 0
prismatic.pivot_in_b.x = 0
prismatic.pivot_in_b.y = 0.1
prismatic.pivot_in_b.z = 0
prismatic.lower_lin_lim = 0.0
prismatic.upper_lin_lim = 0.3
# TODO(lucasw) is this an absolute angle or rate?
prismatic.lower_ang_lim = -0.1
prismatic.upper_ang_lim = 0.1
prismatic.max_motor_impulse = 5000.0
add_compound_request.constraint.append(prismatic)
# connect the top cylinders with p2p joints to top plate
constraint = Constraint()
constraint.name = "tot_cylinder_p2p_" + str(i)
constraint.body_a = "top_plate"
constraint.body_b = top_cylinder.name
constraint.type = Constraint.POINT2POINT
# Need to transform bot_cylinder.pose into bot_plate frame
# to get these
constraint.pivot_in_a.x = top_cylinder.pose.position.x
constraint.pivot_in_a.z = -top_cylinder.pose.position.y
constraint.pivot_in_a.y = -0.2
constraint.pivot_in_b.x = 0
constraint.pivot_in_b.y = -0.2
constraint.pivot_in_b.z = 0
add_compound_request.constraint.append(constraint)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
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)