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
0042584a
Commit
0042584a
authored
Jun 27, 2019
by
Andrea Del Prete
Browse files
Add python bindings for joint-bounds task
parent
abe8dad5
Changes
7
Hide whitespace changes
Inline
Side-by-side
bindings/python/formulations/formulation.hpp
View file @
0042584a
...
...
@@ -31,6 +31,7 @@
#include
"tsid/tasks/task-se3-equality.hpp"
#include
"tsid/tasks/task-com-equality.hpp"
#include
"tsid/tasks/task-actuation-bounds.hpp"
#include
"tsid/tasks/task-joint-bounds.hpp"
namespace
tsid
{
...
...
@@ -56,6 +57,7 @@ namespace tsid
.
def
(
"addMotionTask"
,
&
InvDynPythonVisitor
::
addMotionTask_SE3
,
bp
::
args
(
"task"
,
"weight"
,
"priorityLevel"
,
"transition duration"
))
.
def
(
"addMotionTask"
,
&
InvDynPythonVisitor
::
addMotionTask_COM
,
bp
::
args
(
"task"
,
"weight"
,
"priorityLevel"
,
"transition duration"
))
.
def
(
"addMotionTask"
,
&
InvDynPythonVisitor
::
addMotionTask_Joint
,
bp
::
args
(
"task"
,
"weight"
,
"priorityLevel"
,
"transition duration"
))
.
def
(
"addMotionTask"
,
&
InvDynPythonVisitor
::
addMotionTask_JointBounds
,
bp
::
args
(
"task"
,
"weight"
,
"priorityLevel"
,
"transition duration"
))
.
def
(
"addActuationTask"
,
&
InvDynPythonVisitor
::
addActuationTask_Bounds
,
bp
::
args
(
"task"
,
"weight"
,
"priorityLevel"
,
"transition duration"
))
.
def
(
"updateTaskWeight"
,
&
InvDynPythonVisitor
::
updateTaskWeight
,
bp
::
args
(
"task_name"
,
"weight"
))
...
...
@@ -88,6 +90,9 @@ namespace tsid
static
bool
addMotionTask_Joint
(
T
&
self
,
tasks
::
TaskJointPosture
&
task
,
double
weight
,
unsigned
int
priorityLevel
,
double
transition_duration
){
return
self
.
addMotionTask
(
task
,
weight
,
priorityLevel
,
transition_duration
);
}
static
bool
addMotionTask_JointBounds
(
T
&
self
,
tasks
::
TaskJointBounds
&
task
,
double
weight
,
unsigned
int
priorityLevel
,
double
transition_duration
){
return
self
.
addMotionTask
(
task
,
weight
,
priorityLevel
,
transition_duration
);
}
static
bool
addActuationTask_Bounds
(
T
&
self
,
tasks
::
TaskActuationBounds
&
task
,
double
weight
,
unsigned
int
priorityLevel
,
double
transition_duration
){
return
self
.
addActuationTask
(
task
,
weight
,
priorityLevel
,
transition_duration
);
}
...
...
bindings/python/tasks/expose-tasks.hpp
View file @
0042584a
...
...
@@ -22,6 +22,7 @@
#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"
#include
"tsid/bindings/python/tasks/task-joint-bounds.hpp"
namespace
tsid
...
...
@@ -32,6 +33,7 @@ namespace tsid
void
exposeTaskSE3Equality
();
void
exposeTaskJointPosture
();
void
exposeTaskActuationBounds
();
void
exposeTaskJointBounds
();
inline
void
exposeTasks
()
{
...
...
@@ -39,6 +41,7 @@ namespace tsid
exposeTaskSE3Equality
();
exposeTaskJointPosture
();
exposeTaskActuationBounds
();
exposeTaskJointBounds
();
}
}
// namespace python
...
...
bindings/python/tasks/task-joint-bounds.cpp
0 → 100644
View file @
0042584a
//
// 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-joint-bounds.hpp"
#include
"tsid/bindings/python/tasks/expose-tasks.hpp"
namespace
tsid
{
namespace
python
{
void
exposeTaskJointBounds
()
{
TaskJointBoundsPythonVisitor
<
tsid
::
tasks
::
TaskJointBounds
>::
expose
(
"TaskJointBounds"
);
}
}
}
bindings/python/tasks/task-joint-bounds.hpp
0 → 100644
View file @
0042584a
//
// 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_joint_bounds_hpp__
#define __tsid_python_task_joint_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-joint-bounds.hpp"
#include
"tsid/robots/robot-wrapper.hpp"
#include
"tsid/math/constraint-bound.hpp"
#include
"tsid/math/constraint-base.hpp"
namespace
tsid
{
namespace
python
{
namespace
bp
=
boost
::
python
;
template
<
typename
Task
>
struct
TaskJointBoundsPythonVisitor
:
public
boost
::
python
::
def_visitor
<
TaskJointBoundsPythonVisitor
<
Task
>
>
{
template
<
class
PyClass
>
void
visit
(
PyClass
&
cl
)
const
{
cl
.
def
(
bp
::
init
<
std
::
string
,
robots
::
RobotWrapper
&
,
double
>
((
bp
::
arg
(
"name"
),
bp
::
arg
(
"robot"
),
bp
::
arg
(
"Time step"
)),
"Default Constructor"
))
.
add_property
(
"dim"
,
&
Task
::
dim
,
"return dimension size"
)
.
def
(
"setTimeStep"
,
&
TaskJointBoundsPythonVisitor
::
setTimeStep
,
bp
::
args
(
"dt"
))
.
def
(
"setVelocityBounds"
,
&
TaskJointBoundsPythonVisitor
::
setVelocityBounds
,
bp
::
args
(
"lower"
,
"upper"
))
.
def
(
"setAccelerationBounds"
,
&
TaskJointBoundsPythonVisitor
::
setAccelerationBounds
,
bp
::
args
(
"lower"
,
"upper"
))
.
def
(
"compute"
,
&
TaskJointBoundsPythonVisitor
::
compute
,
bp
::
args
(
"t"
,
"q"
,
"v"
,
"data"
))
.
def
(
"getConstraint"
,
&
TaskJointBoundsPythonVisitor
::
getConstraint
)
.
add_property
(
"getAccelerationLowerBounds"
,
bp
::
make_function
(
&
TaskJointBoundsPythonVisitor
::
getAccelerationLowerBounds
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()))
.
add_property
(
"getAccelerationUpperBounds"
,
bp
::
make_function
(
&
TaskJointBoundsPythonVisitor
::
getAccelerationUpperBounds
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()))
.
add_property
(
"getVelocityLowerBounds"
,
bp
::
make_function
(
&
TaskJointBoundsPythonVisitor
::
getVelocityLowerBounds
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()))
.
add_property
(
"getVelocityUpperBounds"
,
bp
::
make_function
(
&
TaskJointBoundsPythonVisitor
::
getVelocityUpperBounds
,
bp
::
return_value_policy
<
bp
::
copy_const_reference
>
()))
.
add_property
(
"name"
,
&
TaskJointBoundsPythonVisitor
::
name
)
;
}
static
std
::
string
name
(
Task
&
self
){
std
::
string
name
=
self
.
name
();
return
name
;
}
static
math
::
ConstraintBound
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
::
ConstraintBound
cons
(
self
.
getConstraint
().
name
(),
self
.
getConstraint
().
lowerBound
(),
self
.
getConstraint
().
upperBound
());
return
cons
;
}
static
math
::
ConstraintBound
getConstraint
(
const
Task
&
self
){
math
::
ConstraintBound
cons
(
self
.
getConstraint
().
name
(),
self
.
getConstraint
().
lowerBound
(),
self
.
getConstraint
().
upperBound
());
return
cons
;
}
static
const
Eigen
::
VectorXd
&
getAccelerationLowerBounds
(
const
Task
&
self
){
return
self
.
getAccelerationLowerBounds
();
}
static
const
Eigen
::
VectorXd
&
getAccelerationUpperBounds
(
const
Task
&
self
){
return
self
.
getAccelerationUpperBounds
();
}
static
const
Eigen
::
VectorXd
&
getVelocityLowerBounds
(
const
Task
&
self
){
return
self
.
getVelocityLowerBounds
();
}
static
const
Eigen
::
VectorXd
&
getVelocityUpperBounds
(
const
Task
&
self
){
return
self
.
getVelocityUpperBounds
();
}
static
void
setTimeStep
(
Task
&
self
,
const
double
dt
){
return
self
.
setTimeStep
(
dt
);
}
static
void
setVelocityBounds
(
Task
&
self
,
const
Eigen
::
VectorXd
lower
,
const
Eigen
::
VectorXd
upper
){
return
self
.
setVelocityBounds
(
lower
,
upper
);
}
static
void
setAccelerationBounds
(
Task
&
self
,
const
Eigen
::
VectorXd
lower
,
const
Eigen
::
VectorXd
upper
){
return
self
.
setAccelerationBounds
(
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
(
TaskJointBoundsPythonVisitor
<
Task
>
());
}
};
}
}
#endif // ifndef __tsid_python_task_actuation_bounds_hpp__
exercizes/ex_1_ur5.py
View file @
0042584a
...
...
@@ -7,22 +7,25 @@ import plot_utils as plut
import
time
from
tsid_manipulator
import
TsidManipulator
import
ur5_conf
as
conf
#
import ur5_reaching_conf as conf
#
import ur5_conf as conf
import
ur5_reaching_conf
as
conf
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
" Task Space Inverse Dynamics - Manipulator "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
PLOT_EE_POS
=
1
PLOT_EE_VEL
=
0
PLOT_EE_VEL
=
1
PLOT_EE_ACC
=
0
PLOT_JOINT_VEL
=
1
PLOT_TORQUES
=
1
tsid
=
TsidManipulator
(
conf
)
N
=
conf
.
N_SIMULATION
tau
=
matlib
.
empty
((
tsid
.
robot
.
na
,
N
))
*
nan
q
=
matlib
.
empty
((
tsid
.
robot
.
nq
,
N
+
1
))
*
nan
v
=
matlib
.
empty
((
tsid
.
robot
.
nv
,
N
+
1
))
*
nan
ee_pos
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_vel
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_acc
=
matlib
.
empty
((
3
,
N
))
*
nan
...
...
@@ -47,7 +50,7 @@ tsid.gui.addSphere('world/ee', conf.SPHERE_RADIUS, conf.EE_SPHERE_COLOR)
tsid
.
gui
.
addSphere
(
'world/ee_ref'
,
conf
.
REF_SPHERE_RADIUS
,
conf
.
EE_REF_SPHERE_COLOR
)
t
=
0.0
q
,
v
=
tsid
.
q
,
tsid
.
v
q
[:,
0
],
v
[:,
0
]
=
tsid
.
q
,
tsid
.
v
for
i
in
range
(
0
,
N
):
time_start
=
time
.
time
()
...
...
@@ -60,7 +63,7 @@ for i in range(0, N):
sampleEE
.
acc
(
aEE
)
tsid
.
eeTask
.
setReference
(
sampleEE
)
HQPData
=
tsid
.
formulation
.
computeProblemData
(
t
,
q
,
v
)
HQPData
=
tsid
.
formulation
.
computeProblemData
(
t
,
q
[:,
i
],
v
[:,
i
]
)
# if i == 0: HQPData.print_all()
sol
=
tsid
.
solver
.
solve
(
HQPData
)
...
...
@@ -83,11 +86,11 @@ for i in range(0, N):
print
"Time %.3f"
%
(
t
)
print
"
\t
tracking err %s: %.3f"
%
(
tsid
.
eeTask
.
name
.
ljust
(
20
,
'.'
),
norm
(
tsid
.
eeTask
.
position_error
,
2
))
q
,
v
=
tsid
.
integrate_dv
(
q
,
v
,
dv
,
conf
.
dt
)
q
[:,
i
+
1
],
v
[:,
i
+
1
]
=
tsid
.
integrate_dv
(
q
[:,
i
],
v
[:,
i
]
,
dv
,
conf
.
dt
)
t
+=
conf
.
dt
if
i
%
conf
.
DISPLAY_N
==
0
:
tsid
.
robot_display
.
display
(
q
)
tsid
.
robot_display
.
display
(
q
[:,
i
]
)
tsid
.
gui
.
applyConfiguration
(
'world/ee'
,
ee_pos
[:,
i
].
A1
.
tolist
()
+
[
0
,
0
,
0
,
1.
])
tsid
.
gui
.
applyConfiguration
(
'world/ee_ref'
,
ee_pos_ref
[:,
i
].
A1
.
tolist
()
+
[
0
,
0
,
0
,
1.
])
...
...
@@ -140,4 +143,16 @@ if(PLOT_TORQUES):
leg
=
ax
[
i
].
legend
()
leg
.
get_frame
().
set_alpha
(
0.5
)
if
(
PLOT_JOINT_VEL
):
(
f
,
ax
)
=
plut
.
create_empty_figure
(
tsid
.
robot
.
nv
/
2
,
2
)
ax
=
ax
.
reshape
(
tsid
.
robot
.
nv
)
for
i
in
range
(
tsid
.
robot
.
nv
):
ax
[
i
].
plot
(
time
,
v
[
i
,:
-
1
].
A1
,
label
=
'Joint vel '
+
str
(
i
))
ax
[
i
].
plot
([
time
[
0
],
time
[
-
1
]],
2
*
[
tsid
.
v_min
[
i
,
0
]],
':'
)
ax
[
i
].
plot
([
time
[
0
],
time
[
-
1
]],
2
*
[
tsid
.
v_max
[
i
,
0
]],
':'
)
ax
[
i
].
set_xlabel
(
'Time [s]'
)
ax
[
i
].
set_ylabel
(
'Joint velocity [rad/s]'
)
leg
=
ax
[
i
].
legend
()
leg
.
get_frame
().
set_alpha
(
0.5
)
plt
.
show
()
exercizes/tsid_manipulator.py
View file @
0042584a
...
...
@@ -57,7 +57,14 @@ class TsidManipulator:
actuationBoundsTask
.
setBounds
(
self
.
tau_min
,
self
.
tau_max
)
if
(
conf
.
w_torque_bounds
>
0.0
):
formulation
.
addActuationTask
(
actuationBoundsTask
,
conf
.
w_torque_bounds
,
0
,
0.0
)
jointBoundsTask
=
tsid
.
TaskJointBounds
(
"task-joint-bounds"
,
robot
,
conf
.
dt
)
self
.
v_max
=
conf
.
v_max_scaling
*
model
.
velocityLimit
self
.
v_min
=
-
self
.
v_max
jointBoundsTask
.
setVelocityBounds
(
self
.
v_min
,
self
.
v_max
)
if
(
conf
.
w_joint_bounds
>
0.0
):
formulation
.
addMotionTask
(
jointBoundsTask
,
conf
.
w_joint_bounds
,
0
,
0.0
)
trajPosture
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_joint"
,
q
)
postureTask
.
setReference
(
trajPosture
.
computeNext
())
...
...
@@ -67,6 +74,7 @@ class TsidManipulator:
self
.
trajPosture
=
trajPosture
self
.
postureTask
=
postureTask
self
.
actuationBoundsTask
=
actuationBoundsTask
self
.
jointBoundsTask
=
jointBoundsTask
self
.
formulation
=
formulation
self
.
solver
=
solver
self
.
q
=
q
...
...
exercizes/ur5_reaching_conf.py
View file @
0042584a
...
...
@@ -11,7 +11,7 @@ import os
np
.
set_printoptions
(
precision
=
3
,
linewidth
=
200
,
suppress
=
True
)
LINE_WIDTH
=
60
N_SIMULATION
=
30
00
# number of time steps simulated
N_SIMULATION
=
5
00
# number of time steps simulated
dt
=
0.002
# controller time step
q0
=
np
.
matrix
([[
0.
,
-
1.0
,
0.7
,
0.
,
0.
,
0.
]]).
T
# initial configuration
...
...
@@ -23,12 +23,14 @@ offset = np.matrix([0.2, 0.0, 0.0]).T
w_ee
=
1.0
# weight of end-effector task
w_posture
=
1e-3
# weight of joint posture task
w_torque_bounds
=
0.0
# weight of the torque bounds
w_torque_bounds
=
1.0
# weight of the torque bounds
w_joint_bounds
=
1.0
# weight of the joint bounds
kp_ee
=
100.0
# proportional gain of end-effector constraint
kp_posture
=
1.0
# proportional gain of joint posture task
tau_max_scaling
=
0.4
# scaling factor of torque bounds
v_max_scaling
=
0.4
# scaling factor of velocity bounds
ee_frame_name
=
"ee_fixed_joint"
# end-effector frame name
ee_task_mask
=
np
.
matrix
([
1.
,
1
,
1
,
0
,
0
,
0
]).
T
...
...
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