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
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
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
# constraint_pub = rospy.Publisher("add_constraint", Constraint, queue_size=1)
# 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)