def get_slot_position(self, slot_id): self.check_slot(slot_id) if slot_id == 0: return Vector3(-0.5 * SLOT_THICKNESS, 0, 0) else: return self.connection.to_sibling_frame( Vector3(0.5 * JOINT_CONNECTION_THICKNESS, 0, 0), self)
def get_slot_normal(self, slot_id): self.check_slot(slot_id) if slot_id == 0: return Vector3(-1, 0, 0) else: return self.tail.to_sibling_direction(Vector3(1, 0, 0), self)
def get_slot_position(self, slot_id): self.check_slot(slot_id) if slot_id == 0: return Vector3(-0.5 * SLOT_THICKNESS, 0, 0) else: return self.tail.to_sibling_frame( Vector3(0.5 * SLOT_THICKNESS, 0, 0), self)
def get_slot_position(self, slot_id): self.check_slot(slot_id) offset = 0.5 * self.SLOT_THICKNESS if slot_id == 0: # Root has no relative pose so this is easy return Vector3(-offset, 0, 0) else: # This is a posable group, so we must use # sibling conversion. return self.tail.to_sibling_frame(Vector3(offset, 0, 0), self)
def get_slot_position(self, slot_id): self.check_slot(slot_id) offset = 0.5 * SLOT_THICKNESS if slot_id == 0: # The root slot is positioned half the slot # thickness to the left return Vector3(-offset, 0, 0) else: # A `BodyPart` is a posable group, so item positions are # in the parent frame. If we convert to the local frame we can # simply add the offset in the x-direction tail_pos = self.to_local_frame(self.hinge_tail.get_position()) return tail_pos + Vector3(offset, 0, 0)
def get_slot_position(self, slot): """ Returns the attachment position of each of the slots. """ # Again, prevent errors self.check_slot(slot) # The constructor of `BodyPart` stores the initialization's kwargs # parameters in `self.part_params`. length = self.part_params["length"] # The center of the base box lies at (0, 0), move 1/2 x to the # left to get that slot, or move 1/2 x to the right, plus the # variable length minus the offset to get the other. return Vector3(-0.5 * self.x, 0, 0) if slot == 0 \ else Vector3(0.5 * self.x + length - self.JOINT_OFFSET, 0, 0)
def _build_analyzer_body(model, body_parts, connections): """ Builds a model from body parts in analyzer mode - i.e. one link per body part to allow collision detection. :param model: :param body_parts: :type body_parts: list[BodyPart] :return: """ part_map = {} for body_part in body_parts: link = Link("link_" + body_part.id, self_collide=True) link.set_position(body_part.get_position()) link.set_rotation(body_part.get_rotation()) body_part.set_position(Vector3()) body_part.set_rotation(Quaternion()) link.add_element(body_part) model.add_element(link) part_map[body_part.id] = link # Create fake joints for all connections so they will # not trigger collisions. for a, b in connections: joint = FixedJoint(part_map[a], part_map[b]) model.add_element(joint)
def _initialize(self, **kwargs): """ :param kwargs: :return: """ self.mass = MASS self.x = SENSOR_BASE_THICKNESS self.y = self.z = SENSOR_BASE_WIDTH super(LightSensor, self)._initialize(**kwargs) # Add the SDF camera sensor camera = SdfSensor( name="{}_light_sensor".format(self.id), sensor_type="camera", update_rate=self.conf.sensor_update_rate) # TODO: Set field of view cam_details = "<camera>" \ "<image>" \ "<width>1</width><height>1</height>" \ "</image>" \ "<clip><near>{}</near><far>{}</far></clip>" \ "</camera>".format(nf(in_mm(1)), nf(in_mm(50000))) camera.add_element(cam_details) camera.set_position(Vector3(0.5 * self.x, 0, 0)) self.component.add_sensor(camera, "light") if self.conf.visualize_sensors: camera.add_element("<visualize>1</visualize>") self.apply_color()
async def analysis_func(): analyzer = await (BodyAnalyzer.create(address=("127.0.0.1", 11346))) # Try a maximum of 100 times for _ in range(100): # Generate a new robot robot = generate_robot() sdf = get_analysis_robot(robot, builder) # Find out its intersections and bounding box intersections, bbox = await (analyzer.analyze_robot(robot, builder=builder)) if intersections: print("Invalid model - intersections detected.", file=sys.stderr) else: print("No model intersections detected!", file=sys.stderr) if bbox: # Translate the model in z direction so it stands directly on # the ground print("Model bounding box: ({}, {}, {}), ({}, {}, {})".format( bbox.min.x, bbox.min.y, bbox.min.z, bbox.max.x, bbox.max.y, bbox.max.z), file=sys.stderr) model = sdf.elements[0] model.translate(Vector3(0, 0, -bbox.min.z)) print(str(robot_to_sdf(robot, "test_bot", "controllerplugin.so"))) break
def get_slot_position(self, slot_id): self.check_slot(slot_id) offset = 0.5 * SLOT_THICKNESS if slot_id == 0: # The root slot is positioned half the slot # thickness to the left return self.hinge_root.to_sibling_frame(Vector3(-offset, 0, 0), self) else: # A `BodyPart` is a PosableGroup, so child positions are # similar to sibling positions. We can thus take the position # in the tail, and use sibling conversion to get the position # in the body part. return self.hinge_tail.to_sibling_frame(Vector3(offset, 0, 0), self)
def get_slot_tangent(self, slot_id): """ We use the same tangent vector for every slot, pointing horizontally. :param slot_id: :return: """ return Vector3(1, 0, 0)
def _initialize(self, **kwargs): """ Initialize method to generate the hinge. Inheriting from "box" makes sure there is a box of the given size in `self.link`. """ super(PassiveHinge, self)._initialize(**kwargs) # Create the variable size block. `create_link` is the recommended # way of doing this, because it sets some non-default link properties # (such as self_collide) which you generally need. length = kwargs["length"] self.var_block = self.create_component( BoxGeom(length, self.y, self.z, 0.1), "var-block") # We move the block in the x-direction so that it # just about overlaps with the other block (to # make it seem like a somewhat realistic joint) self.var_block.translate( Vector3(0.5 * (length + self.x) - self.JOINT_OFFSET, 0, 0)) # Now create a revolute joint at this same position. The # joint axis is in the y-direction. axis = Vector3(0, 1, 0) passive_joint = Joint( joint_type="revolute", parent=self.component, child=self.var_block, axis=axis) # Set some movement limits on the joint passive_joint.axis.limit = Limit( lower=math.radians(-45), upper=math.radians(45), effort=1.0) # Set the joint position - in the child frame! passive_joint.set_position( Vector3(-0.5 * length + self.JOINT_OFFSET, 0, 0)) # Don't forget to add the joint and the link self.add_joint(passive_joint) # Apply the generated color self.apply_color()
def _initialize(self, **kwargs): """ :param kwargs: :return: """ # Call super to initialize the cylinder part of the wheel super(Wheel, self)._initialize(**kwargs) # Create the small box that serves as the motor box_size = self.MOTOR_SIZE self.attachment = self.create_component( BoxGeom(box_size, box_size, box_size, 0.01), "cylinder-attach") # Get attachment position and axis of the motor joint anchor = Vector3(0, 0, 0.5 * self.LENGTH) axis = Vector3(0, 0, 1) # Size and position the box self.attachment.set_position(anchor + Vector3(0, 0, 0.5 * box_size)) # Create revolute joint. Remember: joint position is in child frame motor_joint = Joint( joint_type="revolute", parent=self.component, child=self.attachment, axis=axis) motor_joint.set_position(Vector3(0, 0, -0.5 * box_size)) # Set a force limit on the joint motor_joint.axis.limit = Limit(effort=0.01) self.add_joint(motor_joint) # Register a joint motor with a maximum velocity of # 50 rounds per minute (the speed is in radians per second) # We also give it a simple PID controller pid = PID(proportional_gain=1.0, integral_gain=0.1) max_speed = 2 * math.pi * 50.0 / 60 self.motors.append(VelocityMotor( part_id=self.id, motor_id="rotate", joint=motor_joint, pid=pid, min_velocity=-max_speed, max_velocity=max_speed)) self.apply_color()
def get_slot_position(self, slot): """ Return slot position """ self.check_slot(slot) vmax = WIDTH / 2.0 if slot == 0: # Front face return Vector3(0, -vmax, 0) elif slot == 1: # Back face return Vector3(0, vmax, 0) elif slot == 2: # Right face return Vector3(vmax, 0, 0) # Left face return Vector3(-vmax, 0, 0)
def get_slot_position(self, slot_id): """ Returns the slot position for the given slot. Slot 0 is defined as the top, slot 1 is the bottom. :param slot_id: :return: """ z = self.length / 2.0 return Vector3(0, 0, z if slot_id == 0 else -z)
def get_slot_position(self, slot_id): """ Modify `get_slot_position` to return the attachment of the motor instead. :param slot_id: :return: """ v = super(Wheel, self).get_slot_position(slot_id) return v + Vector3(0, 0, self.MOTOR_SIZE)
def _initialize(self, **kwargs): # Initialize the root self.root = self.create_component( Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root") # Initialize the servo servo = self.create_component( Box(SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT, MASS_SERVO)) x_servo = 0.5 * (SLOT_THICKNESS + SERVO_LENGTH) + SEPARATION # z_servo = 0.5 * (SERVO_HEIGHT - SLOT_WIDTH) + SERVO_Z_OFFSET z_servo = 0 servo.set_position(Vector3(x_servo, 0, z_servo)) # Initialize the connection self.connection = self.create_component( Box(JOINT_CONNECTION_THICKNESS, JOINT_CONNECTION_WIDTH, JOINT_CONNECTION_WIDTH, MASS_CONNECTION_SLOT), "connection") x_conn = x_servo + 0.5 * (SERVO_LENGTH - JOINT_CONNECTION_THICKNESS) + SEPARATION self.connection.set_position(Vector3(x_conn, 0, 0)) # Fix the links # root <-> servo self.fix(self.root, servo) # servo <revolute> connection self.joint = Joint("revolute", servo, self.connection, axis=Vector3(1, 0, 0)) self.joint.axis.limit = Limit( effort=constants.MAX_SERVO_TORQUE_ROTATIONAL, velocity=constants.MAX_SERVO_VELOCITY) self.add_joint(self.joint) # Now we add a motor that powers the joint. This particular servo # targets a velocity. Use a simple PID controller initially. pid = constants.SERVO_POSITION_PID self.motors.append(PositionMotor(self.id, "rotate", self.joint, pid)) # Call color mixin self.apply_color()
def get_slot_tangent(self, slot): """ Return slot tangent """ self.check_slot(slot) # The top face is tangent to all supported # slots in the plane, front, back, right, left # Since there's no particular reason for choosing any # other we just return the top face for all. return Vector3(0, 0, 1)
def get_slot_tangent(self, slot): """ The tangent vectors determine the "zero orientation", meaning if a body part has an orientation of 0 it will have its tangent vector aligned with its parent. The tangent vector has to be orthogonal to the slot normal. We have no specific orientation requirements, so we simply always return one tangent vector which is orthogonal to both slot normals, i.e. the vector (0, 0, 1). """ self.check_slot(slot) return Vector3(0, 0, 1)
def _initialize(self, **kwargs): self.radius = in_mm(kwargs['radius']) wheel = self.create_component( Cylinder(self.radius, WHEEL_THICKNESS, MASS_WHEEL), "wheel") self.root = self.create_component( Box(SLOT_WIDTH, SLOT_WIDTH, SLOT_THICKNESS, MASS_SLOT)) # Create the wheel z_wheel = 0.5 * SLOT_THICKNESS - ( SLOT_THICKNESS + SLOT_CONNECTION_THICKNESS - SLOT_WHEEL_OFFSET) wheel.set_position(Vector3(0, 0, z_wheel)) # Attach the wheel and the root with a revolute joint joint = Joint("revolute", self.root, wheel, axis=Vector3(0, 0, -1)) # TODO Adequate force limit for passive wheel joint.axis.limit = Limit(effort=constants.MAX_SERVO_TORQUE_ROTATIONAL) self.add_joint(joint) # Call color mixin self.apply_color()
def __init__(self, diameter=3.0, height=3.0, name="birth_clinic"): """ :param diameter: Intended diameter of the birth clinic :param name: :return: """ super(BirthClinic, self).__init__(name=name, static=True) self.diameter = diameter scale = diameter / MESH_DIAMETER # Cannot go higher than mesh height, or lower than the bottom # of the slice. scaled_height = scale * MESH_HEIGHT self.height = max(arg1=min(height, scaled_height), arg2=(SLICE_FRACTION * scaled_height)) mesh = Mesh("model://tol_arena/meshes/BirthClinic.dae", scale=scale) col = Collision("bc_col", mesh) surf = Element(tag_name="surface") friction = Friction( friction=0.01, friction2=0.01, slip1=1.0, slip2=1.0 ) contact = "<contact>" \ "<ode>" \ "<kd>%s</kd>" \ "<kp>%s</kp>" \ "</ode>" \ "</contact>" % ( nf(constants.SURFACE_KD), nf(constants.SURFACE_KP) ) surf.add_element(friction) surf.add_element(contact) col.add_element(surf) vis = Visual("bc_vis", mesh.copy()) self.link = Link("bc_link", elements=[col, vis]) # By default the model has 0.5 * scale * MESH_HEIGHT below # and above the surface. Translate such that we have exactly # the desired height instead. self.link.translate(Vector3(0, 0, self.height - (0.5 * scaled_height))) self.add_element(self.link)
def _initialize(self, **kwargs): """ :param kwargs: :return: """ # Initialize the root, already at 0, 0, 0 self.root = self.create_component( Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "root") # Initialize the first connection part x_part_a = 0.5 * (SLOT_THICKNESS + CONNECTION_PART_LENGTH) + SEPARATION conn_a = self.create_component( Box(CONNECTION_PART_LENGTH, SLOT_WIDTH, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_a") conn_a.set_position(Vector3(x_part_a, 0, 0)) # Initialize the second connection part x_part_b = x_part_a - CONNECTION_PART_LENGTH + 2 * CONNECTION_PART_OFFSET conn_b = self.create_component( Box(CONNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT, SLOT_WIDTH, MASS_FRAME), "conn_b") conn_b.set_position(Vector3(x_part_b, 0, 0)) # Initialize the tail x_tail = x_part_b + 0.5 * (CONNECTION_PART_LENGTH + SLOT_THICKNESS) + SEPARATION self.tail = self.create_component( Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "tail") self.tail.set_position(Vector3(x_tail, 0, 0)) # Fix the parts using joints # root <-> connection a self.fix(self.root, conn_a) # connection a <-> connection b cardan = Joint("universal", conn_a, conn_b, axis=Vector3(0, 0, 1), axis2=Vector3(0, 1, 0)) limit = Limit(lower=-constants.CARDAN_LIMIT, upper=constants.CARDAN_LIMIT) cardan.axis.limit = cardan.axis2.limit = limit cardan.set_position( Vector3(0.5 * CONNECTION_PART_LENGTH - CONNECTION_PART_OFFSET)) self.add_joint(cardan) # connection b <-> tail self.fix(conn_b, self.tail) # Apply color mixin self.apply_color()
def _sensor_helper(self, label, x_sensors, y_sensor): """ :return: """ # Visual is provided by the mesh in the root sensor_component = self.create_component(BoxGeom( SENSOR_THICKNESS, SENSOR_WIDTH, SENSOR_HEIGHT, MASS), label, visual=False) sensor_component.set_position(Vector3(x_sensors, y_sensor, 0)) # Anchor and axis are in child frame self.fix(self.component, sensor_component) # Create and add the SDF sensor contact = SdfSensor(label + "_sensor", "contact", update_rate=self.conf.sensor_update_rate) contact.add_element('<contact><collision>' + sensor_component.collision.name + '</collision></contact>') sensor_component.add_sensor(contact, "touch")
async def run(): world = await World.create() if world: print("Connected to the simulator world.") model = Model( name='sdf_model', static=True, ) model.set_position(position=Vector3(0, 0, 1)) link = Link('sdf_link') link.make_sphere( mass=10e10, radius=0.5, ) link.make_color(0.7, 0.2, 0.0, 1.0) model.add_element(link) sdf_model = SDF(elements=[model]) await world.insert_model(sdf_model) await world.pause(True) while True: await asyncio.sleep(10.0)
def _initialize(self, **kwargs): """ :param kwargs: :return: """ self.mass = MASS self.x = SENSOR_BASE_THICKNESS self.y = self.z = SENSOR_BASE_WIDTH x_sensors = 0.5 * (SENSOR_BASE_THICKNESS + SENSOR_THICKNESS) y_left_sensor = -0.5 * SENSOR_WIDTH - SEPARATION y_right_sensor = 0.5 * SENSOR_WIDTH + SEPARATION visual = Visual("touch_visual", Mesh("model://rg_robot/meshes/TouchSensor.dae")) visual.translate(Vector3(0.5 * SENSOR_THICKNESS, 0, 0)) self.component = self.create_component(BoxGeom(self.x, self.y, self.z, self.mass), "box", visual=visual) self._sensor_helper("left", x_sensors, y_left_sensor) self._sensor_helper("right", x_sensors, y_right_sensor) self.apply_color()
def get_slot_tangent(self, slot): """ Return slot tangent """ self.check_slot(slot) if slot == 0: # Front face tangent: top face return Vector3(0, 0, 1) elif slot == 1: # Back face tangent: top face return Vector3(0, 0, 1) elif slot == 2: # Top face tangent: right face return Vector3(1, 0, 0) elif slot == 3: # Bottom face tangent: right face return Vector3(1, 0, 0) elif slot == 4: # Right face tangent: back face return Vector3(0, 1, 0) # Left face tangent: back face return Vector3(0, 1, 0)
def get_slot_position(self, slot): """ Return slot position """ self.check_slot(slot) xmax, ymax, zmax = self.x / 2.0, self.y / 2.0, self.z / 2.0 if slot == 0: # Front face return Vector3(0, -ymax, 0) elif slot == 1: # Back face return Vector3(0, ymax, 0) elif slot == 2: # Top face return Vector3(0, 0, zmax) elif slot == 3: # Bottom face return Vector3(0, 0, -zmax) elif slot == 4: # Right face return Vector3(xmax, 0, 0) # Left face return Vector3(-xmax, 0, 0)
def _initialize(self, **kwargs): """ :param kwargs: :return: """ # Create components. Visuals are provided by the conn_a/conn_b meshes self.hinge_root = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_a", visual=False) self.hinge_tail = self.create_component(Box(SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, MASS_SLOT), "slot_b", visual=False) mesh = Mesh("model://rg_robot/meshes/PassiveHinge.dae") visual_a = Visual("conn_a_visual", mesh) visual_a.translate(Vector3(-0.5 * SLOT_THICKNESS)) conn_a = self.create_component(Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_a", visual=visual_a) # Flip visual along the x-axis by rotating PI degrees over z # This will put it upside down, so we also flip it PI degrees over x visual_b = Visual("conn_b_visual", mesh.copy()) visual_b.rotate_around(Vector3(1, 0, 0), math.pi, relative_to_child=False) visual_b.rotate_around(Vector3(0, 0, 1), math.pi, relative_to_child=False) visual_b.translate(Vector3(0.5 * SLOT_THICKNESS)) conn_b = self.create_component(Box(CONNECTION_PART_LENGTH, CONNECTION_PART_THICKNESS, CONNECTION_PART_HEIGHT, MASS_FRAME), "conn_b", visual=visual_b) # Shorthand for variables # Position connection part a x_part_a = SLOT_THICKNESS / 2.0 + SEPARATION + CONNECTION_PART_LENGTH / 2.0 conn_a.set_position(Vector3(x_part_a, 0, 0)) # Position connection part b x_part_b = x_part_a + ( CONNECTION_PART_LENGTH / 2.0 - (CONNECTION_PART_LENGTH - CONNECTION_ROTATION_OFFSET)) * 2 conn_b.set_position(Vector3(x_part_b, 0, 0)) # Make the tail x_tail = x_part_b + CONNECTION_PART_LENGTH / 2.0 + SEPARATION + SLOT_THICKNESS / 2.0 self.hinge_tail.set_position(Vector3(x_tail, 0, 0)) # Fix components self.fix(self.hinge_root, conn_a) self.fix(self.hinge_tail, conn_b) # Connection part a <(hinge)> connection part b # Hinge joint axis should point straight up, and anchor # the points in the center. Note that the position of a joint # is expressed in the child link frame, so we need to take the # position from the original code and subtract conn_b's position self.joint = Joint("revolute", conn_a, conn_b, axis=Vector3(0, 0, 1)) self.joint.set_position( Vector3(CONNECTION_PART_LENGTH / 2.0 - CONNECTION_ROTATION_OFFSET, 0, 0)) self.joint.axis.limit = Limit(upper=constants.HINGE_LIMIT, lower=-constants.HINGE_LIMIT) self.add_joint(self.joint) # Apply color mixin self.apply_color()
def get_slot_tangent(self, slot_id): self.check_slot(slot_id) return Vector3(0, 0, 1)
def get_slot_tangent(self, slot): self.check_slot(slot) return Vector3(0, 1, 0)