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
6d705702
Commit
6d705702
authored
Jun 17, 2019
by
Andrea Del Prete
Browse files
Add python script showing how to use TSID with a robot manipulator.
parent
b7ece956
Changes
3
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_1_ur5.py
0 → 100644
View file @
6d705702
import
numpy
as
np
import
numpy.matlib
as
matlib
from
numpy
import
nan
from
numpy.linalg
import
norm
as
norm
import
matplotlib.pyplot
as
plt
import
plot_utils
as
plut
import
time
import
ur5_conf
as
conf
from
tsid_manipulator
import
TsidManipulator
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
" Task Space Inverse Dynamics - Manipulator "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
tsid
=
TsidManipulator
(
conf
)
N
=
conf
.
N_SIMULATION
ee_pos
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_vel
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_acc
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_pos_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_vel_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_acc_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
ee_acc_des
=
matlib
.
empty
((
3
,
N
))
*
nan
# acc_des = acc_ref - Kp*pos_err - Kd*vel_err
sampleEE
=
tsid
.
trajEE
.
computeNext
()
samplePosture
=
tsid
.
trajPosture
.
computeNext
()
amp
=
np
.
matrix
([
0.02
,
0.1
,
0.10
]).
T
# amplitude
phi
=
np
.
matrix
([
0.0
,
0.5
*
np
.
pi
,
0.0
]).
T
# phase
two_pi_f
=
2
*
np
.
pi
*
np
.
matrix
([
1.0
,
0.5
,
0.5
]).
T
# frequency (time 2 PI)
offset
=
sampleEE
.
pos
()
two_pi_f_amp
=
np
.
multiply
(
two_pi_f
,
amp
)
two_pi_f_squared_amp
=
np
.
multiply
(
two_pi_f
,
two_pi_f_amp
)
pEE
=
offset
.
copy
()
vEE
=
matlib
.
zeros
((
6
,
1
))
aEE
=
matlib
.
zeros
((
6
,
1
))
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
for
i
in
range
(
0
,
N
):
time_start
=
time
.
time
()
pEE
[:
3
,
0
]
=
offset
[:
3
,
0
]
+
np
.
multiply
(
amp
,
matlib
.
sin
(
two_pi_f
*
t
+
phi
))
vEE
[:
3
,
0
]
=
np
.
multiply
(
two_pi_f_amp
,
matlib
.
cos
(
two_pi_f
*
t
+
phi
))
aEE
[:
3
,
0
]
=
np
.
multiply
(
two_pi_f_squared_amp
,
-
matlib
.
sin
(
two_pi_f
*
t
+
phi
))
sampleEE
.
pos
(
pEE
)
sampleEE
.
vel
(
vEE
)
sampleEE
.
acc
(
aEE
)
tsid
.
eeTask
.
setReference
(
sampleEE
)
HQPData
=
tsid
.
formulation
.
computeProblemData
(
t
,
q
,
v
)
# if i == 0: HQPData.print_all()
sol
=
tsid
.
solver
.
solve
(
HQPData
)
if
(
sol
.
status
!=
0
):
print
"QP problem could not be solved! Error code:"
,
sol
.
status
break
tau
=
tsid
.
formulation
.
getActuatorForces
(
sol
)
dv
=
tsid
.
formulation
.
getAccelerations
(
sol
)
ee_pos
[:,
i
]
=
tsid
.
robot
.
framePosition
(
tsid
.
formulation
.
data
(),
tsid
.
EE
).
translation
ee_vel
[:,
i
]
=
tsid
.
robot
.
frameVelocityWorldOriented
(
tsid
.
formulation
.
data
(),
tsid
.
EE
).
linear
ee_acc
[:,
i
]
=
tsid
.
eeTask
.
getAcceleration
(
dv
)[:
3
,
0
]
ee_pos_ref
[:,
i
]
=
sampleEE
.
pos
()[:
3
,
0
]
ee_vel_ref
[:,
i
]
=
sampleEE
.
vel
()[:
3
,
0
]
ee_acc_ref
[:,
i
]
=
sampleEE
.
acc
()[:
3
,
0
]
ee_acc_des
[:,
i
]
=
tsid
.
eeTask
.
getDesiredAcceleration
[:
3
,
0
]
if
i
%
conf
.
PRINT_N
==
0
:
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
)
t
+=
conf
.
dt
if
i
%
conf
.
DISPLAY_N
==
0
:
tsid
.
robot_display
.
display
(
q
)
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.
])
time_spent
=
time
.
time
()
-
time_start
if
(
time_spent
<
conf
.
dt
):
time
.
sleep
(
conf
.
dt
-
time_spent
)
# PLOT STUFF
time
=
np
.
arange
(
0.0
,
N
*
conf
.
dt
,
conf
.
dt
)
(
f
,
ax
)
=
plut
.
create_empty_figure
(
3
,
1
)
for
i
in
range
(
3
):
ax
[
i
].
plot
(
time
,
ee_pos
[
i
,:].
A1
,
label
=
'EE '
+
str
(
i
))
ax
[
i
].
plot
(
time
,
ee_pos_ref
[
i
,:].
A1
,
'r:'
,
label
=
'EE Ref '
+
str
(
i
))
ax
[
i
].
set_xlabel
(
'Time [s]'
)
ax
[
i
].
set_ylabel
(
'EE [m]'
)
leg
=
ax
[
i
].
legend
()
leg
.
get_frame
().
set_alpha
(
0.5
)
(
f
,
ax
)
=
plut
.
create_empty_figure
(
3
,
1
)
for
i
in
range
(
3
):
ax
[
i
].
plot
(
time
,
ee_vel
[
i
,:].
A1
,
label
=
'EE Vel '
+
str
(
i
))
ax
[
i
].
plot
(
time
,
ee_vel_ref
[
i
,:].
A1
,
'r:'
,
label
=
'EE Vel Ref '
+
str
(
i
))
ax
[
i
].
set_xlabel
(
'Time [s]'
)
ax
[
i
].
set_ylabel
(
'EE Vel [m/s]'
)
leg
=
ax
[
i
].
legend
()
leg
.
get_frame
().
set_alpha
(
0.5
)
(
f
,
ax
)
=
plut
.
create_empty_figure
(
3
,
1
)
for
i
in
range
(
3
):
ax
[
i
].
plot
(
time
,
ee_acc
[
i
,:].
A1
,
label
=
'EE Acc '
+
str
(
i
))
ax
[
i
].
plot
(
time
,
ee_acc_ref
[
i
,:].
A1
,
'r:'
,
label
=
'EE Acc Ref '
+
str
(
i
))
ax
[
i
].
plot
(
time
,
ee_acc_des
[
i
,:].
A1
,
'g--'
,
label
=
'EE Acc Des '
+
str
(
i
))
ax
[
i
].
set_xlabel
(
'Time [s]'
)
ax
[
i
].
set_ylabel
(
'EE Acc [m/s^2]'
)
leg
=
ax
[
i
].
legend
()
leg
.
get_frame
().
set_alpha
(
0.5
)
plt
.
show
()
exercizes/tsid_manipulator.py
0 → 100644
View file @
6d705702
import
pinocchio
as
se3
import
tsid
import
numpy
as
np
import
numpy.matlib
as
matlib
import
os
import
gepetto.corbaserver
import
time
import
commands
class
TsidManipulator
:
''' Standard TSID formulation for a robot manipulator
- end-effector task
- Postural task
- torque limits
- pos/vel limits
'''
def
__init__
(
self
,
conf
,
viewer
=
True
):
self
.
conf
=
conf
vector
=
se3
.
StdVec_StdString
()
vector
.
extend
(
item
for
item
in
conf
.
path
)
self
.
robot
=
tsid
.
RobotWrapper
(
conf
.
urdf
,
vector
,
False
)
robot
=
self
.
robot
self
.
model
=
robot
.
model
()
try
:
q
=
se3
.
getNeutralConfiguration
(
robot
.
model
(),
conf
.
srdf
,
False
)
# q = robot.model().referenceConfigurations["half_sitting"]
except
:
q
=
conf
.
q0
# q = np.matrix(np.zeros(robot.nv)).T
v
=
np
.
matrix
(
np
.
zeros
(
robot
.
nv
)).
T
assert
self
.
model
.
existFrame
(
conf
.
ee_frame_name
)
formulation
=
tsid
.
InverseDynamicsFormulationAccForce
(
"tsid"
,
robot
,
False
)
formulation
.
computeProblemData
(
0.0
,
q
,
v
)
postureTask
=
tsid
.
TaskJointPosture
(
"task-posture"
,
robot
)
postureTask
.
setKp
(
conf
.
kp_posture
*
matlib
.
ones
(
robot
.
nv
).
T
)
postureTask
.
setKd
(
2.0
*
np
.
sqrt
(
conf
.
kp_posture
)
*
matlib
.
ones
(
robot
.
nv
).
T
)
formulation
.
addMotionTask
(
postureTask
,
conf
.
w_posture
,
1
,
0.0
)
self
.
eeTask
=
tsid
.
TaskSE3Equality
(
"task-ee"
,
self
.
robot
,
self
.
conf
.
ee_frame_name
)
self
.
eeTask
.
setKp
(
self
.
conf
.
kp_ee
*
np
.
matrix
(
np
.
ones
(
6
)).
T
)
self
.
eeTask
.
setKd
(
2.0
*
np
.
sqrt
(
self
.
conf
.
kp_ee
)
*
np
.
matrix
(
np
.
ones
(
6
)).
T
)
self
.
eeTask
.
setMask
(
conf
.
ee_task_mask
)
self
.
eeTask
.
useLocalFrame
(
False
)
self
.
EE
=
robot
.
model
().
getFrameId
(
conf
.
ee_frame_name
)
H_ee_ref
=
self
.
robot
.
framePosition
(
formulation
.
data
(),
self
.
EE
)
self
.
trajEE
=
tsid
.
TrajectorySE3Constant
(
"traj-ee"
,
H_ee_ref
)
formulation
.
addMotionTask
(
self
.
eeTask
,
conf
.
w_ee
,
1
,
0.0
)
trajPosture
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_joint"
,
q
)
postureTask
.
setReference
(
trajPosture
.
computeNext
())
solver
=
tsid
.
SolverHQuadProgFast
(
"qp solver"
)
solver
.
resize
(
formulation
.
nVar
,
formulation
.
nEq
,
formulation
.
nIn
)
self
.
trajPosture
=
trajPosture
self
.
postureTask
=
postureTask
self
.
formulation
=
formulation
self
.
solver
=
solver
self
.
q
=
q
self
.
v
=
v
# for gepetto viewer
if
(
viewer
):
self
.
robot_display
=
se3
.
RobotWrapper
.
BuildFromURDF
(
conf
.
urdf
,
[
conf
.
path
,
])
l
=
commands
.
getstatusoutput
(
"ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l"
)
if
int
(
l
[
1
])
==
0
:
os
.
system
(
'gepetto-gui &'
)
time
.
sleep
(
1
)
gepetto
.
corbaserver
.
Client
()
self
.
robot_display
.
initDisplay
(
loadModel
=
True
)
self
.
robot_display
.
displayCollisions
(
False
)
self
.
robot_display
.
displayVisuals
(
True
)
self
.
robot_display
.
display
(
q
)
self
.
gui
=
self
.
robot_display
.
viewer
.
gui
self
.
gui
.
setCameraTransform
(
0
,
conf
.
CAMERA_TRANSFORM
)
def
integrate_dv
(
self
,
q
,
v
,
dv
,
dt
):
v_mean
=
v
+
0.5
*
dt
*
dv
v
+=
dt
*
dv
q
=
se3
.
integrate
(
self
.
model
,
q
,
dt
*
v_mean
)
return
q
,
v
exercizes/ur5_conf.py
0 → 100644
View file @
6d705702
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 18 09:47:07 2019
@author: student
"""
import
numpy
as
np
import
os
np
.
set_printoptions
(
precision
=
3
,
linewidth
=
200
,
suppress
=
True
)
LINE_WIDTH
=
60
N_SIMULATION
=
4000
# 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
ee_frame_name
=
"ee_fixed_joint"
# end-effector frame name
ee_task_mask
=
np
.
matrix
([
1.
,
1
,
1
,
0
,
0
,
0
]).
T
w_ee
=
1.0
# weight of end-effector task
w_posture
=
1e-3
# weight of joint posture task
kp_ee
=
2.0
# proportional gain of end-effector constraint
kp_posture
=
1.0
# proportional gain of joint posture task
PRINT_N
=
500
# print every PRINT_N time steps
DISPLAY_N
=
20
# update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM
=
[
2.582354784011841
,
1.620774507522583
,
1.0674564838409424
,
0.2770655155181885
,
0.5401807427406311
,
0.6969326734542847
,
0.3817386031150818
]
SPHERE_RADIUS
=
0.03
REF_SPHERE_RADIUS
=
0.03
EE_SPHERE_COLOR
=
(
1
,
0.5
,
0
,
0.5
)
EE_REF_SPHERE_COLOR
=
(
1
,
0
,
0
,
0.5
)
ERROR_MSG
=
'You should set the environment variable UR5_MODEL_DIR to something like "$DEVEL_DIR/install/share"
\n
'
;
path
=
os
.
environ
.
get
(
'UR5_MODEL_DIR'
,
ERROR_MSG
)
urdf
=
path
+
"/ur_description/urdf/ur5_robot.urdf"
;
srdf
=
path
+
'/ur_description/srdf/ur5_robot.srdf'
\ No newline at end of file
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