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
4d9808ba
Commit
4d9808ba
authored
Jun 27, 2019
by
Andrea Del Prete
Browse files
Add task for joint velocity and acceleration bounds.
parent
9ee02ea9
Changes
6
Hide whitespace changes
Inline
Side-by-side
include/tsid/tasks/task-joint-bounds.hpp
View file @
4d9808ba
...
...
@@ -15,15 +15,61 @@
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_tasks_fwd_hpp__
#define __invdyn_tasks_fwd_hpp__
#ifndef __invdyn_task_joint_bounds_hpp__
#define __invdyn_task_joint_bounds_hpp__
#include
<tsid/tasks/task-motion.hpp>
#include
<tsid/math/constraint-bound.hpp>
namespace
tsid
{
namespace
tasks
{
class
TaskJointBounds
:
public
TaskMotion
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
math
::
Vector
Vector
;
typedef
math
::
ConstraintBound
ConstraintBound
;
typedef
pinocchio
::
Data
Data
;
TaskJointBounds
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
,
double
dt
);
int
dim
()
const
;
const
ConstraintBase
&
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
const
Data
&
data
);
const
ConstraintBase
&
getConstraint
()
const
;
void
setTimeStep
(
double
dt
);
void
setVelocityBounds
(
ConstRefVector
lower
,
ConstRefVector
upper
);
void
setAccelerationBounds
(
ConstRefVector
lower
,
ConstRefVector
upper
);
const
Vector
&
getAccelerationLowerBounds
()
const
;
const
Vector
&
getAccelerationUpperBounds
()
const
;
const
Vector
&
getVelocityLowerBounds
()
const
;
const
Vector
&
getVelocityUpperBounds
()
const
;
// const Vector & mask() const;
// void mask(const Vector & mask);
protected:
double
m_dt
;
int
m_na
,
m_nv
;
Vector
m_v_lb
,
m_v_ub
;
Vector
m_a_lb
,
m_a_ub
;
Vector
m_ddq_max_due_to_vel
,
m_ddq_min_due_to_vel
;
// Vector m_mask;
ConstraintBound
m_constraint
;
};
}
}
#endif // ifndef __invdyn_task
s_fwd
_hpp__
#endif // ifndef __invdyn_task
_joint_bounds
_hpp__
include/tsid/tasks/task-motion.hpp
View file @
4d9808ba
...
...
@@ -36,24 +36,25 @@ namespace tsid
TaskMotion
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
);
virtual
const
TrajectorySample
&
getReference
()
const
=
0
;
virtual
const
TrajectorySample
&
getReference
()
const
;
virtual
const
Vector
&
getDesiredAcceleration
()
const
=
0
;
virtual
const
Vector
&
getDesiredAcceleration
()
const
;
virtual
Vector
getAcceleration
(
ConstRefVector
dv
)
const
=
0
;
virtual
Vector
getAcceleration
(
ConstRefVector
dv
)
const
;
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
;
virtual
const
Vector
&
position_error
()
const
;
virtual
const
Vector
&
velocity_error
()
const
;
virtual
const
Vector
&
position
()
const
;
virtual
const
Vector
&
velocity
()
const
;
virtual
const
Vector
&
position_ref
()
const
;
virtual
const
Vector
&
velocity_ref
()
const
;
virtual
void
setMask
(
math
::
ConstRefVector
mask
);
virtual
bool
hasMask
();
protected:
math
::
Vector
m_mask
;
math
::
Vector
m_dummy
;
};
}
}
...
...
src/contacts/contact-point.cpp
View file @
4d9808ba
...
...
@@ -119,8 +119,21 @@ unsigned int ContactPoint::n_force() const { return 3; }
const
Vector
&
ContactPoint
::
Kp
()
const
{
return
m_motionTask
.
Kp
().
head
<
3
>
();
}
const
Vector
&
ContactPoint
::
Kd
()
const
{
return
m_motionTask
.
Kd
().
head
<
3
>
();
}
void
ContactPoint
::
Kp
(
ConstRefVector
Kp
){
Vector6
Kp6
;
Kp6
.
head
<
3
>
()
=
Kp
;
m_motionTask
.
Kp
(
Kp6
);
}
void
ContactPoint
::
Kd
(
ConstRefVector
Kd
){
Vector6
Kd6
;
Kd6
.
head
<
3
>
()
=
Kd
;
m_motionTask
.
Kd
(
Kd6
);
}
void
ContactPoint
::
Kp
(
ConstRefVector
Kp
)
{
assert
(
Kp
.
size
()
==
3
);
Vector6
Kp6
;
Kp6
.
head
<
3
>
()
=
Kp
;
m_motionTask
.
Kp
(
Kp6
);
}
void
ContactPoint
::
Kd
(
ConstRefVector
Kd
)
{
assert
(
Kd
.
size
()
==
3
);
Vector6
Kd6
;
Kd6
.
head
<
3
>
()
=
Kd
;
m_motionTask
.
Kd
(
Kd6
);
}
bool
ContactPoint
::
setContactNormal
(
ConstRefVector
contactNormal
)
{
...
...
src/formulations/inverse-dynamics-formulation-acc-force.cpp
View file @
4d9808ba
...
...
@@ -107,14 +107,15 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
if
(
priorityLevel
==
0
)
m_eq
+=
c
.
rows
();
}
else
if
(
c
.
isInequality
())
else
//
if(c.isInequality())
{
tl
->
constraint
=
new
ConstraintInequality
(
c
.
name
(),
c
.
rows
(),
m_v
+
m_k
);
if
(
priorityLevel
==
0
)
m_in
+=
c
.
rows
();
}
else
tl
->
constraint
=
new
ConstraintBound
(
c
.
name
(),
m_v
+
m_k
);
// 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
));
}
...
...
@@ -376,8 +377,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
}
else
{
(
*
it
)
->
constraint
->
lowerBound
().
head
(
m_v
)
=
c
.
lowerBound
();
(
*
it
)
->
constraint
->
upperBound
().
head
(
m_v
)
=
c
.
upperBound
();
(
*
it
)
->
constraint
->
matrix
().
leftCols
(
m_v
)
=
Matrix
::
Identity
(
m_v
,
m_v
);
(
*
it
)
->
constraint
->
lowerBound
()
=
c
.
lowerBound
();
(
*
it
)
->
constraint
->
upperBound
()
=
c
.
upperBound
();
}
}
...
...
src/tasks/task-joint-bounds.cpp
View file @
4d9808ba
...
...
@@ -15,14 +15,102 @@
// <http://www.gnu.org/licenses/>.
//
#include
<
Eigen/Dense
>
#include
<pinocchio/multibody/model
.hpp
>
#include
<
tsid/tasks/task-joint-bounds.hpp
>
#include
"tsid/robots/robot-wrapper
.hpp
"
namespace
tsid
{
namespace
tasks
{
using
namespace
math
;
using
namespace
trajectories
;
using
namespace
pinocchio
;
TaskJointBounds
::
TaskJointBounds
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
,
double
dt
)
:
TaskMotion
(
name
,
robot
),
m_constraint
(
name
,
robot
.
nv
()),
m_dt
(
dt
),
m_nv
(
robot
.
nv
()),
m_na
(
robot
.
na
())
{
assert
(
dt
>
0.0
);
m_v_lb
=
-
1e10
*
Vector
::
Ones
(
m_na
);
m_v_ub
=
+
1e10
*
Vector
::
Ones
(
m_na
);
m_a_lb
=
-
1e10
*
Vector
::
Ones
(
m_na
);
m_a_ub
=
+
1e10
*
Vector
::
Ones
(
m_na
);
m_ddq_max_due_to_vel
.
setZero
(
m_na
);
m_ddq_max_due_to_vel
.
setZero
(
m_na
);
int
offset
=
m_nv
-
m_na
;
for
(
int
i
=
0
;
i
<
offset
;
i
++
)
{
m_constraint
.
upperBound
()(
i
)
=
1e10
;
m_constraint
.
lowerBound
()(
i
)
=
-
1e10
;
}
}
int
TaskJointBounds
::
dim
()
const
{
return
m_nv
;
}
const
Vector
&
TaskJointBounds
::
getAccelerationLowerBounds
()
const
{
return
m_a_lb
;
}
const
Vector
&
TaskJointBounds
::
getAccelerationUpperBounds
()
const
{
return
m_a_ub
;
}
const
Vector
&
TaskJointBounds
::
getVelocityLowerBounds
()
const
{
return
m_v_lb
;
}
const
Vector
&
TaskJointBounds
::
getVelocityUpperBounds
()
const
{
return
m_v_ub
;
}
void
TaskJointBounds
::
setTimeStep
(
double
dt
)
{
assert
(
dt
>
0
);
m_dt
=
dt
;
}
void
TaskJointBounds
::
setVelocityBounds
(
ConstRefVector
lower
,
ConstRefVector
upper
)
{
assert
(
lower
.
size
()
==
m_na
);
assert
(
upper
.
size
()
==
m_na
);
m_v_lb
=
lower
;
m_v_ub
=
upper
;
}
void
TaskJointBounds
::
setAccelerationBounds
(
ConstRefVector
lower
,
ConstRefVector
upper
)
{
assert
(
lower
.
size
()
==
m_na
);
assert
(
upper
.
size
()
==
m_na
);
m_a_lb
=
lower
;
m_a_ub
=
upper
;
}
const
ConstraintBase
&
TaskJointBounds
::
getConstraint
()
const
{
return
m_constraint
;
}
const
ConstraintBase
&
TaskJointBounds
::
compute
(
const
double
,
ConstRefVector
,
ConstRefVector
v
,
const
Data
&
)
{
// compute min/max joint acc imposed by velocity limits
m_ddq_max_due_to_vel
=
(
m_v_ub
-
v
.
tail
(
m_na
))
/
m_dt
;
m_ddq_min_due_to_vel
=
(
m_v_lb
-
v
.
tail
(
m_na
))
/
m_dt
;
// take most conservative limit between vel and acc
int
offset
=
m_nv
-
m_na
;
for
(
int
i
=
0
;
i
<
m_na
;
i
++
)
{
m_constraint
.
upperBound
()(
offset
+
i
)
=
std
::
min
(
m_ddq_max_due_to_vel
(
i
),
m_a_ub
(
i
));
m_constraint
.
lowerBound
()(
offset
+
i
)
=
std
::
max
(
m_ddq_min_due_to_vel
(
i
),
m_a_lb
(
i
));
}
return
m_constraint
;
}
}
}
src/tasks/task-motion.cpp
View file @
4d9808ba
...
...
@@ -21,6 +21,10 @@ namespace tsid
{
namespace
tasks
{
typedef
math
::
Vector
Vector
;
typedef
trajectories
::
TrajectorySample
TrajectorySample
;
TaskMotion
::
TaskMotion
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
)
:
TaskBase
(
name
,
robot
)
...
...
@@ -35,5 +39,19 @@ namespace tsid
{
return
m_mask
.
size
()
>
0
;
}
const
TrajectorySample
&
TaskMotion
::
getReference
()
const
{
return
TrajectorySample
();
}
const
Vector
&
TaskMotion
::
getDesiredAcceleration
()
const
{
return
m_dummy
;
}
Vector
TaskMotion
::
getAcceleration
(
ConstRefVector
)
const
{
return
m_dummy
;
}
const
Vector
&
TaskMotion
::
position_error
()
const
{
return
m_dummy
;
}
const
Vector
&
TaskMotion
::
velocity_error
()
const
{
return
m_dummy
;
}
const
Vector
&
TaskMotion
::
position
()
const
{
return
m_dummy
;
}
const
Vector
&
TaskMotion
::
velocity
()
const
{
return
m_dummy
;
}
const
Vector
&
TaskMotion
::
position_ref
()
const
{
return
m_dummy
;
}
const
Vector
&
TaskMotion
::
velocity_ref
()
const
{
return
m_dummy
;
}
}
}
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