Blender  V3.3
ControlledObject.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Ruben Smits. */
3 
8 #include "ControlledObject.hpp"
9 
10 
11 namespace iTaSC {
13  Object(Controlled),m_nq(0),m_nc(0),m_nee(0)
14 {
15  // max joint variable = 0.52 radian or 0.52 meter in one timestep
16  m_maxDeltaQ = e_scalar(0.52);
17 }
18 
19 void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee)
20 {
21  assert(_nee >= 1);
22  m_nq = _nq;
23  m_nc = _nc;
24  m_nee = _nee;
25  if (m_nq > 0) {
28  }
29  if (m_nc > 0) {
30  m_Wy = e_scalar_vector(m_nc,1.0);
32  }
33  if (m_nc > 0 && m_nq > 0)
35  // clear all Jacobian if any
36  m_JqArray.clear();
37  // reserve one more to have a zero matrix handy
38  if (m_nq > 0)
39  m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq));
40 }
41 
43 
44 
45 
46 const e_matrix& ControlledObject::getJq(unsigned int ee) const
47 {
48  assert(m_nq > 0);
49  return m_JqArray[(ee>m_nee)?m_nee:ee];
50 }
51 
52 double ControlledObject::getMaxTimestep(double& timestep)
53 {
54  e_scalar maxQdot = m_qdot.array().abs().maxCoeff();
55  if (timestep*maxQdot > m_maxDeltaQ) {
56  timestep = m_maxDeltaQ/maxQdot;
57  }
58  return timestep;
59 }
60 
61 }
std::vector< e_matrix > m_JqArray
virtual const e_matrix & getJq(unsigned int ee) const
virtual double getMaxTimestep(double &timestep)
virtual void initialize(unsigned int _nq, unsigned int _nc, unsigned int _nee)
#define e_scalar_vector
Definition: eigen_types.hpp:43
#define e_scalar
Definition: eigen_types.hpp:37
#define e_zero_vector
Definition: eigen_types.hpp:39
#define e_identity_matrix
Definition: eigen_types.hpp:42
#define e_zero_matrix
Definition: eigen_types.hpp:44
#define e_matrix
Definition: eigen_types.hpp:40