Source code for pagoda.skeleton

'''Articulated "skeleton" class and associated helper functions.'''

import climate
import numpy as np
import ode

from . import parser
from . import physics

logging = climate.get_logger(__name__)


[docs]def pid(kp=0., ki=0., kd=0., smooth=0.1): r'''Create a callable that implements a PID controller. A PID controller returns a control signal :math:`u(t)` given a history of error measurements :math:`e(0) \dots e(t)`, using proportional (P), integral (I), and derivative (D) terms, according to: .. math:: u(t) = kp * e(t) + ki * \int_{s=0}^t e(s) ds + kd * \frac{de(s)}{ds}(t) The proportional term is just the current error, the integral term is the sum of all error measurements, and the derivative term is the instantaneous derivative of the error measurement. Parameters ---------- kp : float The weight associated with the proportional term of the PID controller. ki : float The weight associated with the integral term of the PID controller. kd : float The weight associated with the derivative term of the PID controller. smooth : float in [0, 1] Derivative values will be smoothed with this exponential average. A value of 1 never incorporates new derivative information, a value of 0.5 uses the mean of the historic and new information, and a value of 0 discards historic information (i.e., the derivative in this case will be unsmoothed). The default is 0.1. Returns ------- controller : callable (float, float) -> float Returns a function that accepts an error measurement and a delta-time value since the previous measurement, and returns a control signal. ''' state = dict(p=0, i=0, d=0) def control(error, dt=1): state['d'] = smooth * state['d'] + (1 - smooth) * (error - state['p']) / dt state['i'] += error * dt state['p'] = error return kp * state['p'] + ki * state['i'] + kd * state['d'] return control
def as_flat_array(iterables): '''Given a sequence of sequences, return a flat numpy array. Parameters ---------- iterables : sequence of sequence of number A sequence of tuples or lists containing numbers. Typically these come from something that represents each joint in a skeleton, like angle. Returns ------- ndarray : An array of flattened data from each of the source iterables. ''' arr = [] for x in iterables: arr.extend(x) return np.array(arr)
[docs]class Skeleton: '''A skeleton is a group of rigid bodies connected with articulated joints. Commonly, the skeleton is used to represent an articulated body that is capable of mimicking the motion of the human body. Most often, skeletons are configured by parsing information from a text file of some sort. See :class:`pagoda.parser.BodyParser` for more information about the format of the text file. Skeletons can also be loaded from text files in ASF format; see :class:`pagoda.parser.AsfParser` for more information. Parameters ---------- world : :class:`pagoda.physics.World` A world object that holds bodies and joints for physics simulation. Attributes ---------- bodies : list of :class:`pagoda.physics.Body` A list of the rigid bodies that comprise this skeleton. joints : list of :class:`pagoda.physics.Joint` A list of the joints that connect bodies in this skeleton. '''
[docs] def __init__(self, world): self.world = world self.jointgroup = ode.JointGroup() self.bodies = [] self.joints = []
[docs] def load(self, source, **kwargs): '''Load a skeleton definition from a file. Parameters ---------- source : str or file A filename or file-like object that contains text information describing a skeleton. See :class:`pagoda.parser.Parser` for more information about the format of the text file. ''' if hasattr(source, 'endswith') and source.lower().endswith('.asf'): self.load_asf(source, **kwargs) else: self.load_skel(source, **kwargs)
[docs] def load_skel(self, source, **kwargs): '''Load a skeleton definition from a text file. Parameters ---------- source : str or file A filename or file-like object that contains text information describing a skeleton. See :class:`pagoda.parser.BodyParser` for more information about the format of the text file. ''' logging.info('%s: parsing skeleton configuration', source) if hasattr(source, 'read'): p = parser.parse(source, self.world, self.jointgroup, **kwargs) else: with open(source) as handle: p = parser.parse(handle, self.world, self.jointgroup, **kwargs) self.bodies = p.bodies self.joints = p.joints self.set_pid_params(kp=0.999 / self.world.dt)
[docs] def load_asf(self, source, **kwargs): '''Load a skeleton definition from an ASF text file. Parameters ---------- source : str or file A filename or file-like object that contains text information describing a skeleton, in ASF format. ''' if hasattr(source, 'read'): p = parser.parse_asf(source, self.world, self.jointgroup, **kwargs) else: with open(source) as handle: p = parser.parse_asf(handle, self.world, self.jointgroup, **kwargs) self.bodies = p.bodies self.joints = p.joints self.set_pid_params(kp=0.999 / self.world.dt)
[docs] def set_pid_params(self, *args, **kwargs): '''Set PID parameters for all joints in the skeleton. Parameters for this method are passed directly to the `pid` constructor. ''' for joint in self.joints: joint.target_angles = [None] * joint.ADOF joint.controllers = [pid(*args, **kwargs) for i in range(joint.ADOF)]
@property def color(self): return getattr(self.bodies[0], 'color', (1, 0, 0, 1)) @color.setter def color(self, color): for b in self.bodies: b.color = color @property def num_dofs(self): '''Return the number of degrees of freedom in the skeleton.''' return sum(j.ADOF for j in self.joints) @property def joint_angles(self): '''Get a list of all current joint angles in the skeleton.''' return as_flat_array(j.angles for j in self.joints) @property def joint_velocities(self): '''Get a list of all current joint velocities in the skeleton.''' return as_flat_array(j.velocities for j in self.joints) @property def joint_torques(self): '''Get a list of all current joint torques in the skeleton.''' return as_flat_array(getattr(j, 'amotor', j).feedback[-1][:j.ADOF] for j in self.joints) @property def body_positions(self): '''Get a list of all current body positions in the skeleton.''' return as_flat_array(b.position for b in self.bodies) @property def body_rotations(self): '''Get a list of all current body rotations in the skeleton.''' return as_flat_array(b.quaternion for b in self.bodies) @property def body_linear_velocities(self): '''Get a list of all current body velocities in the skeleton.''' return as_flat_array(b.linear_velocity for b in self.bodies) @property def body_angular_velocities(self): '''Get a list of all current body angular velocities in the skeleton.''' return as_flat_array(b.angular_velocity for b in self.bodies) @property def cfm(self): return self.joints[0].cfm @cfm.setter def cfm(self, cfm): for joint in self.joints: joint.cfm = cfm @property def erp(self): return self.joints[0].erp @erp.setter def erp(self, erp): for joint in self.joints: joint.erp = erp
[docs] def indices_for_joint(self, name): '''Get a list of the indices for a specific joint. Parameters ---------- name : str The name of the joint to look up. Returns ------- list of int : A list of the index values for quantities related to the named joint. Often useful for getting, say, the angles for a specific joint in the skeleton. ''' j = 0 for joint in self.joints: if joint.name == name: return list(range(j, j + joint.ADOF)) j += joint.ADOF return []
[docs] def indices_for_body(self, name, step=3): '''Get a list of the indices for a specific body. Parameters ---------- name : str The name of the body to look up. step : int, optional The number of numbers for each body. Defaults to 3, should be set to 4 for body rotation (since quaternions have 4 values). Returns ------- list of int : A list of the index values for quantities related to the named body. ''' for j, body in enumerate(self.bodies): if body.name == name: return list(range(j * step, (j + 1) * step)) return []
[docs] def joint_distances(self): '''Get the current joint separations for the skeleton. Returns ------- distances : list of float A list expressing the distance between the two joint anchor points, for each joint in the skeleton. These quantities describe how "exploded" the bodies in the skeleton are; a value of 0 indicates that the constraints are perfectly satisfied for that joint. ''' return [((np.array(j.anchor) - j.anchor2) ** 2).sum() for j in self.joints]
[docs] def get_body_states(self): '''Return a list of the states of all bodies in the skeleton.''' return [b.state for b in self.bodies]
[docs] def set_body_states(self, states): '''Set the states of all bodies in the skeleton.''' for state in states: self.world.get_body(state.name).state = state
[docs] def set_joint_velocities(self, target=0): '''Set the target velocity for all joints in the skeleton. Often the target is set to 0 to cancel out any desired joint rotation. Parameters ---------- target : float, optional The target velocity for all joints in the skeleton. Defaults to 0. ''' for joint in self.joints: joint.velocities = target
[docs] def enable_motors(self, max_force): '''Enable the joint motors in this skeleton. This method sets the maximum force that can be applied by each joint to attain the desired target velocities. It also enables torque feedback for all joint motors. Parameters ---------- max_force : float The maximum force that each joint is allowed to apply to attain its target velocity. ''' for joint in self.joints: amotor = getattr(joint, 'amotor', joint) amotor.max_forces = max_force if max_force > 0: amotor.enable_feedback() else: amotor.disable_feedback()
[docs] def disable_motors(self): '''Disable joint motors in this skeleton. This method sets to 0 the maximum force that joint motors are allowed to apply, in addition to disabling torque feedback. ''' self.enable_motors(0)
[docs] def set_target_angles(self, angles): '''Move each joint toward a target angle. This method uses a PID controller to set a target angular velocity for each degree of freedom in the skeleton, based on the difference between the current and the target angle for the respective DOF. PID parameters are by default set to achieve a tiny bit less than complete convergence in one time step, using only the P term (i.e., the P coefficient is set to 1 - \delta, while I and D coefficients are set to 0). PID parameters can be updated by calling the `set_pid_params` method. Parameters ---------- angles : list of float A list of the target angles for every joint in the skeleton. ''' j = 0 for joint in self.joints: velocities = [ ctrl(tgt - cur, self.world.dt) for cur, tgt, ctrl in zip(joint.angles, angles[j:j+joint.ADOF], joint.controllers)] joint.velocities = velocities j += joint.ADOF
[docs] def add_torques(self, torques): '''Add torques for each degree of freedom in the skeleton. Parameters ---------- torques : list of float A list of the torques to add to each degree of freedom in the skeleton. ''' j = 0 for joint in self.joints: joint.add_torques( list(torques[j:j+joint.ADOF]) + [0] * (3 - joint.ADOF)) j += joint.ADOF