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
Pierre Fernbach
tsid
Commits
83889f96
Commit
83889f96
authored
May 23, 2017
by
andreadelprete
Browse files
Minor improvement everywhere
parent
7c7038e1
Changes
13
Hide whitespace changes
Inline
Side-by-side
include/pininvdyn/contacts/contact-6d.hpp
View file @
83889f96
...
...
@@ -60,19 +60,19 @@ namespace pininvdyn
virtual
const
ConstraintBase
&
computeMotionTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
);
const
Data
&
data
);
virtual
const
ConstraintInequality
&
computeForceTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
);
const
Data
&
data
);
virtual
const
Matrix
&
getForceGeneratorMatrix
();
virtual
const
ConstraintEquality
&
computeForceRegularizationTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
);
const
Data
&
data
);
const
TaskMotion
&
getMotionTask
()
const
;
const
ConstraintBase
&
getMotionConstraint
()
const
;
...
...
include/pininvdyn/contacts/contact-base.hpp
View file @
83889f96
...
...
@@ -61,19 +61,19 @@ namespace pininvdyn
virtual
const
ConstraintBase
&
computeMotionTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
=
0
;
const
Data
&
data
)
=
0
;
virtual
const
ConstraintInequality
&
computeForceTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
=
0
;
const
Data
&
data
)
=
0
;
virtual
const
Matrix
&
getForceGeneratorMatrix
()
=
0
;
virtual
const
ConstraintEquality
&
computeForceRegularizationTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
=
0
;
ConstRefVector
q
,
ConstRefVector
v
,
const
Data
&
data
)
=
0
;
virtual
const
TaskMotion
&
getMotionTask
()
const
=
0
;
virtual
const
ConstraintBase
&
getMotionConstraint
()
const
=
0
;
...
...
include/pininvdyn/tasks/task-base.hpp
View file @
83889f96
...
...
@@ -53,7 +53,7 @@ namespace pininvdyn
virtual
const
ConstraintBase
&
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
=
0
;
const
Data
&
data
)
=
0
;
virtual
const
ConstraintBase
&
getConstraint
()
const
=
0
;
...
...
include/pininvdyn/tasks/task-com-equality.hpp
View file @
83889f96
...
...
@@ -50,11 +50,15 @@ namespace pininvdyn
const
ConstraintBase
&
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
);
const
Data
&
data
);
const
ConstraintBase
&
getConstraint
()
const
;
void
setReference
(
const
TrajectorySample
&
ref
);
const
TrajectorySample
&
getReference
()
const
;
const
Vector
&
getDesiredAcceleration
()
const
;
Vector
getAcceleration
(
ConstRefVector
dv
)
const
;
const
Vector
&
position_error
()
const
;
const
Vector
&
velocity_error
()
const
;
...
...
@@ -72,6 +76,8 @@ namespace pininvdyn
Vector3
m_Kp
;
Vector3
m_Kd
;
Vector3
m_p_error
,
m_v_error
;
Vector3
m_a_des
;
Vector3
m_drift
;
Vector
m_p_error_vec
,
m_v_error_vec
;
TrajectorySample
m_ref
;
ConstraintEquality
m_constraint
;
...
...
include/pininvdyn/tasks/task-joint-posture.hpp
View file @
83889f96
...
...
@@ -47,11 +47,15 @@ namespace pininvdyn
const
ConstraintBase
&
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
);
const
Data
&
data
);
const
ConstraintBase
&
getConstraint
()
const
;
void
setReference
(
const
TrajectorySample
&
ref
);
const
TrajectorySample
&
getReference
()
const
;
const
Vector
&
getDesiredAcceleration
()
const
;
Vector
getAcceleration
(
ConstRefVector
dv
)
const
;
const
Vector
&
mask
()
const
;
bool
mask
(
const
Vector
&
mask
);
...
...
@@ -73,6 +77,7 @@ namespace pininvdyn
Vector
m_Kd
;
Vector
m_p_error
,
m_v_error
;
Vector
m_p
,
m_v
;
Vector
m_a_des
;
Vector
m_mask
;
VectorXi
m_activeAxes
;
TrajectorySample
m_ref
;
...
...
include/pininvdyn/tasks/task-motion.hpp
View file @
83889f96
...
...
@@ -19,6 +19,7 @@
#define __invdyn_task_motion_hpp__
#include
<pininvdyn/tasks/task-base.hpp>
#include
<pininvdyn/trajectories/trajectory-base.hpp>
namespace
pininvdyn
{
...
...
@@ -30,16 +31,24 @@ namespace pininvdyn
public:
typedef
pininvdyn
::
RobotWrapper
RobotWrapper
;
typedef
pininvdyn
::
math
::
Vector
Vector
;
typedef
pininvdyn
::
trajectories
::
TrajectorySample
TrajectorySample
;
TaskMotion
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
);
virtual
const
TrajectorySample
&
getReference
()
const
=
0
;
virtual
const
Vector
&
getDesiredAcceleration
()
const
=
0
;
virtual
Vector
getAcceleration
(
ConstRefVector
dv
)
const
=
0
;
virtual
const
Vector
&
position_error
()
const
=
0
;
virtual
const
Vector
&
velocity_error
()
const
=
0
;
virtual
const
Vector
&
position
()
const
=
0
;
virtual
const
Vector
&
velocity
()
const
=
0
;
virtual
const
Vector
&
position_ref
()
const
=
0
;
virtual
const
Vector
&
velocity_ref
()
const
=
0
;
};
}
}
...
...
include/pininvdyn/tasks/task-se3-equality.hpp
View file @
83889f96
...
...
@@ -50,11 +50,15 @@ namespace pininvdyn
const
ConstraintBase
&
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
);
const
Data
&
data
);
const
ConstraintBase
&
getConstraint
()
const
;
void
setReference
(
TrajectorySample
&
ref
);
const
TrajectorySample
&
getReference
()
const
;
const
Vector
&
getDesiredAcceleration
()
const
;
Vector
getAcceleration
(
ConstRefVector
dv
)
const
;
const
Vector
&
position_error
()
const
;
const
Vector
&
velocity_error
()
const
;
...
...
@@ -80,8 +84,10 @@ namespace pininvdyn
Vector
m_Kp
;
Vector
m_Kd
;
Vector
m_a_des
;
Motion
m_drift
;
Matrix6x
m_J
;
ConstraintEquality
m_constraint
;
TrajectorySample
m_ref
;
};
}
...
...
src/contacts/contact-6d.cpp
View file @
83889f96
...
...
@@ -198,7 +198,7 @@ void Contact6d::setReference(const SE3 & ref)
const
ConstraintBase
&
Contact6d
::
computeMotionTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
const
Data
&
data
)
{
return
m_motionTask
.
compute
(
t
,
q
,
v
,
data
);
}
...
...
@@ -206,7 +206,7 @@ const ConstraintBase & Contact6d::computeMotionTask(const double t,
const
ConstraintInequality
&
Contact6d
::
computeForceTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
const
Data
&
data
)
{
return
m_forceInequality
;
}
...
...
@@ -219,7 +219,7 @@ const Matrix & Contact6d::getForceGeneratorMatrix()
const
ConstraintEquality
&
Contact6d
::
computeForceRegularizationTask
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
const
Data
&
data
)
{
return
m_forceRegTask
;
}
...
...
src/inverse-dynamics-formulation-acc-force.cpp
View file @
83889f96
...
...
@@ -221,7 +221,7 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
{
const
ContactTransitionInfo
*
c
=
*
it_ct
;
assert
(
c
->
time_start
<=
m_t
);
if
(
m_t
<
c
->
time_end
)
if
(
m_t
<
=
c
->
time_end
)
{
const
double
alpha
=
(
m_t
-
c
->
time_start
)
/
(
c
->
time_end
-
c
->
time_start
);
const
double
fMax
=
c
->
fMax_start
+
alpha
*
(
c
->
fMax_end
-
c
->
fMax_start
);
...
...
@@ -229,6 +229,8 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
}
else
{
std
::
cout
<<
"[InverseDynamicsFormulationAccForce] Remove contact "
<<
c
->
contactLevel
->
contact
.
name
()
<<
" at time "
<<
time
<<
std
::
endl
;
removeRigidContact
(
c
->
contactLevel
->
contact
.
name
());
// FIXME: this won't work if multiple contact transitions occur at the same time
// because after erasing an element the iterator is invalid
...
...
src/tasks/task-com-equality.cpp
View file @
83889f96
...
...
@@ -22,6 +22,7 @@ namespace pininvdyn
namespace
tasks
{
using
namespace
pininvdyn
::
math
;
using
namespace
pininvdyn
::
trajectories
;
using
namespace
se3
;
TaskComEquality
::
TaskComEquality
(
const
std
::
string
&
name
,
...
...
@@ -60,6 +61,21 @@ namespace pininvdyn
m_ref
=
ref
;
}
const
TrajectorySample
&
TaskComEquality
::
getReference
()
const
{
return
m_ref
;
}
const
Vector
&
TaskComEquality
::
getDesiredAcceleration
()
const
{
return
m_a_des
;
}
Vector
TaskComEquality
::
getAcceleration
(
ConstRefVector
dv
)
const
{
return
m_constraint
.
matrix
()
*
dv
-
m_drift
;
}
const
Vector
&
TaskComEquality
::
position_error
()
const
{
return
m_p_error_vec
;
...
...
@@ -98,17 +114,17 @@ namespace pininvdyn
const
ConstraintBase
&
TaskComEquality
::
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
const
Data
&
data
)
{
Vector3
p_com
,
v_com
,
drift
;
m_robot
.
com
(
data
,
p_com
,
v_com
,
drift
);
Vector3
p_com
,
v_com
;
m_robot
.
com
(
data
,
p_com
,
v_com
,
m_
drift
);
// Compute errors
m_p_error
=
p_com
-
m_ref
.
pos
;
m_v_error
=
v_com
-
m_ref
.
vel
;
Vector3
m_a_des
=
-
m_Kp
.
cwiseProduct
(
m_p_error
)
-
m_Kd
.
cwiseProduct
(
m_v_error
)
+
m_ref
.
acc
;
m_a_des
=
-
m_Kp
.
cwiseProduct
(
m_p_error
)
-
m_Kd
.
cwiseProduct
(
m_v_error
)
+
m_ref
.
acc
;
m_p_error_vec
=
m_p_error
;
m_v_error_vec
=
m_v_error
;
...
...
@@ -121,7 +137,7 @@ namespace pininvdyn
const
Matrix3x
&
Jcom
=
m_robot
.
Jcom
(
data
);
m_constraint
.
setMatrix
(
Jcom
);
m_constraint
.
setVector
(
m_a_des
-
drift
);
m_constraint
.
setVector
(
m_a_des
-
m_
drift
);
return
m_constraint
;
}
...
...
src/tasks/task-joint-posture.cpp
View file @
83889f96
...
...
@@ -22,6 +22,7 @@ namespace pininvdyn
namespace
tasks
{
using
namespace
pininvdyn
::
math
;
using
namespace
pininvdyn
::
trajectories
;
using
namespace
se3
;
TaskJointPosture
::
TaskJointPosture
(
const
std
::
string
&
name
,
...
...
@@ -90,6 +91,21 @@ namespace pininvdyn
m_ref
=
ref
;
}
const
TrajectorySample
&
TaskJointPosture
::
getReference
()
const
{
return
m_ref
;
}
const
Vector
&
TaskJointPosture
::
getDesiredAcceleration
()
const
{
return
m_a_des
;
}
Vector
TaskJointPosture
::
getAcceleration
(
ConstRefVector
dv
)
const
{
return
m_constraint
.
matrix
()
*
dv
;
}
const
Vector
&
TaskJointPosture
::
position_error
()
const
{
return
m_p_error
;
...
...
@@ -128,16 +144,16 @@ namespace pininvdyn
const
ConstraintBase
&
TaskJointPosture
::
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
const
Data
&
data
)
{
// Compute errors
m_p
=
q
.
tail
(
m_robot
.
nv
()
-
6
);
m_v
=
v
.
tail
(
m_robot
.
nv
()
-
6
);
m_p_error
=
m_p
-
m_ref
.
pos
;
m_v_error
=
m_v
-
m_ref
.
vel
;
Vector
m_a_des
=
-
m_Kp
.
cwiseProduct
(
m_p_error
)
-
m_Kd
.
cwiseProduct
(
m_v_error
)
+
m_ref
.
acc
;
m_a_des
=
-
m_Kp
.
cwiseProduct
(
m_p_error
)
-
m_Kd
.
cwiseProduct
(
m_v_error
)
+
m_ref
.
acc
;
for
(
unsigned
int
i
=
0
;
i
<
m_activeAxes
.
size
();
i
++
)
m_constraint
.
vector
()(
i
)
=
m_a_des
(
m_activeAxes
(
i
));
...
...
src/tasks/task-se3-equality.cpp
View file @
83889f96
...
...
@@ -22,6 +22,7 @@ namespace pininvdyn
namespace
tasks
{
using
namespace
pininvdyn
::
math
;
using
namespace
pininvdyn
::
trajectories
;
using
namespace
se3
;
TaskSE3Equality
::
TaskSE3Equality
(
const
std
::
string
&
name
,
...
...
@@ -29,7 +30,8 @@ namespace pininvdyn
const
std
::
string
&
frameName
)
:
TaskMotion
(
name
,
robot
),
m_frame_name
(
frameName
),
m_constraint
(
name
,
6
,
robot
.
nv
())
m_constraint
(
name
,
6
,
robot
.
nv
()),
m_ref
(
12
,
6
)
{
assert
(
m_robot
.
model
().
existFrame
(
frameName
));
m_frame_id
=
m_robot
.
model
().
getFrameId
(
frameName
);
...
...
@@ -74,11 +76,17 @@ namespace pininvdyn
void
TaskSE3Equality
::
setReference
(
TrajectorySample
&
ref
)
{
m_ref
=
ref
;
vectorToSE3
(
ref
.
pos
,
m_M_ref
);
m_v_ref
=
Motion
(
ref
.
vel
);
m_a_ref
=
Motion
(
ref
.
acc
);
}
const
TrajectorySample
&
TaskSE3Equality
::
getReference
()
const
{
return
m_ref
;
}
const
Vector
&
TaskSE3Equality
::
position_error
()
const
{
return
m_p_error_vec
;
...
...
@@ -109,6 +117,16 @@ namespace pininvdyn
return
m_v_ref_vec
;
}
const
Vector
&
TaskSE3Equality
::
getDesiredAcceleration
()
const
{
return
m_a_des
;
}
Vector
TaskSE3Equality
::
getAcceleration
(
ConstRefVector
dv
)
const
{
return
m_constraint
.
matrix
()
*
dv
-
m_drift
.
toVector
();
}
const
ConstraintBase
&
TaskSE3Equality
::
getConstraint
()
const
{
return
m_constraint
;
...
...
@@ -117,13 +135,13 @@ namespace pininvdyn
const
ConstraintBase
&
TaskSE3Equality
::
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
Data
&
data
)
const
Data
&
data
)
{
SE3
oMi
;
Motion
v_frame
,
drift
;
Motion
v_frame
;
m_robot
.
framePosition
(
data
,
m_frame_id
,
oMi
);
m_robot
.
frameVelocity
(
data
,
m_frame_id
,
v_frame
);
m_robot
.
frameClassicAcceleration
(
data
,
m_frame_id
,
drift
);
m_robot
.
frameClassicAcceleration
(
data
,
m_frame_id
,
m_
drift
);
// Transformation from local to world
m_wMl
.
rotation
(
oMi
.
rotation
());
...
...
@@ -144,16 +162,16 @@ namespace pininvdyn
#endif
// desired acc in local frame
m_a_des
=
-
m_Kp
.
cwiseProduct
(
m_p_error
.
toVector
_impl
())
-
m_Kd
.
cwiseProduct
(
m_v_error
.
toVector
_impl
())
+
m_wMl
.
actInv
(
m_a_ref
).
toVector
_impl
();
m_a_des
=
-
m_Kp
.
cwiseProduct
(
m_p_error
.
toVector
())
-
m_Kd
.
cwiseProduct
(
m_v_error
.
toVector
())
+
m_wMl
.
actInv
(
m_a_ref
).
toVector
();
// @todo Since Jacobian computation is cheaper in world frame
// we could do all computations in world frame
m_robot
.
frameJacobianLocal
(
data
,
m_frame_id
,
m_J
);
m_constraint
.
setMatrix
(
m_J
);
m_constraint
.
setVector
(
m_a_des
-
drift
.
toVector
_impl
());
m_constraint
.
setVector
(
m_a_des
-
m_
drift
.
toVector
());
return
m_constraint
;
}
...
...
unittest/invdyn-formulation.cpp
View file @
83889f96
...
...
@@ -23,6 +23,7 @@
#include
<pininvdyn/contacts/contact-6d.hpp>
#include
<pininvdyn/inverse-dynamics-formulation-acc-force.hpp>
#include
<pininvdyn/tasks/task-com-equality.hpp>
#include
<pininvdyn/tasks/task-se3-equality.hpp>
#include
<pininvdyn/tasks/task-joint-posture.hpp>
#include
<pininvdyn/trajectories/trajectory-euclidian.hpp>
#include
<pininvdyn/solvers/solver-HQP-base.hpp>
...
...
@@ -47,6 +48,17 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(is_finite(A), #A<<": "<<A)
#define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B)
#define REQUIRE_TASK_FINITE(task) REQUIRE_FINITE(task.getConstraint().matrix()); \
REQUIRE_FINITE(task.getConstraint().vector())
#define REQUIRE_CONTACT_FINITE(contact) REQUIRE_FINITE(contact.getMotionConstraint().matrix()); \
REQUIRE_FINITE(contact.getMotionConstraint().vector()); \
REQUIRE_FINITE(contact.getForceConstraint().matrix()); \
REQUIRE_FINITE(contact.getForceConstraint().lowerBound()); \
REQUIRE_FINITE(contact.getForceConstraint().upperBound()); \
REQUIRE_FINITE(contact.getForceRegularizationTask().matrix()); \
REQUIRE_FINITE(contact.getForceRegularizationTask().vector())
class
StandardHrp2InvDynCtrl
{
public:
...
...
@@ -145,6 +157,8 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
const
unsigned
int
PRINT_N
=
10
;
const
unsigned
int
REMOVE_CONTACT_N
=
100
;
const
double
CONTACT_TRANSITION_TIME
=
1.0
;
const
double
kp_RF
=
100.0
;
const
double
w_RF
=
1e3
;
double
t
=
0.0
;
StandardHrp2InvDynCtrl
hrp2_inv_dyn
;
...
...
@@ -158,6 +172,20 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
Vector
v
=
hrp2_inv_dyn
.
v
;
const
int
nv
=
robot
.
model
().
nv
;
// Add the right foot task
TaskSE3Equality
*
rightFootTask
=
new
TaskSE3Equality
(
"task-right-foot"
,
robot
,
hrp2_inv_dyn
.
rf_frame_name
);
rightFootTask
->
Kp
(
kp_RF
*
Vector
::
Ones
(
6
));
rightFootTask
->
Kd
(
2.0
*
rightFootTask
->
Kp
().
cwiseSqrt
());
se3
::
SE3
H_rf_ref
=
robot
.
position
(
invDyn
->
data
(),
robot
.
model
().
getJointId
(
hrp2_inv_dyn
.
rf_frame_name
));
invDyn
->
addMotionTask
(
*
rightFootTask
,
w_RF
,
1
);
TrajectorySample
s
(
12
,
6
);
se3ToVector
(
H_rf_ref
,
s
.
pos
);
rightFootTask
->
setReference
(
s
);
Vector3
com_ref
=
robot
.
com
(
invDyn
->
data
());
com_ref
(
1
)
+=
0.1
;
TrajectoryBase
*
trajCom
=
new
TrajectoryEuclidianConstant
(
"traj_com"
,
com_ref
);
...
...
@@ -172,13 +200,15 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
"solver-eiquadprog"
);
solver
->
resize
(
invDyn
->
nVar
(),
invDyn
->
nEq
(),
invDyn
->
nIn
());
Vector
tau_old
(
nv
-
6
);
for
(
int
i
=
0
;
i
<
N_DT
;
i
++
)
{
if
(
i
==
REMOVE_CONTACT_N
)
{
cout
<<
"
B
reak contact right foot
\n
"
;
cout
<<
"
Start b
reak
ing
contact right foot
\n
"
;
invDyn
->
removeRigidContact
(
contactRF
.
name
(),
CONTACT_TRANSITION_TIME
);
}
sampleCom
=
trajCom
->
computeNext
();
comTask
.
setReference
(
sampleCom
);
samplePosture
=
trajPosture
->
computeNext
();
...
...
@@ -188,24 +218,10 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
if
(
i
==
0
)
cout
<<
hqpDataToString
(
hqpData
,
false
)
<<
endl
;
REQUIRE_FINITE
(
postureTask
.
getConstraint
().
matrix
());
REQUIRE_FINITE
(
postureTask
.
getConstraint
().
vector
());
REQUIRE_FINITE
(
comTask
.
getConstraint
().
matrix
());
REQUIRE_FINITE
(
comTask
.
getConstraint
().
vector
());
REQUIRE_FINITE
(
contactRF
.
getMotionConstraint
().
matrix
());
REQUIRE_FINITE
(
contactRF
.
getMotionConstraint
().
vector
());
REQUIRE_FINITE
(
contactRF
.
getForceConstraint
().
matrix
());
REQUIRE_FINITE
(
contactRF
.
getForceConstraint
().
lowerBound
());
REQUIRE_FINITE
(
contactRF
.
getForceConstraint
().
upperBound
());
REQUIRE_FINITE
(
contactRF
.
getForceRegularizationTask
().
matrix
());
REQUIRE_FINITE
(
contactRF
.
getForceRegularizationTask
().
vector
());
REQUIRE_FINITE
(
contactLF
.
getMotionConstraint
().
matrix
());
REQUIRE_FINITE
(
contactLF
.
getMotionConstraint
().
vector
());
REQUIRE_FINITE
(
contactLF
.
getForceConstraint
().
matrix
());
REQUIRE_FINITE
(
contactLF
.
getForceConstraint
().
lowerBound
());
REQUIRE_FINITE
(
contactLF
.
getForceConstraint
().
upperBound
());
REQUIRE_FINITE
(
contactLF
.
getForceRegularizationTask
().
matrix
());
REQUIRE_FINITE
(
contactLF
.
getForceRegularizationTask
().
vector
());
REQUIRE_TASK_FINITE
(
postureTask
);
REQUIRE_TASK_FINITE
(
comTask
);
REQUIRE_CONTACT_FINITE
(
contactRF
);
REQUIRE_CONTACT_FINITE
(
contactLF
);
CHECK_LESS_THAN
(
contactRF
.
getMotionTask
().
position_error
().
norm
(),
1e-3
);
CHECK_LESS_THAN
(
contactLF
.
getMotionTask
().
position_error
().
norm
(),
1e-3
);
...
...
@@ -217,6 +233,23 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
const
Vector
&
tau
=
invDyn
->
getActuatorForces
(
sol
);
const
Vector
&
dv
=
invDyn
->
getAccelerations
(
sol
);
if
(
i
>
0
)
{
CHECK_LESS_THAN
((
tau
-
tau_old
).
norm
(),
1e1
);
if
((
tau
-
tau_old
).
norm
()
>
1e1
)
// || (i>=197 && i<=200))
{
// contactRF.computeMotionTask(t, q, v, invDyn->data());
// rightFootTask->compute(t, q, v, invDyn->data());
cout
<<
"Time "
<<
i
<<
endl
;
cout
<<
"tau:
\n
"
<<
tau
.
transpose
()
<<
"
\n
tauOld:
\n
"
<<
tau_old
.
transpose
()
<<
"
\n
"
;
// cout << "RF contact task des acc: "<<contactRF.getMotionTask().getDesiredAcceleration().transpose()<<endl;
// cout << "RF contact task acc: "<<contactRF.getMotionTask().getAcceleration(dv).transpose()<<endl;
// cout << "RF motion task des acc: "<<rightFootTask->getDesiredAcceleration().transpose()<<endl;
cout
<<
endl
;
}
}
tau_old
=
tau
;
if
(
i
%
PRINT_N
==
0
)
{
cout
<<
"Time "
<<
i
<<
endl
;
...
...
@@ -306,24 +339,10 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force )
if
(
i
==
0
)
cout
<<
hqpDataToString
(
hqpData
,
false
)
<<
endl
;
REQUIRE_FINITE
(
postureTask
.
getConstraint
().
matrix
());
REQUIRE_FINITE
(
postureTask
.
getConstraint
().
vector
());
REQUIRE_FINITE
(
comTask
.
getConstraint
().
matrix
());
REQUIRE_FINITE
(
comTask
.
getConstraint
().
vector
());
REQUIRE_FINITE
(
contactRF
.
getMotionConstraint
().
matrix
());
REQUIRE_FINITE
(
contactRF
.
getMotionConstraint
().
vector
());
REQUIRE_FINITE
(
contactRF
.
getForceConstraint
().
matrix
());
REQUIRE_FINITE
(
contactRF
.
getForceConstraint
().
lowerBound
());
REQUIRE_FINITE
(
contactRF
.
getForceConstraint
().
upperBound
());
REQUIRE_FINITE
(
contactRF
.
getForceRegularizationTask
().
matrix
());
REQUIRE_FINITE
(
contactRF
.
getForceRegularizationTask
().
vector
());
REQUIRE_FINITE
(
contactLF
.
getMotionConstraint
().
matrix
());
REQUIRE_FINITE
(
contactLF
.
getMotionConstraint
().
vector
());
REQUIRE_FINITE
(
contactLF
.
getForceConstraint
().
matrix
());
REQUIRE_FINITE
(
contactLF
.
getForceConstraint
().
lowerBound
());
REQUIRE_FINITE
(
contactLF
.
getForceConstraint
().
upperBound
());
REQUIRE_FINITE
(
contactLF
.
getForceRegularizationTask
().
matrix
());
REQUIRE_FINITE
(
contactLF
.
getForceRegularizationTask
().
vector
());
REQUIRE_TASK_FINITE
(
postureTask
);
REQUIRE_TASK_FINITE
(
comTask
);
REQUIRE_CONTACT_FINITE
(
contactRF
);
REQUIRE_CONTACT_FINITE
(
contactLF
);
CHECK_LESS_THAN
(
contactRF
.
getMotionTask
().
position_error
().
norm
(),
1e-3
);
CHECK_LESS_THAN
(
contactLF
.
getMotionTask
().
position_error
().
norm
(),
1e-3
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment