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
abe8dad5
Commit
abe8dad5
authored
Jun 27, 2019
by
Andrea Del Prete
Browse files
Add unit tests for joint bounds task
parent
4d9808ba
Changes
2
Hide whitespace changes
Inline
Side-by-side
unittest/tasks.cpp
View file @
abe8dad5
...
...
@@ -26,6 +26,7 @@
#include
<tsid/tasks/task-se3-equality.hpp>
#include
<tsid/tasks/task-com-equality.hpp>
#include
<tsid/tasks/task-joint-posture.hpp>
#include
<tsid/tasks/task-joint-bounds.hpp>
#include
<tsid/trajectories/trajectory-se3.hpp>
#include
<tsid/trajectories/trajectory-euclidian.hpp>
...
...
@@ -272,4 +273,50 @@ BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
}
}
BOOST_AUTO_TEST_CASE
(
test_task_joint_bounds
)
{
cout
<<
"
\n\n
*********** TEST TASK JOINT BOUNDS ***********
\n
"
;
vector
<
string
>
package_dirs
;
package_dirs
.
push_back
(
romeo_model_path
);
string
urdfFileName
=
package_dirs
[
0
]
+
"/urdf/romeo.urdf"
;
RobotWrapper
robot
(
urdfFileName
,
package_dirs
,
pinocchio
::
JointModelFreeFlyer
(),
false
);
const
unsigned
int
na
=
robot
.
nv
()
-
6
;
const
double
dt
=
0.001
;
cout
<<
"Gonna create task
\n
"
;
TaskJointBounds
task
(
"task-joint-bounds"
,
robot
,
dt
);
cout
<<
"Gonna set limits
\n
"
<<
na
<<
endl
;
VectorXd
dq_max
=
VectorXd
::
Ones
(
na
);
VectorXd
dq_min
=
-
dq_max
;
task
.
setVelocityBounds
(
dq_min
,
dq_max
);
BOOST_CHECK
(
task
.
getVelocityLowerBounds
().
isApprox
(
dq_min
));
BOOST_CHECK
(
task
.
getVelocityUpperBounds
().
isApprox
(
dq_max
));
cout
<<
"Gonna set up for simulation
\n
"
;
double
t
=
0.0
;
VectorXd
q
=
neutral
(
robot
.
model
());
VectorXd
v
=
VectorXd
::
Zero
(
robot
.
nv
());
pinocchio
::
Data
data
(
robot
.
model
());
for
(
int
i
=
0
;
i
<
max_it
;
i
++
)
{
robot
.
computeAllTerms
(
data
,
q
,
v
);
const
ConstraintBase
&
constraint
=
task
.
compute
(
t
,
q
,
v
,
data
);
BOOST_CHECK
(
constraint
.
rows
()
==
robot
.
nv
());
BOOST_CHECK
(
static_cast
<
tsid
::
math
::
Index
>
(
constraint
.
cols
())
==
static_cast
<
tsid
::
math
::
Index
>
(
robot
.
nv
()));
BOOST_REQUIRE
(
isFinite
(
constraint
.
lowerBound
()));
BOOST_REQUIRE
(
isFinite
(
constraint
.
upperBound
()));
BOOST_REQUIRE
(
isFinite
(
v
));
BOOST_REQUIRE
(
isFinite
(
q
));
t
+=
dt
;
}
}
BOOST_AUTO_TEST_SUITE_END
()
unittest/tsid-formulation.cpp
View file @
abe8dad5
...
...
@@ -26,6 +26,7 @@
#include
<tsid/tasks/task-com-equality.hpp>
#include
<tsid/tasks/task-se3-equality.hpp>
#include
<tsid/tasks/task-joint-posture.hpp>
#include
<tsid/tasks/task-joint-bounds.hpp>
#include
<tsid/trajectories/trajectory-euclidian.hpp>
#include
<tsid/solvers/solver-HQP-factory.hxx>
#include
<tsid/solvers/utils.hpp>
...
...
@@ -99,12 +100,13 @@ class StandardRomeoInvDynCtrl
Contact6d
*
contactLF
;
TaskComEquality
*
comTask
;
TaskJointPosture
*
postureTask
;
TaskJointBounds
*
jointBoundsTask
;
Vector
q
;
Vector
v
;
pinocchio
::
SE3
H_rf_ref
;
pinocchio
::
SE3
H_lf_ref
;
StandardRomeoInvDynCtrl
()
:
t
(
0.
)
StandardRomeoInvDynCtrl
(
double
dt
)
:
t
(
0.
)
{
vector
<
string
>
package_dirs
;
package_dirs
.
push_back
(
romeo_model_path
);
...
...
@@ -162,6 +164,13 @@ class StandardRomeoInvDynCtrl
postureTask
->
Kp
(
kp_posture
*
Vector
::
Ones
(
nv
-
6
));
postureTask
->
Kd
(
2.0
*
postureTask
->
Kp
().
cwiseSqrt
());
tsid
->
addMotionTask
(
*
postureTask
,
w_posture
,
1
);
// Add the joint bounds task
jointBoundsTask
=
new
TaskJointBounds
(
"task-joint-bounds"
,
*
robot
,
dt
);
Vector
dq_max
=
10
*
Vector
::
Ones
(
robot
->
na
());
Vector
dq_min
=
-
dq_max
;
jointBoundsTask
->
setVelocityBounds
(
dq_min
,
dq_max
);
tsid
->
addMotionTask
(
*
jointBoundsTask
,
1.0
,
0
);
}
};
...
...
@@ -194,7 +203,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
const
double
w_RF
=
1e3
;
double
t
=
0.0
;
StandardRomeoInvDynCtrl
romeo_inv_dyn
;
StandardRomeoInvDynCtrl
romeo_inv_dyn
(
dt
)
;
RobotWrapper
&
robot
=
*
(
romeo_inv_dyn
.
robot
);
InverseDynamicsFormulationAccForce
*
tsid
=
romeo_inv_dyn
.
tsid
;
Contact6d
&
contactRF
=
*
(
romeo_inv_dyn
.
contactRF
);
...
...
@@ -323,7 +332,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force )
const
unsigned
int
PRINT_N
=
100
;
double
t
=
0.0
;
StandardRomeoInvDynCtrl
romeo_inv_dyn
;
StandardRomeoInvDynCtrl
romeo_inv_dyn
(
dt
)
;
RobotWrapper
&
robot
=
*
(
romeo_inv_dyn
.
robot
);
InverseDynamicsFormulationAccForce
*
tsid
=
romeo_inv_dyn
.
tsid
;
Contact6d
&
contactRF
=
*
(
romeo_inv_dyn
.
contactRF
);
...
...
@@ -555,8 +564,8 @@ BOOST_AUTO_TEST_CASE ( test_contact_point_invdyn_formulation_acc_force )
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
ContactPoint
*
cp
=
new
ContactPoint
(
"contact_"
+
contactFrames
[
i
],
robot
,
contactFrames
[
i
],
contactNormal
,
mu
,
fMin
,
fMax
);
cp
->
Kp
(
kp_contact
*
Vector
::
Ones
(
6
));
cp
->
Kd
(
2.0
*
cp
->
Kp
().
cwiseSqrt
(
));
cp
->
Kp
(
kp_contact
*
Vector
::
Ones
(
3
));
cp
->
Kd
(
2.0
*
sqrt
(
kp_contact
)
*
Vector
::
Ones
(
3
));
cp
->
setReference
(
robot
.
framePosition
(
data
,
robot
.
model
().
getFrameId
(
contactFrames
[
i
])));
cp
->
useLocalFrame
(
false
);
tsid
->
addRigidContact
(
*
cp
,
w_forceReg
,
1.0
,
1
);
...
...
@@ -648,7 +657,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_computation_time )
const
double
dt
=
0.001
;
double
t
=
0.0
;
StandardRomeoInvDynCtrl
romeo_inv_dyn
;
StandardRomeoInvDynCtrl
romeo_inv_dyn
(
dt
)
;
RobotWrapper
&
robot
=
*
(
romeo_inv_dyn
.
robot
);
InverseDynamicsFormulationAccForce
*
tsid
=
romeo_inv_dyn
.
tsid
;
TaskComEquality
&
comTask
=
*
(
romeo_inv_dyn
.
comTask
);
...
...
@@ -672,7 +681,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_computation_time )
SolverHQPBase
*
solver_fast
=
SolverHQPFactory
::
createNewSolver
(
SOLVER_HQP_EIQUADPROG_FAST
,
"eiquadprog-fast"
);
SolverHQPBase
*
solver_rt
=
SolverHQPFactory
::
createNewSolver
<
61
,
18
,
34
>
(
SOLVER_HQP_EIQUADPROG_RT
,
SolverHQPFactory
::
createNewSolver
<
61
,
18
,
71
>
(
SOLVER_HQP_EIQUADPROG_RT
,
"eiquadprog-rt"
);
solver
->
resize
(
tsid
->
nVar
(),
tsid
->
nEq
(),
tsid
->
nIn
());
...
...
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