Source code for pagoda.physics

'''This module contains convenience wrappers for ODE objects.'''

from __future__ import division

import collections
import numpy as np
import ode


BodyState = collections.namedtuple(
    'BodyState', 'name position quaternion linear_velocity angular_velocity')


class Registrar(type):
    '''A metaclass that builds a registry of its subclasses.'''

    def __init__(cls, name, bases, dct):
        if not hasattr(cls, '_registry'):
            cls._registry = {}
        else:
            key = name.lower()
            for i in range(3, len(name) + 1):
                cls._registry[key[:i]] = cls
        super(Registrar, cls).__init__(name, bases, dct)

    def build(cls, key, *args, **kwargs):
        return cls._registry[key.lower()](*args, **kwargs)


[docs]class Body(Registrar(str('Base'), (), {})): '''This class wraps things that participate in the ODE physics simulation. This class basically provides lots of Python-specific properties that call the equivalent ODE getters and setters for things like position, rotation, etc. '''
[docs] def __init__(self, name, world, density=1000., mass=None, **shape): self.name = name self.world = world self.shape = shape m = ode.Mass() self.init_mass(m, density, mass) self.ode_body = ode.Body(world.ode_world) self.ode_body.setMass(m) self.ode_geom = getattr(ode, 'Geom%s' % self.__class__.__name__)( world.ode_space, **shape) self.ode_geom.setBody(self.ode_body)
def __str__(self): return '{0.__class__.__name__} {0.name} at {1}'.format( self, self.position.round(3)) @property def mass(self): '''The ODE mass object for this body.''' return self.ode_body.getMass() @property def state(self): '''The state of this body includes: - name of the body (str) - position (3-tuple) - quaternion (4-tuple) - linear velocity (3-tuple) - angular velocity (3-tuple) ''' return BodyState(self.name, tuple(self.position), tuple(self.quaternion), tuple(self.linear_velocity), tuple(self.angular_velocity)) @state.setter def state(self, state): '''Set the state of this body. Parameters ---------- state : BodyState tuple The desired state of the body. ''' assert self.name == state.name, \ 'state name "{}" != body name "{}"'.format(state.name, self.name) self.position = state.position self.quaternion = state.quaternion self.linear_velocity = state.linear_velocity self.angular_velocity = state.angular_velocity @property def position(self): '''The (x, y, z) coordinates of the center of this body.''' return np.array(self.ode_body.getPosition()) @position.setter def position(self, position): '''Set the (x, y, z) coordinates of the center of this body. Parameters ---------- position : 3-tuple of float The coordinates of the desired center of this body. ''' self.ode_body.setPosition(tuple(position)) @property def rotation(self): '''The rotation matrix for this body.''' return np.array(self.ode_body.getRotation()).reshape((3, 3)) @rotation.setter def rotation(self, rotation): '''Set the rotation of this body using a rotation matrix. Parameters ---------- rotation : sequence of 9 floats The desired rotation matrix for this body. ''' if isinstance(rotation, np.ndarray): rotation = rotation.ravel() self.ode_body.setRotation(tuple(rotation)) @property def quaternion(self): '''The (w, x, y, z) rotation quaternion for this body.''' return np.array(self.ode_body.getQuaternion()) @quaternion.setter def quaternion(self, quaternion): self.ode_body.setQuaternion(tuple(quaternion)) @property def linear_velocity(self): '''Current linear velocity of this body (in world coordinates).''' return np.array(self.ode_body.getLinearVel()) @linear_velocity.setter def linear_velocity(self, velocity): '''Set the linear velocity for this body. Parameters ---------- velocity : 3-tuple of float The desired velocity for this body, in world coordinates. ''' self.ode_body.setLinearVel(tuple(velocity)) @property def angular_velocity(self): '''Current angular velocity of this body (in world coordinates).''' return np.array(self.ode_body.getAngularVel()) @angular_velocity.setter def angular_velocity(self, velocity): '''Set the angular velocity for this body. Parameters ---------- velocity : 3-tuple of float The desired angular velocity for this body, in world coordinates. ''' self.ode_body.setAngularVel(tuple(velocity)) @property def force(self): '''Current net force acting on this body (in world coordinates).''' return np.array(self.ode_body.getForce()) @force.setter def force(self, force): '''Set the force acting on this body. Parameters ---------- force : 3-tuple of float The desired force acting on this body, in world coordinates. ''' self.ode_body.setForce(tuple(force)) @property def torque(self): '''Current net torque acting on this body (in world coordinates).''' return np.array(self.ode_body.getTorque()) @torque.setter def torque(self, torque): '''Set the torque acting on this body. Parameters ---------- torque : 3-tuple of float The desired torque acting on this body, in world coordinates. ''' self.ode_body.setTorque(tuple(torque)) @property def is_kinematic(self): '''True iff this body is kinematic.''' return self.ode_body.isKinematic() @is_kinematic.setter def is_kinematic(self, is_kinematic): '''Set the kinematic/dynamic attribute for this body. In pagoda, kinematic bodies have infinite mass and do interact with other bodies via collisions. Parameters ---------- is_kinematic : bool If True, this body will be set to kinematic. If False, it will be set to dynamic. ''' if is_kinematic: self.ode_body.setKinematic() else: self.ode_body.setDynamic() @property def follows_gravity(self): '''True iff this body follows gravity.''' return self.ode_body.getGravityMode() @follows_gravity.setter def follows_gravity(self, follows_gravity): '''Set whether this body follows gravity. Parameters ---------- follows_gravity : bool This body will follow gravity iff this parameter is True. ''' self.ode_body.setGravityMode(follows_gravity)
[docs] def rotate_to_body(self, x): '''Rotate the given vector to the same orientation as this body. Parameters ---------- x : 3-tuple of float A point in three dimensions. Returns ------- xrot : 3-tuple of float The same point after rotation into the orientation of this body. ''' return np.dot(x, self.rotation)
[docs] def body_to_world(self, position): '''Convert a body-relative offset to world coordinates. Parameters ---------- position : 3-tuple of float A tuple giving body-relative offsets. Returns ------- position : 3-tuple of float A tuple giving the world coordinates of the given offset. ''' return np.array(self.ode_body.getRelPointPos(tuple(position)))
[docs] def world_to_body(self, position): '''Convert a point in world coordinates to a body-relative offset. Parameters ---------- position : 3-tuple of float A world coordinates position. Returns ------- offset : 3-tuple of float A tuple giving the body-relative offset of the given position. ''' return np.array(self.ode_body.getPosRelPoint(tuple(position)))
[docs] def relative_offset_to_world(self, offset): '''Convert a relative body offset to world coordinates. Parameters ---------- offset : 3-tuple of float The offset of the desired point, given as a relative fraction of the size of this body. For example, offset (0, 0, 0) is the center of the body, while (0.5, -0.2, 0.1) describes a point halfway from the center towards the maximum x-extent of the body, 20% of the way from the center towards the minimum y-extent, and 10% of the way from the center towards the maximum z-extent. Returns ------- position : 3-tuple of float A position in world coordinates of the given body offset. ''' return np.array(self.body_to_world(offset * self.dimensions / 2))
[docs] def add_force(self, force, relative=False, position=None, relative_position=None): '''Add a force to this body. Parameters ---------- force : 3-tuple of float A vector giving the forces along each world or body coordinate axis. relative : bool, optional If False, the force values are assumed to be given in the world coordinate frame. If True, they are assumed to be given in the body-relative coordinate frame. Defaults to False. position : 3-tuple of float, optional If given, apply the force at this location in world coordinates. Defaults to the current position of the body. relative_position : 3-tuple of float, optional If given, apply the force at this relative location on the body. If given, this method ignores the ``position`` parameter. ''' b = self.ode_body if relative_position is not None: op = b.addRelForceAtRelPos if relative else b.addForceAtRelPos op(force, relative_position) elif position is not None: op = b.addRelForceAtPos if relative else b.addForceAtPos op(force, position) else: op = b.addRelForce if relative else b.addForce op(force)
[docs] def add_torque(self, torque, relative=False): '''Add a torque to this body. Parameters ---------- force : 3-tuple of float A vector giving the torque along each world or body coordinate axis. relative : bool, optional If False, the torque values are assumed to be given in the world coordinate frame. If True, they are assumed to be given in the body-relative coordinate frame. Defaults to False. ''' op = self.ode_body.addRelTorque if relative else self.ode_body.addTorque op(torque)
[docs] def join_to(self, joint, other_body=None, **kwargs): '''Connect this body to another one using a joint. This method creates a joint to fasten this body to the other one. See :func:`World.join`. Parameters ---------- joint : str The type of joint to use when connecting these bodies. other_body : :class:`Body` or str, optional The other body to join with this one. If not given, connects this body to the world. ''' self.world.join(joint, self, other_body, **kwargs)
[docs] def connect_to(self, joint, other_body, offset=(0, 0, 0), other_offset=(0, 0, 0), **kwargs): '''Move another body next to this one and join them together. This method will move the ``other_body`` so that the anchor points for the joint coincide. It then creates a joint to fasten the two bodies together. See :func:`World.move_next_to` and :func:`World.join`. Parameters ---------- joint : str The type of joint to use when connecting these bodies. other_body : :class:`Body` or str The other body to join with this one. offset : 3-tuple of float, optional The body-relative offset where the anchor for the joint should be placed. Defaults to (0, 0, 0). See :func:`World.move_next_to` for a description of how offsets are specified. other_offset : 3-tuple of float, optional The offset on the second body where the joint anchor should be placed. Defaults to (0, 0, 0). Like ``offset``, this is given as an offset relative to the size and shape of ``other_body``. ''' anchor = self.world.move_next_to(self, other_body, offset, other_offset) self.world.join(joint, self, other_body, anchor=anchor, **kwargs)
[docs]class Box(Body): @property def lengths(self): return self.shape['lengths'] @property def dimensions(self): return np.array(self.lengths).squeeze() @property def volume(self): return np.prod(self.lengths) def init_mass(self, m, density, mass): if mass: density = mass / self.volume m.setBox(density, *self.lengths)
[docs]class Sphere(Body): @property def radius(self): return self.shape['radius'] @property def dimensions(self): d = 2 * self.radius return np.array([d, d, d]).squeeze() @property def volume(self): return 4 / 3 * np.pi * self.radius ** 3 def init_mass(self, m, density, mass): if mass: density = mass / self.volume m.setSphere(density, self.radius)
[docs]class Cylinder(Body): @property def radius(self): return self.shape['radius'] @property def length(self): return self.shape['length'] @property def dimensions(self): d = 2 * self.radius return np.array([d, d, self.length]).squeeze() @property def volume(self): return self.length * np.pi * self.radius ** 2 def init_mass(self, m, density, mass): if mass: density = mass / self.volume m.setCylinder(density, 3, self.radius, self.length)
[docs]class Capsule(Body): @property def radius(self): return self.shape['radius'] @property def length(self): return self.shape['length'] @property def dimensions(self): d = 2 * self.radius return np.array([d, d, d + self.length]).squeeze() @property def volume(self): return 4 / 3 * np.pi * self.radius ** 3 + \ self.length * np.pi * self.radius ** 2 def init_mass(self, m, density, mass): if mass: density = mass / self.volume m.setCapsule(density, 3, self.radius, self.length)
def _get_params(target, param, dof): '''Get the given param from each of the DOFs for a joint.''' return [target.getParam(getattr(ode, 'Param{}{}'.format(param, s))) for s in ['', '2', '3'][:dof]] def _set_params(target, param, values, dof): '''Set the given param for each of the DOFs for a joint.''' if not isinstance(values, (list, tuple, np.ndarray)): values = [values] * dof assert dof == len(values) for s, value in zip(['', '2', '3'][:dof], values): target.setParam(getattr(ode, 'Param{}{}'.format(param, s)), value)
[docs]class Joint(Registrar(str('Base'), (), {})): '''Base class for joints connecting two bodies. In ODE, :class:`Body` objects represent mass/inertia properties, while :class:`Joint` and :class:`Motor` objects represent mathematical constraints that govern how specific pairs of bodies interact. For example, a :class:`BallJoint` that connects two bodies will force the anchor point for those two bodies to remain in the same location in world coordinates -- any linear force that displaces one of the bodies will also cause a force to be applied to the second body, because of the constraint imposed by the ball joint. As another example, a :class:`Slider` that connects two bodies allows those two bodies to displace relative to one another along a single axis, but not to rotate with respect to one another -- any torque applied to one body will also cause a torque to be applied to the other body. Constraints can be applied to angular degrees of freedom (e.g., :class:`AMotor`), linear degrees of freedom (e.g., :class:`BallJoint`, :class:`LMotor`), or both (e.g., :class:`PistonJoint`). Both joints and motors apply constraints to pairs of bodies, but they are quite different in many ways and so are represented using specific subclasses. This superclass is just a mixin to avoid repeating the getters and setters that are common between motors and joints. ''' ADOF = 0 LDOF = 0 @property def feedback(self): '''Feedback buffer (list of 3-tuples) for this ODE motor/joint.''' return self.ode_obj.getFeedback() @property def positions(self): '''List of positions for linear degrees of freedom.''' return [self.ode_obj.getPosition(i) for i in range(self.LDOF)] @property def position_rates(self): '''List of position rates for linear degrees of freedom.''' return [self.ode_obj.getPositionRate(i) for i in range(self.LDOF)] @property def angles(self): '''List of angles for rotational degrees of freedom.''' return [self.ode_obj.getAngle(i) for i in range(self.ADOF)] @property def angle_rates(self): '''List of angle rates for rotational degrees of freedom.''' return [self.ode_obj.getAngleRate(i) for i in range(self.ADOF)] @property def axes(self): '''List of axes for this object's degrees of freedom.''' return [np.array(self.ode_obj.getAxis(i)) for i in range(self.ADOF or self.LDOF)] @axes.setter def axes(self, axes): '''Set the axes for this object's degrees of freedom. Parameters ---------- axes : list of axes specifications A list of axis values to set. This list must have the same number of elements as the degrees of freedom of the underlying ODE object. Each element can be (a) None, which has no effect on the corresponding axis, or (b) three floats specifying the axis to set. ''' assert self.ADOF == len(axes) or self.LDOF == len(axes) for i, axis in enumerate(axes): if axis is not None: self.ode_obj.setAxis(i, 0, axis) @property def lo_stops(self): '''List of lo stop values for this object's degrees of freedom.''' return _get_params(self.ode_obj, 'LoStop', self.ADOF + self.LDOF) @lo_stops.setter def lo_stops(self, lo_stops): '''Set the lo stop values for this object's degrees of freedom. Parameters ---------- lo_stops : float or sequence of float A lo stop value to set on all degrees of freedom, or a list containing one such value for each degree of freedom. For rotational degrees of freedom, these values must be in radians. ''' _set_params(self.ode_obj, 'LoStop', lo_stops, self.ADOF + self.LDOF) @property def hi_stops(self): '''List of hi stop values for this object's degrees of freedom.''' return _get_params(self.ode_obj, 'HiStop', self.ADOF + self.LDOF) @hi_stops.setter def hi_stops(self, hi_stops): '''Set the hi stop values for this object's degrees of freedom. Parameters ---------- hi_stops : float or sequence of float A hi stop value to set on all degrees of freedom, or a list containing one such value for each degree of freedom. For rotational degrees of freedom, these values must be in radians. ''' _set_params(self.ode_obj, 'HiStop', hi_stops, self.ADOF + self.LDOF) @property def velocities(self): '''List of target velocity values for rotational degrees of freedom.''' return _get_params(self.ode_obj, 'Vel', self.ADOF + self.LDOF) @velocities.setter def velocities(self, velocities): '''Set the target velocities for this object's degrees of freedom. Parameters ---------- velocities : float or sequence of float A target velocity value to set on all degrees of freedom, or a list containing one such value for each degree of freedom. For rotational degrees of freedom, these values must be in radians / second. ''' _set_params(self.ode_obj, 'Vel', velocities, self.ADOF + self.LDOF) @property def max_forces(self): '''List of max force values for rotational degrees of freedom.''' return _get_params(self.ode_obj, 'FMax', self.ADOF + self.LDOF) @max_forces.setter def max_forces(self, max_forces): '''Set the maximum forces for this object's degrees of freedom. Parameters ---------- max_forces : float or sequence of float A maximum force value to set on all degrees of freedom, or a list containing one such value for each degree of freedom. ''' _set_params(self.ode_obj, 'FMax', max_forces, self.ADOF + self.LDOF) @property def erps(self): '''List of ERP values for this object's degrees of freedom.''' return _get_params(self.ode_obj, 'ERP', self.ADOF + self.LDOF) @erps.setter def erps(self, erps): '''Set the ERP values for this object's degrees of freedom. Parameters ---------- erps : float or sequence of float An ERP value to set on all degrees of freedom, or a list containing one such value for each degree of freedom. ''' _set_params(self.ode_obj, 'ERP', erps, self.ADOF + self.LDOF) @property def cfms(self): '''List of CFM values for this object's degrees of freedom.''' return _get_params(self.ode_obj, 'CFM', self.ADOF + self.LDOF) @cfms.setter def cfms(self, cfms): '''Set the CFM values for this object's degrees of freedom. Parameters ---------- cfms : float or sequence of float A CFM value to set on all degrees of freedom, or a list containing one such value for each degree of freedom. ''' _set_params(self.ode_obj, 'CFM', cfms, self.ADOF + self.LDOF) @property def stop_cfms(self): '''List of lo/hi stop CFM values.''' return _get_params(self.ode_obj, 'StopCFM', self.ADOF + self.LDOF) @stop_cfms.setter def stop_cfms(self, stop_cfms): '''Set the CFM values for this object's DOF limits. Parameters ---------- stop_cfms : float or sequence of float A CFM value to set on all degrees of freedom limits, or a list containing one such value for each degree of freedom limit. ''' _set_params(self.ode_obj, 'StopCFM', stop_cfms, self.ADOF + self.LDOF) @property def stop_erps(self): '''List of lo/hi stop ERP values.''' return _get_params(self.ode_obj, 'StopERP', self.ADOF + self.LDOF) @stop_erps.setter def stop_erps(self, stop_erps): '''Set the ERP values for this object's DOF limits. Parameters ---------- stop_erps : float or sequence of float An ERP value to set on all degrees of freedom limits, or a list containing one such value for each degree of freedom limit. ''' _set_params(self.ode_obj, 'StopERP', stop_erps, self.ADOF + self.LDOF)
[docs] def enable_feedback(self): '''Enable feedback on this ODE object.''' self.ode_obj.setFeedback(True)
[docs] def disable_feedback(self): '''Disable feedback on this ODE object.''' self.ode_obj.setFeedback(False)
class Dynamic(Joint): '''This class wraps an ODE motor -- either an LMotor or an AMotor. Parameters ---------- name : str A name for this object in the world. world : :class:`World` A world object to which this motor belongs. body_a : :class:`Body` A first body connected to this joint. body_b : :class:`Body`, optional A second body connected to this joint. If not given, the joint will connect the first body to the world. feedback : bool, optional Feedback will be enabled on this motor iff this is True. Defaults to False. dof : int, optional Number of degrees of freedom in this motor. Defaults to 3. jointgroup : ode.JointGroup, optional A joint group to which this motor belongs. Defaults to the default joint group in the world. ''' def __init__(self, name, world, body_a, body_b=None, feedback=False, dof=3, jointgroup=None): self.name = name self.ode_obj = self.MOTOR_FACTORY(world.ode_world, jointgroup=jointgroup) self.ode_obj.attach(body_a.ode_body, body_b.ode_body if body_b else None) self.ode_obj.setNumAxes(dof) self.cfms = 1e-8 if feedback: self.enable_feedback() else: self.disable_feedback()
[docs]class AMotor(Dynamic): '''An angular motor applies torques to change an angle in the physics world. AMotors can be created in "user" mode---in which case the user must supply all axis and angle values---or, for 3-DOF motors, in "euler" mode---in which case the first and last axes must be specified, and ODE computes the middle axis automatically. ''' MOTOR_FACTORY = ode.AMotor
[docs] def __init__(self, *args, **kwargs): mode = kwargs.pop('mode', 'user') if isinstance(mode, str): mode = ode.AMotorEuler if mode.lower() == 'euler' else ode.AMotorUser super(AMotor, self).__init__(*args, **kwargs) self.ode_obj.setMode(mode)
@property def ADOF(self): '''Number of angular degrees of freedom for this motor.''' return self.ode_obj.getNumAxes() @property def axes(self): '''List of axes for this object's degrees of freedom.''' return [np.array(self.ode_obj.getAxis(i)) for i in range(self.ADOF)] @axes.setter def axes(self, axes): '''Set the axes for this object's degrees of freedom. Parameters ---------- axes : list of axis parameters A list of axis values to set. This list must have the same number of elements as the degrees of freedom of the underlying ODE object. Each element can be (a) None, which has no effect on the corresponding axis, or (b) three floats specifying the axis to set, or (c) a dictionary with an "axis" key specifying the axis to set and an optional "rel" key (defaults to 0) specifying the relative body to set the axis on. ''' assert len(axes) == self.ADOF for i, ax in enumerate(axes): if ax is None: continue if not isinstance(ax, dict): ax = dict(axis=ax) self.ode_obj.setAxis(i, ax.get('rel', 0), ax['axis'])
[docs] def add_torques(self, torques): '''Add the given torques along this motor's axes. Parameters ---------- torques : sequence of float A sequence of torque values to apply to this motor's axes. ''' self.ode_obj.addTorques(*torques)
[docs]class LMotor(Dynamic): '''An LMotor applies forces to change a position in the physics world.''' MOTOR_FACTORY = ode.LMotor @property def LDOF(self): '''Number of linear degrees of freedom for this motor.''' return self.ode_obj.getNumAxes()
class Kinematic(Joint): '''This class wraps kinematic ODE joints with some Python properties. Parameters ---------- name : str Name of the joint to create. This is only to make the joint discoverable in the world. world : :class:`World` Wrapper for the world in which this joint exists. body_a : :class:`Body` Wrapper for the first body that this joint connects. body_b : :class:`Body`, optional Wrapper for the second body that this joint connects. If this is None, the joint will connect ``body_a`` to the ``world``. anchor : 3-tuple of floats, optional Anchor in world coordinates for the joint. Optional for :class:`Fixed` joint. feedback : bool, optional If this is True, a force feedback structure will be enabled for this joint, which will make it possible to record the forces that this joint exerts on its two bodies. By default, no structure will be allocated. jointgroup : ODE joint group, optional Add the joint to this group. Defaults to the default world joint group. ''' def __init__(self, name, world, body_a, body_b=None, anchor=None, feedback=False, jointgroup=None, amotor=True, lmotor=True): self.name = name build = getattr(ode, '{}Joint'.format(self.__class__.__name__)) self.ode_obj = build(world.ode_world, jointgroup=jointgroup) self.ode_obj.attach(body_a.ode_body, body_b.ode_body if body_b else None) if anchor is not None: self.ode_obj.setAnchor(tuple(anchor)) self.ode_obj.setParam(ode.ParamCFM, 0) self.amotor = None if self.ADOF > 0 and amotor: self.amotor = AMotor(name=name + ':amotor', world=world, body_a=body_a, body_b=body_b, feedback=feedback, jointgroup=jointgroup, dof=self.ADOF, mode='euler' if self.ADOF == 3 else 'user') self.lmotor = None if self.LDOF > 0 and lmotor: self.lmotor = LMotor(name=name + ':lmotor', world=world, body_a=body_a, body_b=body_b, feedback=feedback, jointgroup=jointgroup, dof=self.LDOF) if feedback: self.enable_feedback() else: self.disable_feedback() def __str__(self): return self.name @property def anchor(self): '''3-tuple specifying location of this joint's anchor.''' return np.array(self.ode_obj.getAnchor()) @property def anchor2(self): '''3-tuple specifying location of the anchor on the second body.''' return np.array(self.ode_obj.getAnchor2()) def add_torques(self, *torques): '''Add the given torques along this joint's axes. Parameters ---------- torques : sequence of float A sequence of torque values to apply to this motor's axes. ''' self.amotor.add_torques(*torques)
[docs]class Fixed(Kinematic): ADOF = 0 LDOF = 0
[docs]class Slider(Kinematic): ADOF = 0 LDOF = 1 @property def positions(self): '''List of positions for this joint's linear degrees of freedom.''' return [self.ode_obj.getPosition()] @property def position_rates(self): '''List of position rates for this joint's degrees of freedom.''' return [self.ode_obj.getPositionRate()] @property def axes(self): '''Axis of displacement for this joint.''' return [np.array(self.ode_obj.getAxis())] @axes.setter def axes(self, axes): '''Set the linear axis of displacement for this joint. Parameters ---------- axes : list containing one 3-tuple of floats A list of the axes for this joint. For a slider joint, which has one degree of freedom, this must contain one 3-tuple specifying the X, Y, and Z axis for the joint. ''' self.lmotor.axes = [axes[0]] self.ode_obj.setAxis(tuple(axes[0]))
[docs]class Hinge(Kinematic): ADOF = 1 LDOF = 0 @property def angles(self): '''List of angles for this joint's rotational degrees of freedom.''' return [self.ode_obj.getAngle()] @property def angle_rates(self): '''List of angle rates for this joint's degrees of freedom.''' return [self.ode_obj.getAngleRate()] @property def axes(self): '''Axis of rotation for this joint.''' return [np.array(self.ode_obj.getAxis())] @axes.setter def axes(self, axes): '''Set the angular axis of rotation for this joint. Parameters ---------- axes : list containing one 3-tuple of floats A list of the axes for this joint. For a hinge joint, which has one degree of freedom, this must contain one 3-tuple specifying the X, Y, and Z axis for the joint. ''' self.amotor.axes = [axes[0]] self.ode_obj.setAxis(tuple(axes[0]))
[docs]class Piston(Kinematic): ADOF = 1 LDOF = 1 @property def axes(self): '''Axis of rotation and displacement for this joint.''' return [np.array(self.ode_obj.getAxis())] @axes.setter def axes(self, axes): self.amotor.axes = [axes[0]] self.lmotor.axes = [axes[0]] self.ode_obj.setAxis(axes[0])
[docs]class Universal(Kinematic): ADOF = 2 LDOF = 0 @property def axes(self): '''A list of axes of rotation for this joint.''' return [np.array(self.ode_obj.getAxis1()), np.array(self.ode_obj.getAxis2())] @axes.setter def axes(self, axes): self.amotor.axes = [axes[0], axes[1]] setters = [self.ode_obj.setAxis1, self.ode_obj.setAxis2] for axis, setter in zip(axes, setters): if axis is not None: setter(tuple(axis)) @property def angles(self): '''A list of two angles for this joint's degrees of freedom.''' return [self.ode_obj.getAngle1(), self.ode_obj.getAngle2()] @property def angle_rates(self): '''A list of two angle rates for this joint's degrees of freedom.''' return [self.ode_obj.getAngle1Rate(), self.ode_obj.getAngle2Rate()]
[docs]class Ball(Kinematic): ADOF = 3 LDOF = 0
[docs] def __init__(self, name, *args, **kwargs): super(Ball, self).__init__(name, *args, **kwargs) # we augment ball joints with an additional motor that allows us to set # rotation limits. keys = 'name world body_a body_b feedback dof jointgroup'.split() self.alimit = AMotor(name + ':alimit', *args, dof=self.ADOF, mode='euler', **{k: v for k, v in kwargs.items() if k in keys})
@property def angles(self): return self.alimit.angles @property def angle_rates(self): return self.alimit.angle_rates @property def axes(self): return self.alimit.axes @axes.setter def axes(self, axes): if len(axes) == 2: axes = dict(rel=1, axis=axes[0]), None, dict(rel=2, axis=axes[1]) self.amotor.axes = axes self.alimit.axes = axes @property def lo_stops(self): return self.alimit.lo_stops @lo_stops.setter def lo_stops(self, lo_stops): self.alimit.lo_stops = lo_stops @property def hi_stops(self): return self.alimit.hi_stops @hi_stops.setter def hi_stops(self, hi_stops): self.alimit.hi_stops = hi_stops
[docs]def make_quaternion(theta, *axis): '''Given an angle and an axis, create a quaternion.''' x, y, z = axis r = np.sqrt(x * x + y * y + z * z) st = np.sin(theta / 2.) ct = np.cos(theta / 2.) return [x * st / r, y * st / r, z * st / r, ct]
[docs]def center_of_mass(bodies): '''Given a set of bodies, compute their center of mass in world coordinates. ''' x = np.zeros(3.) t = 0. for b in bodies: m = b.mass x += b.body_to_world(m.c) * m.mass t += m.mass return x / t
[docs]class World(object): '''A wrapper for an ODE World object, for running in a simulator.'''
[docs] def __init__(self, dt=1. / 60, max_angular_speed=20): self.ode_world = ode.World() self.ode_world.setMaxAngularSpeed(max_angular_speed) self.ode_space = ode.QuadTreeSpace((0, 0, 0), (100, 100, 20), 10) self.ode_floor = ode.GeomPlane(self.ode_space, (0, 0, 1), 0) self.ode_contactgroup = ode.JointGroup() self.frame_no = 0 self.dt = dt self.elasticity = 0.1 self.friction = 2000 self.gravity = 0, 0, -9.81 self.cfm = 1e-6 self.erp = 0.7 self._bodies = {} self._joints = {}
@property def gravity(self): '''Current gravity vector in the world.''' return self.ode_world.getGravity() @gravity.setter def gravity(self, gravity): '''Set the gravity vector in the world. Parameters ---------- gravity : 3-tuple of float The vector where gravity should point. ''' return self.ode_world.setGravity(gravity) @property def cfm(self): '''Current global CFM value.''' return self.ode_world.getCFM() @cfm.setter def cfm(self, cfm): '''Set the global CFM value. Parameters ---------- cfm : float The desired global CFM value. ''' return self.ode_world.setCFM(cfm) @property def erp(self): '''Current global ERP value.''' return self.ode_world.getERP() @erp.setter def erp(self, erp): '''Set the global ERP value. Parameters ---------- erp : float The desired global ERP value. ''' return self.ode_world.setERP(erp) @property def bodies(self): '''Sequence of all bodies in the world, sorted by name.''' for k in sorted(self._bodies): yield self._bodies[k] @property def joints(self): '''Sequence of all joints in the world, sorted by name.''' for k in sorted(self._joints): yield self._joints[k]
[docs] def get_body(self, key): '''Get a body by key. Parameters ---------- key : str, None, or :class:`Body` The key for looking up a body. If this is None or a :class:`Body` instance, the key itself will be returned. Returns ------- body : :class:`Body` The body in the world with the given key. ''' return self._bodies.get(key, key)
[docs] def get_joint(self, key): '''Get a joint by key. Parameters ---------- key : str The key for a joint to look up. Returns ------- joint : :class:`Joint` The joint in the world with the given key, or None if there is no such joint. ''' return self._joints.get(key, None)
[docs] def create_body(self, shape, name=None, **kwargs): '''Create a new body. Parameters ---------- shape : str The "shape" of the body to be created. This should name a type of body object, e.g., "box" or "cap". name : str, optional The name to use for this body. If not given, a default name will be constructed of the form "{shape}{# of objects in the world}". Returns ------- body : :class:`Body` The created body object. ''' shape = shape.lower() if name is None: for i in range(1 + len(self._bodies)): name = '{}{}'.format(shape, i) if name not in self._bodies: break self._bodies[name] = Body.build(shape, name, self, **kwargs) return self._bodies[name]
[docs] def join(self, shape, body_a, body_b=None, name=None, **kwargs): '''Create a new joint that connects two bodies together. Parameters ---------- shape : str The "shape" of the joint to use for joining together two bodies. This should name a type of joint, such as "ball" or "piston". body_a : str or :class:`Body` The first body to join together with this joint. If a string is given, it will be used as the name of a body to look up in the world. body_b : str or :class:`Body`, optional If given, identifies the second body to join together with ``body_a``. If not given, ``body_a`` is joined to the world. name : str, optional If given, use this name for the created joint. If not given, a name will be constructed of the form "{body_a.name}^{shape}^{body_b.name}". Returns ------- joint : :class:`Joint` The joint object that was created. ''' ba = self.get_body(body_a) bb = self.get_body(body_b) shape = shape.lower() if name is None: name = '{}^{}^{}'.format(ba.name, shape, bb.name if bb else '') self._joints[name] = Joint.build( shape, name, self, body_a=ba, body_b=bb, **kwargs) return self._joints[name]
[docs] def move_next_to(self, body_a, body_b, offset_a, offset_b): '''Move one body to be near another one. After moving, the location described by ``offset_a`` on ``body_a`` will be coincident with the location described by ``offset_b`` on ``body_b``. Parameters ---------- body_a : str or :class:`Body` The body to use as a reference for moving the other body. If this is a string, it is treated as the name of a body to look up in the world. body_b : str or :class:`Body` The body to move next to ``body_a``. If this is a string, it is treated as the name of a body to look up in the world. offset_a : 3-tuple of float The offset of the anchor point, given as a relative fraction of the size of ``body_a``. See :func:`Body.relative_offset_to_world`. offset_b : 3-tuple of float The offset of the anchor point, given as a relative fraction of the size of ``body_b``. Returns ------- anchor : 3-tuple of float The location of the shared point, which is often useful to use as a joint anchor. ''' ba = self.get_body(body_a) bb = self.get_body(body_b) if ba is None: return bb.relative_offset_to_world(offset_b) if bb is None: return ba.relative_offset_to_world(offset_a) anchor = ba.relative_offset_to_world(offset_a) offset = bb.relative_offset_to_world(offset_b) bb.position = bb.position + anchor - offset return anchor
[docs] def get_body_states(self): '''Return the complete state of all bodies in the world. Returns ------- states : list of state information tuples A list of body state information for each body in the world. See :func:`Body.state`. ''' return [b.state for b in self.bodies]
[docs] def set_body_states(self, states): '''Set the states of some bodies in the world. Parameters ---------- states : sequence of states A complete state tuple for one or more bodies in the world. See :func:`get_body_states`. ''' for state in states: self.get_body(state.name).state = state
[docs] def step(self, substeps=2): '''Step the world forward by one frame. Parameters ---------- substeps : int, optional Split the step into this many sub-steps. This helps to prevent the time delta for an update from being too large. ''' self.frame_no += 1 dt = self.dt / substeps for _ in range(substeps): self.ode_contactgroup.empty() self.ode_space.collide(None, self.on_collision) self.ode_world.step(dt)
[docs] def needs_reset(self): '''Return True iff the world needs to be reset.''' return False
[docs] def reset(self): '''Reset the state of the world.''' pass
[docs] def on_key_press(self, key, modifiers, keymap): '''Handle an otherwise unhandled keypress event (from a GUI).''' if key == keymap.ENTER: self.reset() return True
[docs] def are_connected(self, body_a, body_b): '''Determine whether the given bodies are currently connected. Parameters ---------- body_a : str or :class:`Body` One body to test for connectedness. If this is a string, it is treated as the name of a body to look up. body_b : str or :class:`Body` One body to test for connectedness. If this is a string, it is treated as the name of a body to look up. Returns ------- connected : bool Return True iff the two bodies are connected. ''' return bool(ode.areConnected( self.get_body(body_a).ode_body, self.get_body(body_b).ode_body))
[docs] def on_collision(self, args, geom_a, geom_b): '''Callback function for the collide() method. Parameters ---------- args : None Arguments passed when the callback was registered. Not used. geom_a : ODE geometry The geometry object of one of the bodies that has collided. geom_b : ODE geometry The geometry object of one of the bodies that has collided. ''' body_a = geom_a.getBody() body_b = geom_b.getBody() if ode.areConnected(body_a, body_b) or \ (body_a and body_a.isKinematic()) or \ (body_b and body_b.isKinematic()): return for c in ode.collide(geom_a, geom_b): c.setBounce(self.elasticity) c.setMu(self.friction) ode.ContactJoint(self.ode_world, self.ode_contactgroup, c).attach( geom_a.getBody(), geom_b.getBody())