/***************************************************************************** * DynaMechs: A Multibody Dynamic Simulation Library * * Copyright (C) 1994-2001 Scott McMillan All Rights Reserved. * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * General Public License for more details. * * You should have received a copy of the GNU Library General Public * License along with this library; if not, write to the Free * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. ***************************************************************************** * File: dmSystem.hpp * Author: Scott McMillan * Summary: Class definitions for dmSystem - top level DynaMechs structure *****************************************************************************/ #ifndef _DM_SYSTEM_HPP #define _DM_SYSTEM_HPP #include #include //====================================================================== /** The {\tt dmSystem} is the abstract base class for all types of objects that need to be simulated by a {\tt dmIntegrator} class. As such this class defines the interface needed by the dmIntegrator classes, and is the base class for robotic systems (dmArticulation and dmClosedArticulation), dynamic environments (dmTreadmill through dmEnvironment), and (in the future) control systems. By adding all the dynamic systems to a single integrator, it will be ensured that all the systems will be numerically integrated in synchrony with one another. This is especially important when multi-step and/or variable stepsize numerical integrators are used, where the system dynamics are calculated at more then one point across the interval, where some computations may be rejected due to accuracy constraints, and the interval may be subdivided into smaller steps. The {\tt setState} and {\tt getState} functions are used to set and query joint states in the entire tree. {\tt setState} sets the state of the DOFs in the articulation. The two arguments are both packed arrays (like {\tt joint\_input}) containing the joint positions and velocities of the links. The reverse operation, {\tt getState}, fills packed arrays with the joint positions velocities. A convenience function, {\tt getNumDOFs}, returns the total number of degrees of freedom in the articulation and can be used to determine the appropriate size of the above arrays. The {\tt initSimVars} function initializes the state and derivative of state vectors with the current state of the system. The parameters, {\tt qy} and {\tt qdy}, are the arrays with 2*{\tt getNumDOFs()} elements. All necessary calls to {\tt initSimVars} are done automatically by the {\tt dmIntegrator} class when a {\tt dmSystem} object is added to or removed from it. Note that {\tt initSimVars} is different from the {\tt getState} function that returns just the position and velocity state vectors in two separate vectors ({\tt q} and {\tt qd}) of {\tt getNumDOFs()} each. {\bf IMPORTANT}: The {\tt dmIntegrator} class cannot detect subsequent changes that would affect the number of DOFs (e.g., addition/removal of links); therefore, the entire system must be built before it is assigned to the {\tt dmIntegrator} object and no changes can be made after this point. Specification of an inertial coordinate system other than the origin can be specified by calling {\tt setRefSystem} and passing the position and orientation (relative to the origin) of the new coordinate system. This ability is not necessary or even computationally efficient, but it makes it much more flexible in specifying systems. The functions {\tt getRefSystem} and {\tt getPose} return this position and orientation -- the latter in two different formats: quaternion or rotation matrix. The functions, {\tt rtxFromICS} and {\tt rtxToICS}, are used to rotate free vectors from one system to the other. The {\tt dynamics} function is the entry point for computation of the dynamics of the system. It takes, as input, the state vector {\tt qy} and computes the derivative of state vector, {\tt qdy}. This is the derivative function that the {\tt dmIntegrator} class uses to perform the numerical integration. Two functions are provided for use by adaptive-stepsize integrators which may need to back out of a rejected integration step. This is required because some {\tt dmForce} objects attached to links may retain state information. The {\tt pushForceStates} function is used to save the state information of all force objects attached to links in the system, while the {\tt popForceStates} function is used if necessary to restore the state to the stored values. Another two functions are used for retrieving the energy of the system. The {\tt getPotentialEnergy} function returns the potential energy. Note that the potential energy zero is defined by the plane which passes through the origin and has the gravity vector as its normal. The {\tt getKineticEnergy} function returns the sum of translational and rotational kinetic energy of the system. See also: {\tt dmArticulation, dmEnvironment, dmIntegrator}. */ //====================================================================== class DM_DLL_API dmSystem : public dmObject { public: /// dmSystem(); /// virtual ~dmSystem(); /// virtual unsigned int getNumDOFs() const = 0; /// virtual void setState(Float q[], Float qd[]) = 0; /// qy and qdy are arrays with getNumDOFs() elements virtual void getState(Float q[], Float qd[]) const = 0; /// qy and qdy are arrays with 2*getNumDOFs() elements void initSimVars(Float *qy, Float *qdy); // transformation functions /// void setRefSystem(Quaternion quat, CartesianVector pos); /// void getRefSystem(Quaternion quat, CartesianVector pos) const; /// void getPose(RotationMatrix R, CartesianVector pos) const; /// inline void rtxFromICS(const CartesianVector p_ICS, CartesianVector p_ref) const; /// inline void rtxToICS(const CartesianVector p_ref, CartesianVector p_ICS) const; /// virtual void pushForceStates() = 0; /// virtual void popForceStates() = 0; /// virtual Float getPotentialEnergy() const = 0; /// virtual Float getKineticEnergy() const = 0; // dynamics algorithm /// virtual void dynamics(Float *qy, Float *qdy) = 0; virtual void draw() const = 0; protected: // not implemented dmSystem(const dmSystem &); dmSystem &operator=(const dmSystem &); // forward kinematics helper routine void forwardKinematics(dmABForKinStruct &fk) const; protected: // description of transform to reference system Quaternion m_quat_ICS; RotationMatrix m_R_ICS; // pose of links wrt ICS - ^{ICS}R_{i} CartesianVector m_p_ICS; // pos. of links wrt ICS - ^{ICS}p_{i} }; //---------------------------------------------------------------------------- // Summary: 3D rotational transformation from ICS to ref mem CS // Parameters: p_ICS - 3-vector expressed in the ICS // Returns: p_ref - 3-vector expressed in ref mem's CS //---------------------------------------------------------------------------- inline void dmSystem::rtxFromICS(const CartesianVector p_ICS, CartesianVector p_ref) const { p_ref[0] = m_R_ICS[0][0]*p_ICS[0] + m_R_ICS[0][1]*p_ICS[1] + m_R_ICS[0][2]*p_ICS[2]; p_ref[1] = m_R_ICS[1][0]*p_ICS[0] + m_R_ICS[1][1]*p_ICS[1] + m_R_ICS[1][2]*p_ICS[2]; p_ref[2] = m_R_ICS[2][0]*p_ICS[0] + m_R_ICS[2][1]*p_ICS[1] + m_R_ICS[2][2]*p_ICS[2]; } //---------------------------------------------------------------------------- // Summary: 3D rotational transformation from ref mem CS to ICS // Parameters: p_ref - 3-vector expressed in ref mem's CS // Returns: p_ICS - 3-vector expressed in the ICS //---------------------------------------------------------------------------- inline void dmSystem::rtxToICS(const CartesianVector p_ref, CartesianVector p_ICS) const { p_ICS[0] = m_R_ICS[0][0]*p_ref[0] + m_R_ICS[1][0]*p_ref[1] + m_R_ICS[2][0]*p_ref[2]; p_ICS[1] = m_R_ICS[0][1]*p_ref[0] + m_R_ICS[1][1]*p_ref[1] + m_R_ICS[2][1]*p_ref[2]; p_ICS[2] = m_R_ICS[0][2]*p_ref[0] + m_R_ICS[1][2]*p_ref[1] + m_R_ICS[2][2]*p_ref[2]; } #endif