Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
tsid
Commits
60616a84
Unverified
Commit
60616a84
authored
Sep 25, 2020
by
Guilhem Saurel
Committed by
GitHub
Sep 25, 2020
Browse files
Merge pull request #112 from resibots/fix_leaks
Fix memory leaks by using std::shared_ptr<>
parents
7ff3f8b1
342baae9
Changes
15
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
60616a84
...
...
@@ -69,6 +69,8 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING)
ADD_DEFINITIONS
(
-DEIGEN_NO_AUTOMATIC_RESIZING
)
ENDIF
(
EIGEN_NO_AUTOMATIC_RESIZING
)
CHECK_MINIMAL_CXX_STANDARD
(
11 ENFORCE
)
# Project dependencies
ADD_PROJECT_DEPENDENCY
(
pinocchio 2.3.1 REQUIRED
PKG_CONFIG_REQUIRES
"pinocchio >= 2.3.1"
)
...
...
include/tsid/bindings/python/utils/container.hpp
View file @
60616a84
...
...
@@ -46,7 +46,7 @@ namespace tsid
stringstream
ss
;
for
(
ConstraintLevel
::
const_iterator
iit
=
m_std_const
.
begin
();
iit
!=
m_std_const
.
end
();
iit
++
)
{
const
math
::
ConstraintBase
*
c
=
iit
->
second
;
auto
c
=
iit
->
second
;
ss
<<
" - "
<<
c
->
name
()
<<
": w="
<<
iit
->
first
<<
", "
;
if
(
c
->
isEquality
())
ss
<<
"equality, "
;
...
...
@@ -62,14 +62,14 @@ namespace tsid
return
m_std_const
;
}
inline
void
append_eq
(
double
num
,
math
::
ConstraintEquality
*
i
){
m_std_const
.
push_back
(
solvers
::
make_pair
<
double
,
math
::
ConstraintBase
*
>
(
num
,
i
));
inline
void
append_eq
(
double
num
,
std
::
shared_ptr
<
math
::
ConstraintEquality
>
i
){
m_std_const
.
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
math
::
ConstraintBase
>
>
(
num
,
i
));
}
inline
void
append_ineq
(
double
num
,
math
::
ConstraintInequality
*
i
){
m_std_const
.
push_back
(
solvers
::
make_pair
<
double
,
math
::
ConstraintBase
*
>
(
num
,
i
));
inline
void
append_ineq
(
double
num
,
std
::
shared_ptr
<
math
::
ConstraintInequality
>
i
){
m_std_const
.
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
math
::
ConstraintBase
>
>
(
num
,
i
));
}
inline
void
append_bound
(
double
num
,
math
::
ConstraintBound
*
i
){
m_std_const
.
push_back
(
solvers
::
make_pair
<
double
,
math
::
ConstraintBase
*
>
(
num
,
i
));
inline
void
append_bound
(
double
num
,
std
::
shared_ptr
<
math
::
ConstraintBound
>
i
){
m_std_const
.
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
math
::
ConstraintBase
>
>
(
num
,
i
));
}
private:
ConstraintLevel
m_std_const
;
...
...
@@ -93,7 +93,7 @@ namespace tsid
ss
<<
"Level "
<<
priority
<<
endl
;
for
(
ConstraintLevel
::
const_iterator
iit
=
it
->
begin
();
iit
!=
it
->
end
();
iit
++
)
{
const
math
::
ConstraintBase
*
c
=
iit
->
second
;
auto
c
=
iit
->
second
;
ss
<<
" - "
<<
c
->
name
()
<<
": w="
<<
iit
->
first
<<
", "
;
if
(
c
->
isEquality
())
ss
<<
"equality, "
;
...
...
include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
View file @
60616a84
...
...
@@ -25,15 +25,13 @@
namespace
tsid
{
class
TaskLevel
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
tasks
::
TaskBase
&
task
;
math
::
ConstraintBase
*
constraint
;
// double weight;
std
::
shared_ptr
<
math
::
ConstraintBase
>
constraint
;
unsigned
int
priority
;
TaskLevel
(
tasks
::
TaskBase
&
task
,
...
...
@@ -46,9 +44,9 @@ namespace tsid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
contacts
::
ContactBase
&
contact
;
math
::
ConstraintBase
*
motionConstraint
;
math
::
ConstraintInequality
*
forceConstraint
;
math
::
ConstraintEquality
*
forceRegTask
;
std
::
shared_ptr
<
math
::
ConstraintBase
>
motionConstraint
;
std
::
shared_ptr
<
math
::
ConstraintInequality
>
forceConstraint
;
std
::
shared_ptr
<
math
::
ConstraintEquality
>
forceRegTask
;
unsigned
int
index
;
/// index of 1st element of associated force variable in the force vector
ContactLevel
(
contacts
::
ContactBase
&
contact
);
...
...
@@ -63,7 +61,7 @@ namespace tsid
double
time_end
;
double
fMax_start
;
/// max normal force at time time_start
double
fMax_end
;
/// max normal force at time time_end
ContactLevel
*
contactLevel
;
std
::
shared_ptr
<
ContactLevel
>
contactLevel
;
};
class
InverseDynamicsFormulationAccForce
:
...
...
@@ -140,7 +138,7 @@ namespace tsid
public:
void
addTask
(
TaskLevel
*
task
,
void
addTask
(
std
::
shared_ptr
<
TaskLevel
>
task
,
double
weight
,
unsigned
int
priorityLevel
);
...
...
@@ -152,10 +150,10 @@ namespace tsid
Data
m_data
;
HQPData
m_hqpData
;
std
::
vector
<
TaskLevel
*
>
m_taskMotions
;
std
::
vector
<
TaskLevel
*
>
m_taskContactForces
;
std
::
vector
<
TaskLevel
*
>
m_taskActuations
;
std
::
vector
<
ContactLevel
*
>
m_contacts
;
std
::
vector
<
std
::
shared_ptr
<
TaskLevel
>
>
m_taskMotions
;
std
::
vector
<
std
::
shared_ptr
<
TaskLevel
>
>
m_taskContactForces
;
std
::
vector
<
std
::
shared_ptr
<
TaskLevel
>
>
m_taskActuations
;
std
::
vector
<
std
::
shared_ptr
<
ContactLevel
>
>
m_contacts
;
double
m_t
;
/// time
unsigned
int
m_k
;
/// number of contact-force variables
unsigned
int
m_v
;
/// number of acceleration variables
...
...
@@ -163,14 +161,14 @@ namespace tsid
unsigned
int
m_eq
;
/// number of equality constraints
unsigned
int
m_in
;
/// number of inequality constraints
Matrix
m_Jc
;
/// contact force Jacobian
math
::
ConstraintEquality
m_baseDynamics
;
std
::
shared_ptr
<
math
::
ConstraintEquality
>
m_baseDynamics
;
bool
m_solutionDecoded
;
Vector
m_dv
;
Vector
m_f
;
Vector
m_tau
;
std
::
vector
<
ContactTransitionInfo
*
>
m_contactTransitions
;
std
::
vector
<
std
::
shared_ptr
<
ContactTransitionInfo
>
>
m_contactTransitions
;
};
}
...
...
include/tsid/math/constraint-base.hpp
View file @
60616a84
...
...
@@ -46,6 +46,7 @@ namespace tsid
ConstraintBase
(
const
std
::
string
&
name
,
ConstRefMatrix
A
);
virtual
~
ConstraintBase
()
{}
virtual
const
std
::
string
&
name
()
const
;
virtual
unsigned
int
rows
()
const
=
0
;
...
...
include/tsid/math/constraint-bound.hpp
View file @
60616a84
...
...
@@ -38,6 +38,7 @@ namespace tsid
ConstraintBound
(
const
std
::
string
&
name
,
ConstRefVector
lb
,
ConstRefVector
ub
);
virtual
~
ConstraintBound
()
{}
unsigned
int
rows
()
const
;
unsigned
int
cols
()
const
;
...
...
include/tsid/math/constraint-equality.hpp
View file @
60616a84
...
...
@@ -39,6 +39,7 @@ namespace tsid
ConstraintEquality
(
const
std
::
string
&
name
,
ConstRefMatrix
A
,
ConstRefVector
b
);
virtual
~
ConstraintEquality
()
{}
unsigned
int
rows
()
const
;
unsigned
int
cols
()
const
;
...
...
include/tsid/math/constraint-inequality.hpp
View file @
60616a84
...
...
@@ -40,6 +40,7 @@ namespace tsid
ConstRefMatrix
A
,
ConstRefVector
lb
,
ConstRefVector
ub
);
virtual
~
ConstraintInequality
()
{}
unsigned
int
rows
()
const
;
unsigned
int
cols
()
const
;
...
...
include/tsid/solvers/fwd.hpp
View file @
60616a84
...
...
@@ -18,6 +18,8 @@
#ifndef __invdyn_solvers_fwd_hpp__
#define __invdyn_solvers_fwd_hpp__
#include
<memory>
#include
"tsid/config.hpp"
#include
"tsid/math/fwd.hpp"
#include
<pinocchio/container/aligned-vector.hpp>
...
...
@@ -81,8 +83,8 @@ namespace tsid
{
return
aligned_pair
<
T1
,
T2
>
(
t1
,
t2
);
}
typedef
pinocchio
::
container
::
aligned_vector
<
aligned_pair
<
double
,
math
::
ConstraintBase
*
>
>
ConstraintLevel
;
typedef
pinocchio
::
container
::
aligned_vector
<
aligned_pair
<
double
,
const
math
::
ConstraintBase
*
>
>
ConstConstraintLevel
;
typedef
pinocchio
::
container
::
aligned_vector
<
aligned_pair
<
double
,
std
::
shared_ptr
<
math
::
ConstraintBase
>
>
>
ConstraintLevel
;
typedef
pinocchio
::
container
::
aligned_vector
<
aligned_pair
<
double
,
std
::
shared_ptr
<
const
math
::
ConstraintBase
>
>
>
ConstConstraintLevel
;
typedef
pinocchio
::
container
::
aligned_vector
<
ConstraintLevel
>
HQPData
;
typedef
pinocchio
::
container
::
aligned_vector
<
ConstConstraintLevel
>
ConstHQPData
;
...
...
include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx
View file @
60616a84
...
...
@@ -86,7 +86,7 @@ namespace tsid
const
unsigned
int
n
=
cl0
[
0
].
second
->
cols
();
for
(
ConstraintLevel
::
const_iterator
it
=
cl0
.
begin
();
it
!=
cl0
.
end
();
it
++
)
{
const
ConstraintBase
*
constr
=
it
->
second
;
auto
constr
=
it
->
second
;
assert
(
n
==
constr
->
cols
());
if
(
constr
->
isEquality
())
neq
+=
constr
->
rows
();
...
...
@@ -99,7 +99,7 @@ namespace tsid
int
i_eq
=
0
,
i_in
=
0
;
for
(
ConstraintLevel
::
const_iterator
it
=
cl0
.
begin
();
it
!=
cl0
.
end
();
it
++
)
{
const
ConstraintBase
*
constr
=
it
->
second
;
auto
constr
=
it
->
second
;
if
(
constr
->
isEquality
())
{
m_CE
.
middleRows
(
i_eq
,
constr
->
rows
())
=
constr
->
matrix
();
...
...
@@ -137,7 +137,7 @@ namespace tsid
for
(
ConstraintLevel
::
const_iterator
it
=
cl1
.
begin
();
it
!=
cl1
.
end
();
it
++
)
{
const
double
&
w
=
it
->
first
;
const
ConstraintBase
*
constr
=
it
->
second
;
auto
constr
=
it
->
second
;
if
(
!
constr
->
isEquality
())
assert
(
false
&&
"Inequalities in the cost function are not implemented yet"
);
...
...
@@ -196,7 +196,7 @@ namespace tsid
{
for
(
ConstraintLevel
::
const_iterator
it
=
cl0
.
begin
();
it
!=
cl0
.
end
();
it
++
)
{
const
ConstraintBase
*
constr
=
it
->
second
;
auto
constr
=
it
->
second
;
if
(
constr
->
checkConstraint
(
x
)
==
false
)
{
if
(
constr
->
isEquality
())
...
...
src/formulations/inverse-dynamics-formulation-acc-force.cpp
View file @
60616a84
...
...
@@ -44,7 +44,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
bool
verbose
)
:
InverseDynamicsFormulationBase
(
name
,
robot
,
verbose
),
m_data
(
robot
.
model
()),
m_baseDynamics
(
"base-dynamics"
,
robot
.
nv
()
-
robot
.
na
(),
robot
.
nv
()),
m_baseDynamics
(
new
math
::
ConstraintEquality
(
"base-dynamics"
,
robot
.
nv
()
-
robot
.
na
(),
robot
.
nv
())
)
,
m_solutionDecoded
(
false
)
{
m_t
=
0.0
;
...
...
@@ -55,7 +55,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
m_in
=
0
;
m_hqpData
.
resize
(
2
);
m_Jc
.
setZero
(
m_k
,
m_v
);
m_hqpData
[
0
].
push_back
(
make_pair
<
double
,
ConstraintBase
*
>
(
1.0
,
&
m_baseDynamics
));
m_hqpData
[
0
].
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
ConstraintBase
>
>
(
1.0
,
m_baseDynamics
));
}
...
...
@@ -83,7 +83,7 @@ unsigned int InverseDynamicsFormulationAccForce::nIn() const
void
InverseDynamicsFormulationAccForce
::
resizeHqpData
()
{
m_Jc
.
setZero
(
m_k
,
m_v
);
m_baseDynamics
.
resize
(
m_u
,
m_v
+
m_k
);
m_baseDynamics
->
resize
(
m_u
,
m_v
+
m_k
);
for
(
HQPData
::
iterator
it
=
m_hqpData
.
begin
();
it
!=
m_hqpData
.
end
();
it
++
)
{
for
(
ConstraintLevel
::
iterator
itt
=
it
->
begin
();
itt
!=
it
->
end
();
itt
++
)
...
...
@@ -94,7 +94,7 @@ void InverseDynamicsFormulationAccForce::resizeHqpData()
}
void
InverseDynamicsFormulationAccForce
::
addTask
(
TaskLevel
*
tl
,
void
InverseDynamicsFormulationAccForce
::
addTask
(
std
::
shared_ptr
<
TaskLevel
>
tl
,
double
weight
,
unsigned
int
priorityLevel
)
{
...
...
@@ -103,20 +103,20 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
const
ConstraintBase
&
c
=
tl
->
task
.
getConstraint
();
if
(
c
.
isEquality
())
{
tl
->
constraint
=
new
ConstraintEquality
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
tl
->
constraint
=
std
::
make_shared
<
ConstraintEquality
>
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
if
(
priorityLevel
==
0
)
m_eq
+=
c
.
rows
();
}
else
//if(c.isInequality())
{
tl
->
constraint
=
new
ConstraintInequality
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
tl
->
constraint
=
std
::
make_shared
<
ConstraintInequality
>
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
if
(
priorityLevel
==
0
)
m_in
+=
c
.
rows
();
}
// don't use bounds for now because EiQuadProg doesn't exploit them anyway
// else
// tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
m_hqpData
[
priorityLevel
].
push_back
(
make_pair
<
double
,
ConstraintBase
*
>
(
weight
,
tl
->
constraint
));
m_hqpData
[
priorityLevel
].
push_back
(
make_pair
<
double
,
std
::
shared_ptr
<
ConstraintBase
>
>
(
weight
,
tl
->
constraint
));
}
...
...
@@ -137,7 +137,7 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
if
(
transition_duration
<
0.0
)
std
::
cerr
<<
"transition_duration should be positive"
<<
std
::
endl
;
TaskLevel
*
tl
=
new
TaskLevel
(
task
,
priorityLevel
);
auto
tl
=
std
::
make_shared
<
TaskLevel
>
(
task
,
priorityLevel
);
m_taskMotions
.
push_back
(
tl
);
addTask
(
tl
,
weight
,
priorityLevel
);
...
...
@@ -160,7 +160,7 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
// This part is not used frequently so we can do some tests.
if
(
transition_duration
<
0.0
)
std
::
cerr
<<
"transition_duration should be positive"
<<
std
::
endl
;
TaskLevel
*
tl
=
new
TaskLevel
(
task
,
priorityLevel
);
auto
tl
=
std
::
make_shared
<
TaskLevel
>
(
task
,
priorityLevel
);
m_taskContactForces
.
push_back
(
tl
);
addTask
(
tl
,
weight
,
priorityLevel
);
return
true
;
...
...
@@ -182,7 +182,7 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
if
(
transition_duration
<
0.0
)
std
::
cerr
<<
"transition_duration should be positive"
<<
std
::
endl
;
TaskLevel
*
tl
=
new
TaskLevel
(
task
,
priorityLevel
);
auto
tl
=
std
::
make_shared
<
TaskLevel
>
(
task
,
priorityLevel
);
m_taskActuations
.
push_back
(
tl
);
if
(
priorityLevel
>
m_hqpData
.
size
())
...
...
@@ -191,18 +191,18 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
const
ConstraintBase
&
c
=
tl
->
task
.
getConstraint
();
if
(
c
.
isEquality
())
{
tl
->
constraint
=
new
ConstraintEquality
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
tl
->
constraint
=
std
::
make_shared
<
ConstraintEquality
>
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
if
(
priorityLevel
==
0
)
m_eq
+=
c
.
rows
();
}
else
// an actuator bound becomes an inequality because actuator forces are not in the problem variables
{
tl
->
constraint
=
new
ConstraintInequality
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
tl
->
constraint
=
std
::
make_shared
<
ConstraintInequality
>
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
if
(
priorityLevel
==
0
)
m_in
+=
c
.
rows
();
}
m_hqpData
[
priorityLevel
].
push_back
(
make_pair
<
double
,
ConstraintBase
*
>
(
weight
,
tl
->
constraint
));
m_hqpData
[
priorityLevel
].
push_back
(
make_pair
<
double
,
std
::
shared_ptr
<
ConstraintBase
>
>
(
weight
,
tl
->
constraint
));
return
true
;
}
...
...
@@ -232,23 +232,23 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact,
double
motion_weight
,
unsigned
int
motionPriorityLevel
)
{
ContactLevel
*
cl
=
new
ContactLevel
(
contact
);
auto
cl
=
std
::
make_shared
<
ContactLevel
>
(
contact
);
cl
->
index
=
m_k
;
m_k
+=
contact
.
n_force
();
m_contacts
.
push_back
(
cl
);
resizeHqpData
();
const
ConstraintBase
&
motionConstr
=
contact
.
getMotionConstraint
();
cl
->
motionConstraint
=
new
ConstraintEquality
(
contact
.
name
()
+
"_motion_task"
,
motionConstr
.
rows
(),
m_v
+
m_k
);
m_hqpData
[
motionPriorityLevel
].
push_back
(
make_pair
<
double
,
ConstraintBase
*
>
(
motion_weight
,
cl
->
motionConstraint
));
cl
->
motionConstraint
=
std
::
make_shared
<
ConstraintEquality
>
(
contact
.
name
()
+
"_motion_task"
,
motionConstr
.
rows
(),
m_v
+
m_k
);
m_hqpData
[
motionPriorityLevel
].
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
ConstraintBase
>
>
(
motion_weight
,
cl
->
motionConstraint
));
const
ConstraintInequality
&
forceConstr
=
contact
.
getForceConstraint
();
cl
->
forceConstraint
=
new
ConstraintInequality
(
contact
.
name
()
+
"_force_constraint"
,
forceConstr
.
rows
(),
m_v
+
m_k
);
m_hqpData
[
0
].
push_back
(
make_pair
<
double
,
ConstraintBase
*
>
(
1.0
,
cl
->
forceConstraint
));
cl
->
forceConstraint
=
std
::
make_shared
<
ConstraintInequality
>
(
contact
.
name
()
+
"_force_constraint"
,
forceConstr
.
rows
(),
m_v
+
m_k
);
m_hqpData
[
0
].
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
ConstraintBase
>
>
(
1.0
,
cl
->
forceConstraint
));
const
ConstraintEquality
&
forceRegConstr
=
contact
.
getForceRegularizationTask
();
cl
->
forceRegTask
=
new
ConstraintEquality
(
contact
.
name
()
+
"_force_reg_task"
,
forceRegConstr
.
rows
(),
m_v
+
m_k
);
m_hqpData
[
1
].
push_back
(
make_pair
<
double
,
ConstraintBase
*
>
(
force_regularization_weight
,
cl
->
forceRegTask
));
cl
->
forceRegTask
=
std
::
make_shared
<
ConstraintEquality
>
(
contact
.
name
()
+
"_force_reg_task"
,
forceRegConstr
.
rows
(),
m_v
+
m_k
);
m_hqpData
[
1
].
push_back
(
solvers
::
make_pair
<
double
,
std
::
shared_ptr
<
ConstraintBase
>
>
(
force_regularization_weight
,
cl
->
forceRegTask
));
m_eq
+=
motionConstr
.
rows
();
m_in
+=
forceConstr
.
rows
();
...
...
@@ -302,10 +302,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
{
m_t
=
time
;
std
::
vector
<
ContactTransitionInfo
*>::
iterator
it_ct
;
for
(
it_ct
=
m_contactTransitions
.
begin
();
it_ct
!=
m_contactTransitions
.
end
();
it_ct
++
)
for
(
auto
it_ct
=
m_contactTransitions
.
begin
();
it_ct
!=
m_contactTransitions
.
end
();
it_ct
++
)
{
const
ContactTransitionInfo
*
c
=
*
it_ct
;
auto
c
=
*
it_ct
;
assert
(
c
->
time_start
<=
m_t
);
if
(
m_t
<=
c
->
time_end
)
{
...
...
@@ -327,9 +326,10 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
m_robot
.
computeAllTerms
(
m_data
,
q
,
v
);
for
(
std
::
vector
<
ContactLevel
*>::
iterator
it
=
m_contacts
.
begin
();
it
!=
m_contacts
.
end
();
it
++
)
for
(
auto
cl
:
m_contacts
)
// std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
{
ContactLevel
*
cl
=
*
it
;
//
ContactLevel* cl = *it;
unsigned
int
m
=
cl
->
contact
.
n_force
();
const
ConstraintBase
&
mc
=
cl
->
contact
.
computeMotionTask
(
time
,
q
,
v
,
m_data
);
...
...
@@ -356,64 +356,65 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
const
Vector
&
h_u
=
m_robot
.
nonLinearEffects
(
m_data
).
head
(
m_u
);
const
Matrix
&
J_u
=
m_Jc
.
leftCols
(
m_u
);
m_baseDynamics
.
matrix
().
leftCols
(
m_v
)
=
M_u
;
m_baseDynamics
.
matrix
().
rightCols
(
m_k
)
=
-
J_u
.
transpose
();
m_baseDynamics
.
vector
()
=
-
h_u
;
m_baseDynamics
->
matrix
().
leftCols
(
m_v
)
=
M_u
;
m_baseDynamics
->
matrix
().
rightCols
(
m_k
)
=
-
J_u
.
transpose
();
m_baseDynamics
->
vector
()
=
-
h_u
;
std
::
vector
<
TaskLevel
*>::
iterator
it
;
for
(
it
=
m_taskMotions
.
begin
();
it
!=
m_taskMotions
.
end
();
it
++
)
// std::vector<TaskLevel*>::iterator it;
// for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
for
(
auto
&
it
:
m_taskMotions
)
{
const
ConstraintBase
&
c
=
(
*
it
)
->
task
.
compute
(
time
,
q
,
v
,
m_data
);
const
ConstraintBase
&
c
=
it
->
task
.
compute
(
time
,
q
,
v
,
m_data
);
if
(
c
.
isEquality
())
{
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
c
.
matrix
();
(
*
it
)
->
constraint
->
vector
()
=
c
.
vector
();
it
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
c
.
matrix
();
it
->
constraint
->
vector
()
=
c
.
vector
();
}
else
if
(
c
.
isInequality
())
{
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
c
.
matrix
();
(
*
it
)
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
(
*
it
)
->
constraint
->
upperBound
()
=
c
.
upperBound
();
it
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
c
.
matrix
();
it
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
it
->
constraint
->
upperBound
()
=
c
.
upperBound
();
}
else
{
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
Matrix
::
Identity
(
m_v
,
m_v
);
(
*
it
)
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
(
*
it
)
->
constraint
->
upperBound
()
=
c
.
upperBound
();
it
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
Matrix
::
Identity
(
m_v
,
m_v
);
it
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
it
->
constraint
->
upperBound
()
=
c
.
upperBound
();
}
}
for
(
it
=
m_taskContactForces
.
begin
();
it
!=
m_taskContactForces
.
end
();
it
++
)
for
(
auto
&
it
:
m_taskContactForces
)
{
assert
(
false
);
}
for
(
it
=
m_taskActuations
.
begin
();
it
!=
m_taskActuations
.
end
();
it
++
)
for
(
auto
&
it
:
m_taskActuations
)
{
const
ConstraintBase
&
c
=
(
*
it
)
->
task
.
compute
(
time
,
q
,
v
,
m_data
);
const
ConstraintBase
&
c
=
it
->
task
.
compute
(
time
,
q
,
v
,
m_data
);
if
(
c
.
isEquality
())
{
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
).
noalias
()
=
c
.
matrix
()
*
M_a
;
(
*
it
)
->
constraint
->
matrix
().
rightCols
(
m_k
).
noalias
()
=
-
c
.
matrix
()
*
J_a
.
transpose
();
(
*
it
)
->
constraint
->
vector
()
=
c
.
vector
();
(
*
it
)
->
constraint
->
vector
().
noalias
()
-=
c
.
matrix
()
*
h_a
;
it
->
constraint
->
matrix
().
leftCols
(
m_v
).
noalias
()
=
c
.
matrix
()
*
M_a
;
it
->
constraint
->
matrix
().
rightCols
(
m_k
).
noalias
()
=
-
c
.
matrix
()
*
J_a
.
transpose
();
it
->
constraint
->
vector
()
=
c
.
vector
();
it
->
constraint
->
vector
().
noalias
()
-=
c
.
matrix
()
*
h_a
;
}
else
if
(
c
.
isInequality
())
{
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
).
noalias
()
=
c
.
matrix
()
*
M_a
;
(
*
it
)
->
constraint
->
matrix
().
rightCols
(
m_k
).
noalias
()
=
-
c
.
matrix
()
*
J_a
.
transpose
();
(
*
it
)
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
(
*
it
)
->
constraint
->
lowerBound
().
noalias
()
-=
c
.
matrix
()
*
h_a
;
(
*
it
)
->
constraint
->
upperBound
()
=
c
.
upperBound
();
(
*
it
)
->
constraint
->
upperBound
().
noalias
()
-=
c
.
matrix
()
*
h_a
;
it
->
constraint
->
matrix
().
leftCols
(
m_v
).
noalias
()
=
c
.
matrix
()
*
M_a
;
it
->
constraint
->
matrix
().
rightCols
(
m_k
).
noalias
()
=
-
c
.
matrix
()
*
J_a
.
transpose
();
it
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
it
->
constraint
->
lowerBound
().
noalias
()
-=
c
.
matrix
()
*
h_a
;
it
->
constraint
->
upperBound
()
=
c
.
upperBound
();
it
->
constraint
->
upperBound
().
noalias
()
-=
c
.
matrix
()
*
h_a
;
}
else
{
// NB: An actuator bound becomes an inequality
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
M_a
;
(
*
it
)
->
constraint
->
matrix
().
rightCols
(
m_k
)
=
-
J_a
.
transpose
();
(
*
it
)
->
constraint
->
lowerBound
()
=
c
.
lowerBound
()
-
h_a
;
(
*
it
)
->
constraint
->
upperBound
()
=
c
.
upperBound
()
-
h_a
;
it
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
M_a
;
it
->
constraint
->
matrix
().
rightCols
(
m_k
)
=
-
J_a
.
transpose
();
it
->
constraint
->
lowerBound
()
=
c
.
lowerBound
()
-
h_a
;
it
->
constraint
->
upperBound
()
=
c
.
upperBound
()
-
h_a
;
}
}
...
...
@@ -462,12 +463,13 @@ Vector InverseDynamicsFormulationAccForce::getContactForces(const std::string &
const
HQPOutput
&
sol
)
{
decodeSolution
(
sol
);
for
(
std
::
vector
<
ContactLevel
*>::
iterator
it
=
m_contacts
.
begin
();
it
!=
m_contacts
.
end
();
it
++
)
// for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
for
(
auto
&
it
:
m_contacts
)
{
if
(
(
*
it
)
->
contact
.
name
()
==
name
)
if
(
it
->
contact
.
name
()
==
name
)
{
const
int
k
=
(
*
it
)
->
contact
.
n_force
();
return
m_f
.
segment
(
(
*
it
)
->
index
,
k
);
const
int
k
=
it
->
contact
.
n_force
();
return
m_f
.
segment
(
it
->
index
,
k
);
}
}
return
Vector
::
Zero
(
0
);
...
...
@@ -478,13 +480,13 @@ bool InverseDynamicsFormulationAccForce::getContactForces(const std::string & na
RefVector
f
)
{
decodeSolution
(
sol
);
for
(
std
::
vector
<
ContactLevel
*>::
iterator
it
=
m_contacts
.
begin
();
it
!=
m_contacts
.
end
();
it
++
)
for
(
auto
&
it
:
m_contacts
)
{
if
(
(
*
it
)
->
contact
.
name
()
==
name
)
if
(
it
->
contact
.
name
()
==
name
)
{
const
int
k
=
(
*
it
)
->
contact
.
n_force
();
const
int
k
=
it
->
contact
.
n_force
();
assert
(
f
.
size
()
==
k
);
f
=
m_f
.
segment
(
(
*
it
)
->
index
,
k
);
f
=
m_f
.
segment
(
it
->
index
,
k
);
return
true
;
}
}
...
...
@@ -501,8 +503,7 @@ bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName
removeFromHqpData
(
taskName
);
#endif
std
::
vector
<
TaskLevel
*>::
iterator
it
;
for
(
it
=
m_taskMotions
.
begin
();
it
!=
m_taskMotions
.
end
();
it
++
)
for
(
auto
it
=
m_taskMotions
.
begin
();
it
!=
m_taskMotions
.
end
();
it
++
)
{
if
((
*
it
)
->
task
.
name
()
==
taskName
)
{
...
...
@@ -517,7 +518,7 @@ bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName
return
true
;
}
}
for
(
it
=
m_taskContactForces
.
begin
();
it
!=
m_taskContactForces
.
end
();
it
++
)
for
(
auto
it
=
m_taskContactForces
.
begin
();
it
!=
m_taskContactForces
.
end
();
it
++
)
{
if
((
*
it
)
->
task
.
name
()
==
taskName
)
{
...
...
@@ -525,7 +526,7 @@ bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName
return
true
;
}
}
for
(
it
=
m_taskActuations
.
begin
();
it
!=
m_taskActuations
.
end
();
it
++
)
for
(
auto
it
=
m_taskActuations
.
begin
();
it
!=
m_taskActuations
.
end
();
it
++
)
{
if
((
*
it
)
->
task
.
name
()
==
taskName
)
{
...
...
@@ -549,25 +550,25 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
{
if
(
transition_duration
>
0.0
)
{
for
(
std
::
vector
<
ContactLevel
*>::
iterator
it
=
m_contacts
.
begin
();
it
!=
m_contacts
.
end
();
it
++
)
for
(
auto
&
it
:
m_contacts
)
{
if
(
(
*
it
)
->
contact
.
name
()
==
contactName
)
if
(
it
->
contact
.
name
()
==
contactName
)
{
ContactTransitionInfo
*
transitionInfo
=
new
ContactTransitionInfo
();
transitionInfo
->
contactLevel
=
(
*
it
)
;
auto
transitionInfo
=
std
::
make_shared
<
ContactTransitionInfo
>
();
transitionInfo
->
contactLevel
=
it
;
transitionInfo
->
time_start
=
m_t
;
transitionInfo
->
time_end
=
m_t
+
transition_duration
;
const
int
k
=
(
*
it
)
->
contact
.
n_force
();
if
(
m_f
.
size
()
>=
(
*
it
)
->
index
+
k
)
const
int
k
=
it
->
contact
.
n_force
();
if
(
m_f
.
size
()
>=
it
->
index
+
k
)