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)