Blender
V3.3
intern
itasc
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
{
12
ControlledObject::ControlledObject
():
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) {
26
m_Wq
=
e_identity_matrix
(
m_nq
,
m_nq
);
27
m_qdot
=
e_zero_vector
(
m_nq
);
28
}
29
if
(
m_nc
> 0) {
30
m_Wy
=
e_scalar_vector
(
m_nc
,1.0);
31
m_ydot
=
e_zero_vector
(
m_nc
);
32
}
33
if
(
m_nc
> 0 &&
m_nq
> 0)
34
m_Cq
=
e_zero_matrix
(
m_nc
,
m_nq
);
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
42
ControlledObject::~ControlledObject
() {}
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
}
ControlledObject.hpp
iTaSC::ControlledObject::m_JqArray
std::vector< e_matrix > m_JqArray
Definition:
ControlledObject.hpp:28
iTaSC::ControlledObject::m_Wq
e_matrix m_Wq
Definition:
ControlledObject.hpp:26
iTaSC::ControlledObject::m_ydot
e_vector m_ydot
Definition:
ControlledObject.hpp:27
iTaSC::ControlledObject::m_qdot
e_vector m_qdot
Definition:
ControlledObject.hpp:27
iTaSC::ControlledObject::getJq
virtual const e_matrix & getJq(unsigned int ee) const
Definition:
ControlledObject.cpp:46
iTaSC::ControlledObject::ControlledObject
ControlledObject()
Definition:
ControlledObject.cpp:12
iTaSC::ControlledObject::m_Wy
e_vector m_Wy
Definition:
ControlledObject.hpp:27
iTaSC::ControlledObject::~ControlledObject
virtual ~ControlledObject()
Definition:
ControlledObject.cpp:42
iTaSC::ControlledObject::getMaxTimestep
virtual double getMaxTimestep(double ×tep)
Definition:
ControlledObject.cpp:52
iTaSC::ControlledObject::m_nq
unsigned int m_nq
Definition:
ControlledObject.hpp:25
iTaSC::ControlledObject::m_nee
unsigned int m_nee
Definition:
ControlledObject.hpp:25
iTaSC::ControlledObject::initialize
virtual void initialize(unsigned int _nq, unsigned int _nc, unsigned int _nee)
Definition:
ControlledObject.cpp:19
iTaSC::ControlledObject::m_maxDeltaQ
e_scalar m_maxDeltaQ
Definition:
ControlledObject.hpp:24
iTaSC::ControlledObject::m_nc
unsigned int m_nc
Definition:
ControlledObject.hpp:25
iTaSC::ControlledObject::m_Cq
e_matrix m_Cq
Definition:
ControlledObject.hpp:26
iTaSC::Object
Definition:
Object.hpp:19
e_scalar_vector
#define e_scalar_vector
Definition:
eigen_types.hpp:43
e_scalar
#define e_scalar
Definition:
eigen_types.hpp:37
e_zero_vector
#define e_zero_vector
Definition:
eigen_types.hpp:39
e_identity_matrix
#define e_identity_matrix
Definition:
eigen_types.hpp:42
e_zero_matrix
#define e_zero_matrix
Definition:
eigen_types.hpp:44
e_matrix
#define e_matrix
Definition:
eigen_types.hpp:40
iTaSC
Definition:
Armature.cpp:13
Generated on Sat Jul 27 2024 14:57:55 for Blender by
doxygen
1.9.1