diff options
author | Lukas Steiblys <imbusy@imbusy.org> | 2009-10-02 02:29:15 +0400 |
---|---|---|
committer | Lukas Steiblys <imbusy@imbusy.org> | 2009-10-02 02:29:15 +0400 |
commit | 0677398a649b6b8c293df3ce3c6668f0a3be3bc8 (patch) | |
tree | 9d510a5bd23559bf4fae670ed04d7e5d6c12578c /intern/itasc/ControlledObject.cpp | |
parent | 59248e9f62006ba05e3098e4d213f3dcb23fe711 (diff) | |
parent | bc942eceacb638735dc4f4f68252c4c207147a70 (diff) |
merge from 23153 to 23595soc-2009-imbusy
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..b987e176031 --- /dev/null +++ b/intern/itasc/ControlledObject.cpp @@ -0,0 +1,61 @@ +/* $Id$ + * 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; +} + +} |