def init_ik(self, active_dofs, doflim_gain=0.5): """ Initialize the IK solver. Parameters ---------- active_dofs : list Specifies DOFs used by the IK. doflim_gain : scalar Gain between 0 and 1 used for DOF limits. """ self.ik = VelocitySolver(self, active_dofs, doflim_gain)
def init_ik(self, gains=None, weights=None): """ Initialize the IK solver. INPUT: - ``gains`` -- dictionary of default task gains - ``weights`` -- dictionary of default task weights """ self.ik = VelocitySolver( self, default_gains=gains, default_weights=weights)
def init_ik(self, active_dofs, doflim_gain=0.5): """ Initialize the IK solver. INPUT: - ``active_dofs`` -- list of DOFs used by the IK - ``doflim_gain`` -- gain between 0 and 1 used for DOF limits """ from ik import VelocitySolver class IKProcess(Process): def on_tick(_, sim): self.step_ik(sim.dt) self.ik = VelocitySolver(self, active_dofs, doflim_gain) self.ik_process = IKProcess()
class Robot(object): """ Robot with a fixed base. This class wraps OpenRAVE's Robot type. """ __default_xml = """ <environment> <robot file="%s" name="%s" /> </environment> """ def __init__(self, path=None, xml=None, qd_lim=None): """ Create a new robot model. INPUT: - ``path`` -- path to the COLLADA model of the robot - ``xml`` -- (optional) string in OpenRAVE XML format - ``qd_lim`` -- maximum angular joint velocity (in [rad] / [s]) """ assert path is not None or xml is not None name = basename(splitext(path)[0]) if xml is None: xml = Robot.__default_xml % (path, name) env = get_env() env.LoadData(xml) rave = env.GetRobot(name) nb_dofs = rave.GetDOF() set_default_background_color() # [dirty] reset by LoadData q_min, q_max = rave.GetDOFLimits() rave.SetDOFVelocities([0] * nb_dofs) rave.SetDOFVelocityLimits([1000.] * nb_dofs) if qd_lim is None: qd_lim = 10. # [rad] / [s]; this is already quite fast qd_max = +qd_lim * ones(nb_dofs) qd_min = -qd_lim * ones(nb_dofs) self.active_dofs = None self.has_free_flyer = False self.ik = None # created by self.init_ik() self.ik_lock = None self.ik_thread = None self.is_visible = True self.mass = sum([link.GetMass() for link in rave.GetLinks()]) self.nb_dofs = nb_dofs self.q_max = q_max self.q_max.flags.writeable = False self.q_max_active = None self.q_min = q_min self.q_min.flags.writeable = False self.q_min_active = None self.qd_max = qd_max self.qd_max_active = None self.qd_min = qd_min self.qd_min_active = None self.qdd_max = None # set in child class self.rave = rave self.tau_max = None # set by hand in child robot class self.transparency = 0. # initially opaque """ Visualization ============= """ def hide(self): self.rave.SetVisible(False) def set_color(self, r, g, b): for link in self.rave.GetLinks(): for geom in link.GetGeometries(): geom.SetAmbientColor([r, g, b]) geom.SetDiffuseColor([r, g, b]) def set_transparency(self, transparency): self.transparency = transparency for link in self.rave.GetLinks(): for geom in link.GetGeometries(): geom.SetTransparency(transparency) def set_visible(self, visible): self.is_visible = visible self.rave.SetVisible(visible) def show(self): self.rave.SetVisible(True) """ Degrees of freedom ================== OpenRAVE calls "DOF values" what we will also call "joint angles". Same for "DOF velocities" and "joint velocities". """ @property def q(self): return self.rave.GetDOFValues() @property def qd(self): return self.rave.GetDOFVelocities() def get_dof_limits(self, dof_indices=None): """ Get the couple (q_min, q_max) of DOF limits. INPUT: - ``dof_indices`` -- (optional) only compute limits for these indices .. NOTE:: This OpenRAVE function is wrapped because it is too slow in practice. On my machine: In [1]: %timeit robot.get_dof_limits() 1000000 loops, best of 3: 205 ns per loop In [2]: %timeit robot.rave.GetDOFLimits() 100000 loops, best of 3: 2.59 µs per loop Recall that this function is called at every IK step. """ q_min, q_max = self.q_min, self.q_max if dof_indices is not None: q_max = q_max[dof_indices] q_min = q_min[dof_indices] return (q_min, q_max) def get_dof_values(self, dof_indices=None): if dof_indices is not None: return self.rave.GetDOFValues(dof_indices) return self.rave.GetDOFValues() def get_dof_velocities(self, dof_indices=None): if dof_indices is not None: return self.rave.GetDOFVelocities(dof_indices) return self.rave.GetDOFVelocities() def set_dof_limits(self, q_min, q_max, dof_indices=None): self.rave.SetDOFLimits(q_min, q_max, dof_indices) self.q_max.flags.writeable = True self.q_min.flags.writeable = True if dof_indices is not None: assert len(q_min) == len(q_max) == len(dof_indices) self.q_max[dof_indices] = q_max self.q_min[dof_indices] = q_min else: assert len(q_min) == len(q_max) == self.nb_dofs self.q_max = q_max self.q_min = q_min if self.active_dofs: self.q_max_active = self.q_max[self.active_dofs] self.q_min_active = self.q_min[self.active_dofs] self.q_max.flags.writeable = False self.q_min.flags.writeable = False def set_dof_values(self, q, dof_indices=None): if dof_indices is not None: return self.rave.SetDOFValues(q, dof_indices) return self.rave.SetDOFValues(q) def set_dof_velocities(self, qd, dof_indices=None): check_dof_limits = 0 # CLA_Nothing if dof_indices is not None: return self.rave.SetDOFVelocities(qd, check_dof_limits, dof_indices) return self.rave.SetDOFVelocities(qd) """ Active DOFs =========== We simply wrap around OpenRAVE here. Active DOFs are used with the IK. """ @property def nb_active_dofs(self): return self.rave.GetActiveDOF() @property def q_active(self): return self.rave.GetActiveDOFValues() def get_active_dof_values(self): return self.rave.GetActiveDOFValues() def get_active_dof_velocities(self): return self.rave.GetActiveDOFVelocities() def set_active_dofs(self, active_dofs): self.active_dofs = active_dofs self.rave.SetActiveDOFs(active_dofs) self.q_max_active = self.q_max[active_dofs] self.q_min_active = self.q_min[active_dofs] self.qd_max_active = self.qd_max[active_dofs] self.qd_min_active = self.qd_min[active_dofs] def set_active_dof_values(self, q_active): return self.rave.SetActiveDOFValues(q_active) def set_active_dof_velocities(self, qd_active): check_dof_limits = 0 # CLA_Nothing return self.rave.SetActiveDOFVelocities(qd_active, check_dof_limits) """ Jacobians and Hessians ====================== """ def compute_link_jacobian(self, link, p=None): """ Compute the jacobian J(q) of the reference frame of the link, i.e. the velocity of the link frame is given by: [v omega] = J(q) * qd where v and omega are the linear and angular velocities of the link frame, respectively. INPUT: - ``link`` -- link index or pymanoid.Link object - ``p`` -- link frame origin (optional: if None, link.p is used) - ``q`` -- vector of joint angles (optional: if None, robot.q is used) """ link_index = link if type(link) is int else link.index p = p if type(link) is int else link.p J_lin = self.rave.ComputeJacobianTranslation(link_index, p) J_ang = self.rave.ComputeJacobianAxisAngle(link_index) J = vstack([J_lin, J_ang]) return J def compute_link_pose_jacobian(self, link): J_trans = self.rave.CalculateJacobian(link.index, link.p) or_quat = link.rave.GetTransformPose()[:4] # don't use link.pose J_quat = self.rave.CalculateRotationJacobian(link.index, or_quat) if or_quat[0] < 0: # we enforce positive first coefficients J_quat *= -1. J = vstack([J_quat, J_trans]) return J def compute_link_pos_jacobian(self, link, p=None): """ Compute the position Jacobian of a point p on a given robot link. INPUT: - ``link`` -- link index or pymanoid.Link object - ``p`` -- point coordinates in world frame (optional, default is the origin of the link reference frame) """ link_index = link if type(link) is int else link.index p = link.p if p is None else p J = self.rave.ComputeJacobianTranslation(link_index, p) return J def compute_link_hessian(self, link, p=None): """ Compute the hessian H(q) of the reference frame of the link, i.e. the acceleration of the link frame is given by: [a omegad] = J(q) * qdd + qd.T * H(q) * qd where a and omegad are the linear and angular accelerations of the frame, and J(q) is the frame jacobian. INPUT: - ``link`` -- link index or pymanoid.Link object - ``p`` -- link frame origin (optional: if None, link.p is used) """ link_index = link if type(link) is int else link.index p = p if type(link) is int else link.p H_lin = self.rave.ComputeHessianTranslation(link_index, p) H_ang = self.rave.ComputeHessianAxisAngle(link_index) H = concatenate([H_lin, H_ang], axis=1) return H def compute_link_pos_hessian(self, link, p=None): """ Compute the hessian H(q) of a point p on ``link``. INPUT: - ``link`` -- link index or pymanoid.Link object - ``p`` -- point coordinates in world frame (optional, default is the origin of the link reference frame) """ link_index = link if type(link) is int else link.index p = p if type(link) is int else link.p H = self.rave.ComputeHessianTranslation(link_index, p) return H """ Inverse Kinematics ================== """ def init_ik(self, gains=None, weights=None): """ Initialize the IK solver. INPUT: - ``gains`` -- dictionary of default task gains - ``weights`` -- dictionary of default task weights """ self.ik = VelocitySolver( self, default_gains=gains, default_weights=weights) def step_ik(self, dt): qd_active = self.ik.compute_velocity(dt) q_active = minimum( maximum( self.q_min_active, self.q_active + qd_active * dt), self.q_max_active) self.set_active_dof_values(q_active) self.set_active_dof_velocities(qd_active) def solve_ik(self, max_it=1000, conv_tol=1e-5, dt=1e-2, debug=False): """ Compute joint-angles q satisfying all kinematic constraints at best. INPUT: - ``max_it`` -- maximum number of solver iterations - ``conv_tol`` -- stop when cost improvement is less than this threshold - ``dt`` -- time step for the differential IK - ``debug`` -- print extra debug info .. NOTE:: Good values of dt depend on the weights of the IK tasks. Small values make convergence slower, while big values may jeopardize it. """ if debug: print "solve_ik(max_it=%d, conv_tol=%e)" % (max_it, conv_tol) cost = 100000. for itnum in xrange(max_it): prev_cost = cost cost = self.ik.compute_cost(dt) cost_relvar = abs(cost - prev_cost) / prev_cost if debug: print "%2d: %.3f (%+.2e)" % (itnum, cost, cost_relvar) if cost_relvar < conv_tol: if abs(cost) > 0.1: warn("IK did not converge to solution. " "Is the problem feasible? " "If so, try restarting from a random guess.") break self.step_ik(dt) return itnum, cost def start_ik_thread(self, dt, sleep_fun=None): """ Start a new thread stepping the IK every dt, then calling sleep_fun(dt). INPUT: - ``dt`` -- stepping period in seconds - ``sleep_fun`` -- sleeping function (default: time.sleep) """ if sleep_fun is None: sleep_fun = rt_sleep self.ik_lock = Lock() self.ik_thread = Thread(target=self.run_ik_thread, args=(dt, sleep_fun)) self.ik_thread.daemon = True self.ik_thread.start() def run_ik_thread(self, dt, sleep_fun): while self.ik_lock: with self.ik_lock: self.step_ik(dt) sleep_fun(dt) def pause_ik_thread(self): self.ik_lock.acquire() def resume_ik_thread(self): self.ik_lock.release() def stop_ik_thread(self): self.ik_lock = None """ Inverse Dynamics ================ """ def compute_inertia_matrix(self, external_torque=None): """ Compute the inertia matrix of the robot. INPUT: - ``external_torque`` -- vector of external torques (optional) .. NOTE:: The inertia matrix is the matrix M(q) such that the equations of motion are: M(q) * qdd + qd.T * C(q) * qd + g(q) = F + external_torque with: q -- vector of joint angles (DOF values) qd -- vector of joint velocities qdd -- vector of joint accelerations C(q) -- Coriolis tensor (derivative of M(q) w.r.t. q) g(q) -- gravity vector F -- generalized forces (joint torques, contact wrenches, ...) external_torque -- additional torque vector (optional) This function applies the unit-vector method described by Walker & Orin <https://dx.doi.org/10.1115/1.3139699>. It is inefficient, so if you are looking for performance, you should consider more recent libraries such as <https://github.com/stack-of-tasks/pinocchio>. """ M = zeros((self.nb_dofs, self.nb_dofs)) for (i, e_i) in enumerate(eye(self.nb_dofs)): tm, _, _ = self.rave.ComputeInverseDynamics( e_i, external_torque, returncomponents=True) M[:, i] = tm return M def compute_inverse_dynamics(self, qdd=None, external_torque=None): """ Wrapper around OpenRAVE's ComputeInverseDynamics function, which implements the Recursive Newton-Euler algorithm by Walker & Orin <https://dx.doi.org/10.1115/1.3139699>. The function returns three terms tm, tc and tg such that tm = M(q) * qdd tc = qd.T * C(q) * qd tg = g(q) where the equations of motion are written: tm + tc + tg = F + external_torque INPUT: ``qdd`` -- vector of joint accelerations (optional; if not present, the return value for tm will be None) ``external_torque`` -- vector of external joint torques (optional) """ if qdd is None: _, tc, tg = self.rave.ComputeInverseDynamics( zeros(self.nb_dofs), external_torque, returncomponents=True) return None, tc, tg tm, tc, tg = self.rave.ComputeInverseDynamics( qdd, external_torque, returncomponents=True) return tm, tc, tg
class Robot(object): """ Robot with a fixed base. This class wraps OpenRAVE's Robot type. Parameters ---------- path : string Path to the COLLADA model of the robot. xml : string, optional Environment description in `OpenRAVE XML format <http://openrave.programmingvision.com/wiki/index.php/Format:XML>`_. """ __default_xml = """ <environment> <robot file="%s" name="%s" /> </environment> """ def __init__(self, path=None, xml=None): assert path is not None or xml is not None name = basename(splitext(path)[0]) if xml is None: xml = Robot.__default_xml % (path, name) env = get_openrave_env() env.LoadData(xml) rave = env.GetRobot(name) nb_dofs = rave.GetDOF() q_min, q_max = rave.GetDOFLimits() rave.SetDOFVelocityLimits([1000.] * nb_dofs) self.has_free_flyer = False self.ik = None # call init_ik() to instantiate self.ik_thread = None self.mass = sum([link.GetMass() for link in rave.GetLinks()]) self.nb_dofs = nb_dofs self.q_max = q_max self.q_max.flags.writeable = False self.q_min = q_min self.q_min.flags.writeable = False self.rave = rave self.tau_max = None # set by hand in child robot class self.transparency = 0. # initially opaque """ Visualization ============= """ def hide(self): """Make the robot invisible.""" self.rave.SetVisible(False) def set_color(self, r, g, b): """ Update the RGB properties of each link of the robot. Parameters ---------- r : scalar Red value between 0 and 1. g : scalar Green value between 0 and 1. b : scalar Blue value between 0 and 1. """ for link in self.rave.GetLinks(): for geom in link.GetGeometries(): geom.SetAmbientColor([r, g, b]) geom.SetDiffuseColor([r, g, b]) def set_transparency(self, transparency): """ Update robot transparency. Parameters ---------- transparency : scalar Transparency value from 0 (opaque) to 1 (invisible). """ self.transparency = transparency for link in self.rave.GetLinks(): for geom in link.GetGeometries(): geom.SetTransparency(transparency) def show(self): """Make the robot visible.""" self.rave.SetVisible(True) """ Degrees of freedom ================== OpenRAVE calls "DOF values" what we will also call "joint angles". Same for "DOF velocities" and "joint velocities". """ @property def q(self): """Vector of DOF values.""" return self.rave.GetDOFValues() @property def qd(self): """Vector of DOF velocities.""" return self.rave.GetDOFVelocities() def get_dof_limits(self, dof_indices=None): """ Get the couple :math:`(q_\\mathrm{min}, q_\\mathrm{max})` of DOF limits. Parameters ---------- dof_indices : list of DOF indexes, optional Only compute limits for these indices. Returns ------- q_min : array Vector of minimal DOF values. q_max : array Vector of maximal DOF values. Notes ----- This OpenRAVE function is wrapped because it is too slow in practice. On my machine: In [1]: %timeit robot.get_dof_limits() 1000000 loops, best of 3: 237 ns per loop In [2]: %timeit robot.rave.GetDOFLimits() 100000 loops, best of 3: 9.24 µs per loop """ q_min, q_max = self.q_min, self.q_max if dof_indices is not None: q_max = q_max[dof_indices] q_min = q_min[dof_indices] return (q_min, q_max) def get_dof_values(self, dof_indices=None): if dof_indices is not None: return self.rave.GetDOFValues(dof_indices) return self.rave.GetDOFValues() def get_dof_velocities(self, dof_indices=None): if dof_indices is not None: return self.rave.GetDOFVelocities(dof_indices) return self.rave.GetDOFVelocities() def set_dof_limits(self, q_min, q_max, dof_indices=None): if self.ik is not None: warn("DOF limit updates will not be forwarded to ongoing IK") self.rave.SetDOFLimits(q_min, q_max, dof_indices) self.q_max.flags.writeable = True self.q_min.flags.writeable = True if dof_indices is not None: assert len(q_min) == len(q_max) == len(dof_indices) self.q_max[dof_indices] = q_max self.q_min[dof_indices] = q_min else: assert len(q_min) == len(q_max) == self.nb_dofs self.q_max = q_max self.q_min = q_min self.q_max.flags.writeable = False self.q_min.flags.writeable = False def set_dof_values(self, q, dof_indices=None, clamp=False): """ Set the joint values of the robot. Parameters ---------- q : list or array Joint angle values, ordered by DOF indices. dof_indices : list, optional List of DOF indices to update. clamp : bool Correct joint angles when exceeding joint limits. """ if clamp: q = minimum(maximum(self.q_min, q), self.q_max) if dof_indices is not None: return self.rave.SetDOFValues(q, dof_indices) return self.rave.SetDOFValues(q) def set_dof_velocities(self, qd, dof_indices=None): """ Set the joint velocities of the robot. Parameters ---------- qd : list or array Joint angle velocities, ordered by DOF indices. dof_indices : list, optional List of DOF indices to update. """ check_dof_limits = 0 # CLA_Nothing if dof_indices is not None: return self.rave.SetDOFVelocities(qd, check_dof_limits, dof_indices) return self.rave.SetDOFVelocities(qd) """ Jacobians and Hessians ====================== """ def compute_link_jacobian(self, link, p=None): """ Compute the Jacobian `J(q)` of a frame attached to a given link, the velocity of this frame being given by: .. math:: \\left[\\begin{array}{c} v_p \\\\ \\varomega} \\end{array}\\right] = J(q) \\dot{q} where :math:`v_p` is the linear velocity of the link at point `p` (default is the origin of the link frame) and :math:`\\varomega` is the angular velocity of the link. Parameters ---------- link : integer or pymanoid.Link Link identifier: either a link index, or the Link object directly. p : array Point coordinates in the world frame. """ link_index = link if type(link) is int else link.index p = p if type(link) is int else link.p J_lin = self.rave.ComputeJacobianTranslation(link_index, p) J_ang = self.rave.ComputeJacobianAxisAngle(link_index) J = vstack([J_lin, J_ang]) return J def compute_link_pose_jacobian(self, link): """ Compute the pose Jacobian of a given link, i.e. the matrix `J(q)` such that: .. math:: \\left[\\begin{array}{c} \\dot{\\xi} \\\\ v_L} \\end{array}\\right] = J(q) \\dot{q}, with :math:`\\xi` a quaternion for the link orientation and :math:`v_L = \\dot{p}_L` the velocity of the origin `L` of the link frame, so that the link pose is :math:`[\\xi p_L]` and the left-hand side of the equation above is its time-derivative. Parameters ---------- link : integer or pymanoid.Link Link identifier: either a link index, or the Link object directly. """ J_trans = self.rave.CalculateJacobian(link.index, link.p) or_quat = link.rave.GetTransformPose()[:4] # don't use link.pose J_quat = self.rave.CalculateRotationJacobian(link.index, or_quat) if or_quat[0] < 0: # we enforce positive first coefficients J_quat *= -1. J = vstack([J_quat, J_trans]) return J def compute_link_pos_jacobian(self, link, p=None): """ Compute the position Jacobian of a point `p` on a given robot link. Parameters ---------- link : integer or pymanoid.Link Link identifier: either a link index, or the Link object directly. p : array Point coordinates in the world frame. """ link_index = link if type(link) is int else link.index p = link.p if p is None else p J = self.rave.ComputeJacobianTranslation(link_index, p) return J def compute_link_hessian(self, link, p=None): """ Compute the Hessian `H(q)` of a frame attached to a robot link, the acceleration of which is given by: .. math:: \\left[\\begin{array}{c} a_p \\\\ \\dot{\\varomega} \\end{array}\\right] = J(q) \\ddot{q} + \\dot{q}^T H(q) \\dot{q} where :math:`a_p` is the linear acceleration of the point `p` (default is the origin of the link frame) and :math:`\\dot{\\varomega}` is the angular accelerations of the frame. Parameters ---------- link : integer or pymanoid.Link Link identifier: either a link index, or the Link object directly. p : array Point coordinates in the world frame. """ link_index = link if type(link) is int else link.index p = p if type(link) is int else link.p H_lin = self.rave.ComputeHessianTranslation(link_index, p) H_ang = self.rave.ComputeHessianAxisAngle(link_index) H = concatenate([H_lin, H_ang], axis=1) return H def compute_link_pos_hessian(self, link, p=None): """ Compute the translation Hessian H(q) of a point `p` on ``link``, i.e. the matrix such that the acceleration of `p` is given by: .. math:: a_p = J(q) \\ddot{q} + \\dot{q}^T H(q) \\dot{q}. Parameters ---------- link : integer or pymanoid.Link Link identifier: either a link index, or the Link object directly. p : array Point coordinates in the world frame. """ link_index = link if type(link) is int else link.index p = p if type(link) is int else link.p H = self.rave.ComputeHessianTranslation(link_index, p) return H """ Inverse Kinematics ================== """ def init_ik(self, active_dofs, doflim_gain=0.5): """ Initialize the IK solver. Parameters ---------- active_dofs : list Specifies DOFs used by the IK. doflim_gain : scalar Gain between 0 and 1 used for DOF limits. """ self.ik = VelocitySolver(self, active_dofs, doflim_gain) def step_ik(self, dt, method='safe', verbose=False): """ Apply velocities computed by inverse kinematics. Parameters ---------- dt : scalar Time step in [s]. method : string, optional Choice between 'fast' and 'safe' (default). """ qd = self.ik.compute_velocity(dt, method) if verbose: print "\n TASK COST", print "\n------------------------------" for task in self.ik.tasks.itervalues(): J = task.jacobian() r = task.residual(dt) print "%20s %.2e" % (task.name, norm(dot(J, qd) - r)) print "" self.set_dof_values(self.q + qd * dt, clamp=True) self.set_dof_velocities(qd) def solve_ik(self, max_it=1000, conv_tol=1e-5, dt=5e-3, debug=True, method='fast'): """ Compute joint-angles q satisfying all kinematic constraints at best. Parameters ---------- max_it : integer, optional Maximum number of solver iterations. conv_tol : scalar, optional Stop when cost improvement is less than this threshold. dt : scalar, optional Time step in [s]. debug : bool, optional Print extra debug info. method : string, optional Either 'fast', or 'safe' for more joint-limit avoidance Returns ------- (itnum, cost) : (integer, scalar) Number of iterations taken, followed by final IK cost. Note ---- Good values of dt depend on the weights of the IK tasks. Small values make convergence slower, while big values make the optimization unstable (in which case there may be no convergence at all). """ if debug: print "solve_ik(max_it=%d, conv_tol=%e)" % (max_it, conv_tol) cost = 100000. self.ik.qd_max *= 1000 self.ik.qd_min *= 1000 for itnum in xrange(max_it): prev_cost = cost cost = self.ik.compute_cost(dt) cost_relvar = abs(cost - prev_cost) / prev_cost if debug: print "%2d: %.3f (%+.2e)" % (itnum, cost, cost_relvar) if cost_relvar < conv_tol: break self.step_ik(dt, method) self.set_dof_velocities(zeros(self.qd.shape)) self.ik.qd_max /= 1000 self.ik.qd_min /= 1000 return itnum, cost """ Inverse Dynamics ================ """ def compute_inertia_matrix(self, external_torque=None): """ Compute the inertia matrix of the robot. Parameters ---------- external_torque : array, optional Vector of external torques. Notes ----- The inertia matrix is the matrix :math:`M(q)` such that the equations of motion are written as: .. math:: M(q) \\ddot{q} + \\dot{q}^T C(q) \\dot{q} + g(q) = F + \\tau_\\mathrm{ext} with: - :math:`q` -- vector of joint angles (DOF values) - :math:`\\dot{q}` -- vector of joint velocities - :math:`\\ddot{q}` -- vector of joint accelerations - :math:`C(q)` -- Coriolis tensor (derivative of M(q) w.r.t. q) - :math:`g(q)` -- gravity vector - :math:`F` -- generalized forces (joint torques, contact wrenches, ...) - :math:`\\tau_\\mathrm{ext}` -- additional torque vector This function applies the unit-vector method described by Walker & Orin in [WO82]_. It is not efficient, so if you are looking for performance, you should consider more recent libraries such as `pinocchio <https://github.com/stack-of-tasks/pinocchio>`_. References ---------- .. [WO82] M. Walker and D. Orin. “Efficient dynamic computer simulation of robotic mechanisms.” ASME Trans. J. dynamics Systems, Measurement and Control 104 (1982): 205-211. """ M = zeros((self.nb_dofs, self.nb_dofs)) for (i, e_i) in enumerate(eye(self.nb_dofs)): tm, _, _ = self.rave.ComputeInverseDynamics( e_i, external_torque, returncomponents=True) M[:, i] = tm return M def compute_inverse_dynamics(self, qdd=None, external_torque=None): """ Wrapper around OpenRAVE's ComputeInverseDynamics function, which implements the Recursive Newton-Euler algorithm by Walker & Orin <http://doai.io/10.1115/1.3139699>. The function returns three terms tm, tc and tg such that tm = M(q) * qdd tc = qd.T * C(q) * qd tg = g(q) where the equations of motion are written: tm + tc + tg = F + external_torque Parameters ---------- qdd : array, optional Vector of joint accelerations. external_torque : array, optional Vector of external joint torques. Note ---- When ``qdd`` is not provided, the value returned for ``tm`` is ``None``. """ if qdd is None: _, tc, tg = self.rave.ComputeInverseDynamics( zeros(self.nb_dofs), external_torque, returncomponents=True) return None, tc, tg tm, tc, tg = self.rave.ComputeInverseDynamics( qdd, external_torque, returncomponents=True) return tm, tc, tg def compute_static_torques(self, external_torque=None): """ Compute static-equilibrium torques for the manipulator. Parameters ---------- external_torque : array Vector of external joint torques. Returns ------- tg : array Vector of static joint torques compensating gravity, and ``external_torque`` if applicable. """ qd = self.qd qz = zeros(self.nb_dofs) self.set_dof_velocities(qz) tg = self.rave.ComputeInverseDynamics(qz, external_torque) self.set_dof_velocities(qd) return tg