diff options
Diffstat (limited to 'intern/itasc/ControlledObject.cpp')
-rw-r--r-- | intern/itasc/ControlledObject.cpp | 61 |
1 files changed, 61 insertions, 0 deletions
diff --git a/intern/itasc/ControlledObject.cpp b/intern/itasc/ControlledObject.cpp new file mode 100644 index 00000000000..f9e819d2950 --- /dev/null +++ b/intern/itasc/ControlledObject.cpp @@ -0,0 +1,61 @@ +/* $Id: ControlledObject.cpp 19905 2009-04-23 13:29:54Z ben2610 $ + * ControlledObject.cpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#include "ControlledObject.hpp" + + +namespace iTaSC { +ControlledObject::ControlledObject(): + Object(Controlled),m_nq(0),m_nc(0),m_nee(0) +{ + // max joint variable = 0.52 radian or 0.52 meter in one timestep + m_maxDeltaQ = e_scalar(0.52); +} + +void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee) +{ + assert(_nee >= 1); + m_nq = _nq; + m_nc = _nc; + m_nee = _nee; + if (m_nq > 0) { + m_Wq = e_identity_matrix(m_nq,m_nq); + m_qdot = e_zero_vector(m_nq); + } + if (m_nc > 0) { + m_Wy = e_scalar_vector(m_nc,1.0); + m_ydot = e_zero_vector(m_nc); + } + if (m_nc > 0 && m_nq > 0) + m_Cq = e_zero_matrix(m_nc,m_nq); + // clear all Jacobian if any + m_JqArray.clear(); + // reserve one more to have a zero matrix handy + if (m_nq > 0) + m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq)); +} + +ControlledObject::~ControlledObject() {} + + + +const e_matrix& ControlledObject::getJq(unsigned int ee) const +{ + assert(m_nq > 0); + return m_JqArray[(ee>m_nee)?m_nee:ee]; +} + +double ControlledObject::getMaxTimestep(double& timestep) +{ + e_scalar maxQdot = m_qdot.cwise().abs().maxCoeff(); + if (timestep*maxQdot > m_maxDeltaQ) { + timestep = m_maxDeltaQ/maxQdot; + } + return timestep; +} + +} |