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
c5d08cee
Commit
c5d08cee
authored
Jun 21, 2019
by
Andrea Del Prete
Browse files
Add task for actuation bounds
parent
b51917a2
Changes
5
Hide whitespace changes
Inline
Side-by-side
bindings/python/tasks/expose-tasks.hpp
View file @
c5d08cee
...
...
@@ -21,7 +21,7 @@
#include
"tsid/bindings/python/tasks/task-com-equality.hpp"
#include
"tsid/bindings/python/tasks/task-se3-equality.hpp"
#include
"tsid/bindings/python/tasks/task-joint-posture.hpp"
#include
"tsid/bindings/python/tasks/task-actuation-bounds.hpp"
namespace
tsid
...
...
@@ -31,12 +31,14 @@ namespace tsid
void
exposeTaskComEquality
();
void
exposeTaskSE3Equality
();
void
exposeTaskJointPosture
();
void
exposeTaskActuationBounds
();
inline
void
exposeTasks
()
{
exposeTaskComEquality
();
exposeTaskSE3Equality
();
exposeTaskJointPosture
();
exposeTaskActuationBounds
();
}
}
// namespace python
...
...
bindings/python/tasks/task-actuation-bounds.cpp
0 → 100644
View file @
c5d08cee
//
// Copyright (c) 2018 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#include
"tsid/bindings/python/tasks/task-actuation-bounds.hpp"
#include
"tsid/bindings/python/tasks/expose-tasks.hpp"
namespace
tsid
{
namespace
python
{
void
exposeTaskActuationBounds
()
{
TaskActuationBoundsPythonVisitor
<
tsid
::
tasks
::
TaskActuationBounds
>::
expose
(
"TaskActuationBounds"
);
}
}
}
bindings/python/tasks/task-actuation-bounds.hpp
0 → 100644
View file @
c5d08cee
//
// Copyright (c) 2018 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#ifndef __tsid_python_task_actuation_bounds_hpp__
#define __tsid_python_task_actuation_bounds_hpp__
#include
<boost/python.hpp>
#include
<string>
#include
<eigenpy/eigenpy.hpp>
#include
<boost/python/suite/indexing/vector_indexing_suite.hpp>
#include
"tsid/tasks/task-actuation-bounds.hpp"
#include
"tsid/robots/robot-wrapper.hpp"
#include
"tsid/math/constraint-inequality.hpp"
#include
"tsid/math/constraint-base.hpp"
namespace
tsid
{
namespace
python
{
namespace
bp
=
boost
::
python
;
template
<
typename
Task
>
struct
TaskActuationBoundsPythonVisitor
:
public
boost
::
python
::
def_visitor
<
TaskActuationBoundsPythonVisitor
<
Task
>
>
{
template
<
class
PyClass
>
void
visit
(
PyClass
&
cl
)
const
{
cl
.
def
(
bp
::
init
<
std
::
string
,
robots
::
RobotWrapper
&>
((
bp
::
arg
(
"name"
),
bp
::
arg
(
"robot"
)),
"Default Constructor"
))
.
add_property
(
"dim"
,
&
Task
::
dim
,
"return dimension size"
)
.
add_property
(
"mask"
,
bp
::
make_function
(
&
TaskActuationBoundsPythonVisitor
::
getmask
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()),
"Return mask"
)
.
def
(
"mask"
,
&
TaskActuationBoundsPythonVisitor
::
setmask
,
bp
::
arg
(
"mask"
))
.
def
(
"setBounds"
,
&
TaskActuationBoundsPythonVisitor
::
setBounds
,
bp
::
args
(
"lower"
,
"upper"
))
.
def
(
"compute"
,
&
TaskActuationBoundsPythonVisitor
::
compute
,
bp
::
args
(
"t"
,
"q"
,
"v"
,
"data"
))
.
def
(
"getConstraint"
,
&
TaskActuationBoundsPythonVisitor
::
getConstraint
)
.
add_property
(
"getLowerBounds"
,
bp
::
make_function
(
&
TaskActuationBoundsPythonVisitor
::
getLowerBounds
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()))
.
add_property
(
"getUpperBounds"
,
bp
::
make_function
(
&
TaskActuationBoundsPythonVisitor
::
getUpperBounds
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()))
.
add_property
(
"name"
,
&
TaskActuationBoundsPythonVisitor
::
name
)
;
}
static
std
::
string
name
(
Task
&
self
){
std
::
string
name
=
self
.
name
();
return
name
;
}
static
math
::
ConstraintInequality
compute
(
Task
&
self
,
const
double
t
,
const
Eigen
::
VectorXd
&
q
,
const
Eigen
::
VectorXd
&
v
,
const
pinocchio
::
Data
&
data
){
self
.
compute
(
t
,
q
,
v
,
data
);
math
::
ConstraintInequality
cons
(
self
.
getConstraint
().
name
(),
self
.
getConstraint
().
matrix
(),
self
.
getConstraint
().
lowerBound
(),
self
.
getConstraint
().
upperBound
());
return
cons
;
}
static
math
::
ConstraintInequality
getConstraint
(
const
Task
&
self
){
math
::
ConstraintInequality
cons
(
self
.
getConstraint
().
name
(),
self
.
getConstraint
().
matrix
(),
self
.
getConstraint
().
lowerBound
(),
self
.
getConstraint
().
upperBound
());
return
cons
;
}
static
const
Eigen
::
VectorXd
&
getmask
(
const
Task
&
self
){
return
self
.
mask
();
}
static
void
setmask
(
Task
&
self
,
const
Eigen
::
VectorXd
mask
){
return
self
.
mask
(
mask
);
}
static
const
Eigen
::
VectorXd
&
getLowerBounds
(
const
Task
&
self
){
return
self
.
getLowerBounds
();
}
static
const
Eigen
::
VectorXd
&
getUpperBounds
(
const
Task
&
self
){
return
self
.
getUpperBounds
();
}
static
void
setBounds
(
Task
&
self
,
const
Eigen
::
VectorXd
lower
,
const
Eigen
::
VectorXd
upper
){
return
self
.
setBounds
(
lower
,
upper
);
}
static
void
expose
(
const
std
::
string
&
class_name
)
{
std
::
string
doc
=
"Task info."
;
bp
::
class_
<
Task
>
(
class_name
.
c_str
(),
doc
.
c_str
(),
bp
::
no_init
)
.
def
(
TaskActuationBoundsPythonVisitor
<
Task
>
());
}
};
}
}
#endif // ifndef __tsid_python_task_actuation_bounds_hpp__
include/tsid/tasks/task-actuation-bounds.hpp
View file @
c5d08cee
...
...
@@ -15,15 +15,56 @@
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_tasks_fwd_hpp__
#define __invdyn_tasks_fwd_hpp__
#ifndef __invdyn_task_actuation_bounds_hpp__
#define __invdyn_task_actuation_bounds_hpp__
#include
<tsid/tasks/task-actuation.hpp>
#include
<tsid/trajectories/trajectory-base.hpp>
#include
<tsid/math/constraint-inequality.hpp>
namespace
tsid
{
namespace
tasks
{
class
TaskActuationBounds
:
public
TaskActuation
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
math
::
Index
Index
;
typedef
trajectories
::
TrajectorySample
TrajectorySample
;
typedef
math
::
Vector
Vector
;
typedef
math
::
VectorXi
VectorXi
;
typedef
math
::
ConstraintInequality
ConstraintInequality
;
typedef
pinocchio
::
Data
Data
;
TaskActuationBounds
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
);
int
dim
()
const
;
const
ConstraintBase
&
compute
(
const
double
t
,
ConstRefVector
q
,
ConstRefVector
v
,
const
Data
&
data
);
const
ConstraintBase
&
getConstraint
()
const
;
void
setBounds
(
ConstRefVector
lower
,
ConstRefVector
upper
);
const
Vector
&
getLowerBounds
()
const
;
const
Vector
&
getUpperBounds
()
const
;
const
Vector
&
mask
()
const
;
void
mask
(
const
Vector
&
mask
);
protected:
Vector
m_mask
;
VectorXi
m_activeAxes
;
ConstraintInequality
m_constraint
;
};
}
}
#endif // ifndef __invdyn_task
s_fwd
_hpp__
#endif // ifndef __invdyn_task
_actuation_bounds
_hpp__
src/tasks/task-actuation-bounds.cpp
View file @
c5d08cee
...
...
@@ -15,11 +15,80 @@
// <http://www.gnu.org/licenses/>.
//
#include
<tsid/tasks/task-actuation-bounds.hpp>
#include
"tsid/robots/robot-wrapper.hpp"
namespace
tsid
{
namespace
tasks
{
using
namespace
math
;
using
namespace
trajectories
;
using
namespace
pinocchio
;
TaskActuationBounds
::
TaskActuationBounds
(
const
std
::
string
&
name
,
RobotWrapper
&
robot
)
:
TaskActuation
(
name
,
robot
),
m_constraint
(
name
,
robot
.
na
(),
robot
.
na
())
{
Vector
m
=
Vector
::
Ones
(
robot
.
na
());
mask
(
m
);
}
const
Vector
&
TaskActuationBounds
::
mask
()
const
{
return
m_mask
;
}
void
TaskActuationBounds
::
mask
(
const
Vector
&
m
)
{
assert
(
m
.
size
()
==
m_robot
.
na
());
m_mask
=
m
;
const
Vector
::
Index
dim
=
static_cast
<
Vector
::
Index
>
(
m
.
sum
());
Matrix
S
=
Matrix
::
Zero
(
dim
,
m_robot
.
na
());
m_activeAxes
.
resize
(
dim
);
unsigned
int
j
=
0
;
for
(
unsigned
int
i
=
0
;
i
<
m
.
size
();
i
++
)
if
(
m
(
i
)
!=
0.0
)
{
assert
(
m
(
i
)
==
1.0
);
S
(
j
,
i
)
=
1.0
;
m_activeAxes
(
j
)
=
i
;
j
++
;
}
m_constraint
.
resize
((
unsigned
int
)
dim
,
m_robot
.
na
());
m_constraint
.
setMatrix
(
S
);
}
int
TaskActuationBounds
::
dim
()
const
{
return
(
int
)
m_mask
.
sum
();
}
const
Vector
&
TaskActuationBounds
::
getLowerBounds
()
const
{
return
m_constraint
.
lowerBound
();
}
const
Vector
&
TaskActuationBounds
::
getUpperBounds
()
const
{
return
m_constraint
.
upperBound
();
}
void
TaskActuationBounds
::
setBounds
(
ConstRefVector
lower
,
ConstRefVector
upper
)
{
assert
(
lower
.
size
()
==
dim
());
assert
(
upper
.
size
()
==
dim
());
m_constraint
.
setLowerBound
(
lower
);
m_constraint
.
setUpperBound
(
upper
);
}
const
ConstraintBase
&
TaskActuationBounds
::
getConstraint
()
const
{
return
m_constraint
;
}
const
ConstraintBase
&
TaskActuationBounds
::
compute
(
const
double
,
ConstRefVector
,
ConstRefVector
,
const
Data
&
)
{
return
m_constraint
;
}
}
}
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