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
e8642f31
Commit
e8642f31
authored
Apr 18, 2019
by
student
Browse files
Add third exercize with GUI to allow user to play with TSID for balancing a biped
parent
9ff17681
Changes
5
Hide whitespace changes
Inline
Side-by-side
exercizes/ex_1.py
View file @
e8642f31
import
pinocchio
as
se3
import
tsid
import
numpy
as
np
import
numpy.matlib
as
matlib
from
numpy
import
nan
from
numpy.linalg
import
norm
as
norm
import
os
import
matplotlib.pyplot
as
plt
import
plot_utils
as
plut
import
gepetto.corbaserver
import
time
import
commands
np
.
set_printoptions
(
precision
=
3
,
linewidth
=
200
,
suppress
=
True
)
LINE_WIDTH
=
60
print
""
.
center
(
LINE_WIDTH
,
'#'
)
print
" Test Task Space Inverse Dynamics "
.
center
(
LINE_WIDTH
,
'#'
)
print
""
.
center
(
LINE_WIDTH
,
'#'
),
'
\n
'
lxp
=
0.14
# foot length in positive x direction
lxn
=
0.077
# foot length in negative x direction
lyp
=
0.069
# foot length in positive y direction
lyn
=
0.069
# foot length in negative y direction
lz
=
0.105
# foot sole height with respect to ankle joint
mu
=
0.3
# friction coefficient
fMin
=
5.0
# minimum normal force
fMax
=
1000.0
# maximum normal force
rf_frame_name
=
"RAnkleRoll"
# right foot frame name
lf_frame_name
=
"LAnkleRoll"
# left foot frame name
contactNormal
=
np
.
matrix
([
0.
,
0.
,
1.
]).
T
# direction of the normal to the contact surface
w_com
=
1.0
# weight of center of mass task
w_posture
=
1e-3
# weight of joint posture task
w_forceRef
=
1e-5
# weight of force regularization task
kp_contact
=
10.0
# proportional gain of contact constraint
kp_com
=
10.0
# proportional gain of center of mass task
kp_posture
=
10.0
# proportional gain of joint posture task
dt
=
0.001
# controller time step
PRINT_N
=
500
# print every PRINT_N time steps
DISPLAY_N
=
25
# update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION
=
4000
# number of time steps simulated
filename
=
str
(
os
.
path
.
dirname
(
os
.
path
.
abspath
(
__file__
)))
path
=
filename
+
'/../models/romeo'
urdf
=
path
+
'/urdf/romeo.urdf'
vector
=
se3
.
StdVec_StdString
()
vector
.
extend
(
item
for
item
in
path
)
robot
=
tsid
.
RobotWrapper
(
urdf
,
vector
,
se3
.
JointModelFreeFlyer
(),
False
)
srdf
=
path
+
'/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display
=
se3
.
RobotWrapper
.
BuildFromURDF
(
urdf
,
[
path
,
],
se3
.
JointModelFreeFlyer
())
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
)
cl
=
gepetto
.
corbaserver
.
Client
()
gui
=
cl
.
gui
robot_display
.
initDisplay
(
loadModel
=
True
)
q
=
se3
.
getNeutralConfiguration
(
robot
.
model
(),
srdf
,
False
)
q
[
2
]
+=
0.84
v
=
np
.
matrix
(
np
.
zeros
(
robot
.
nv
)).
T
robot_display
.
displayCollisions
(
False
)
robot_display
.
displayVisuals
(
True
)
robot_display
.
display
(
q
)
assert
robot
.
model
().
existFrame
(
rf_frame_name
)
assert
robot
.
model
().
existFrame
(
lf_frame_name
)
t
=
0.0
# time
invdyn
=
tsid
.
InverseDynamicsFormulationAccForce
(
"tsid"
,
robot
,
False
)
invdyn
.
computeProblemData
(
t
,
q
,
v
)
data
=
invdyn
.
data
()
contact_Point
=
np
.
matrix
(
np
.
ones
((
3
,
4
))
*
lz
)
contact_Point
[
0
,
:]
=
[
-
lxn
,
-
lxn
,
lxp
,
lxp
]
contact_Point
[
1
,
:]
=
[
-
lyn
,
lyp
,
-
lyn
,
lyp
]
contactRF
=
tsid
.
Contact6d
(
"contact_rfoot"
,
robot
,
rf_frame_name
,
contact_Point
,
contactNormal
,
mu
,
fMin
,
fMax
)
contactRF
.
setKp
(
kp_contact
*
matlib
.
ones
(
6
).
T
)
contactRF
.
setKd
(
2.0
*
np
.
sqrt
(
kp_contact
)
*
matlib
.
ones
(
6
).
T
)
H_rf_ref
=
robot
.
position
(
data
,
robot
.
model
().
getJointId
(
rf_frame_name
))
contactRF
.
setReference
(
H_rf_ref
)
invdyn
.
addRigidContact
(
contactRF
,
w_forceRef
)
contactLF
=
tsid
.
Contact6d
(
"contact_lfoot"
,
robot
,
lf_frame_name
,
contact_Point
,
contactNormal
,
mu
,
fMin
,
fMax
)
contactLF
.
setKp
(
kp_contact
*
matlib
.
ones
(
6
).
T
)
contactLF
.
setKd
(
2.0
*
np
.
sqrt
(
kp_contact
)
*
matlib
.
ones
(
6
).
T
)
H_lf_ref
=
robot
.
position
(
data
,
robot
.
model
().
getJointId
(
lf_frame_name
))
contactLF
.
setReference
(
H_lf_ref
)
invdyn
.
addRigidContact
(
contactLF
,
w_forceRef
)
comTask
=
tsid
.
TaskComEquality
(
"task-com"
,
robot
)
comTask
.
setKp
(
kp_com
*
matlib
.
ones
(
3
).
T
)
comTask
.
setKd
(
2.0
*
np
.
sqrt
(
kp_com
)
*
matlib
.
ones
(
3
).
T
)
invdyn
.
addMotionTask
(
comTask
,
w_com
,
1
,
0.0
)
postureTask
=
tsid
.
TaskJointPosture
(
"task-posture"
,
robot
)
postureTask
.
setKp
(
kp_posture
*
matlib
.
ones
(
robot
.
nv
-
6
).
T
)
postureTask
.
setKd
(
2.0
*
np
.
sqrt
(
kp_posture
)
*
matlib
.
ones
(
robot
.
nv
-
6
).
T
)
invdyn
.
addMotionTask
(
postureTask
,
w_posture
,
1
,
0.0
)
com_ref
=
robot
.
com
(
data
)
#com_ref[1] += 0.05
trajCom
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_com"
,
com_ref
)
q_ref
=
q
[
7
:]
trajPosture
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_joint"
,
q_ref
)
solver
=
tsid
.
SolverHQuadProgFast
(
"qp solver"
)
solver
.
resize
(
invdyn
.
nVar
,
invdyn
.
nEq
,
invdyn
.
nIn
)
com_pos
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_vel
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_acc
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_pos_ref
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_vel_ref
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_acc_ref
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_acc_des
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
# acc_des = acc_ref - Kp*pos_err - Kd*vel_err
for
i
in
range
(
0
,
N_SIMULATION
):
import
romeo_conf
as
conf
from
tsid_biped
import
TsidBiped
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
" Test Task Space Inverse Dynamics "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
tsid
=
TsidBiped
(
conf
)
N
=
conf
.
N_SIMULATION
com_pos
=
matlib
.
empty
((
3
,
N
))
*
nan
com_vel
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc
=
matlib
.
empty
((
3
,
N
))
*
nan
com_pos_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
com_vel_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc_des
=
matlib
.
empty
((
3
,
N
))
*
nan
# acc_des = acc_ref - Kp*pos_err - Kd*vel_err
t
=
0.0
q
,
v
=
tsid
.
q
,
tsid
.
v
for
i
in
range
(
0
,
N
):
time_start
=
time
.
time
()
sampleCom
=
trajCom
.
computeNext
()
comTask
.
setReference
(
sampleCom
)
samplePosture
=
trajPosture
.
computeNext
()
postureTask
.
setReference
(
samplePosture
)
sampleCom
=
tsid
.
trajCom
.
computeNext
()
tsid
.
comTask
.
setReference
(
sampleCom
)
samplePosture
=
tsid
.
trajPosture
.
computeNext
()
tsid
.
postureTask
.
setReference
(
samplePosture
)
HQPData
=
invdy
n
.
computeProblemData
(
t
,
q
,
v
)
HQPData
=
tsid
.
formulatio
n
.
computeProblemData
(
t
,
q
,
v
)
# if i == 0: HQPData.print_all()
sol
=
solver
.
solve
(
HQPData
)
sol
=
tsid
.
solver
.
solve
(
HQPData
)
if
(
sol
.
status
!=
0
):
print
"QP problem could not be solved! Error code:"
,
sol
.
status
break
tau
=
invdy
n
.
getActuatorForces
(
sol
)
dv
=
invdy
n
.
getAccelerations
(
sol
)
tau
=
tsid
.
formulatio
n
.
getActuatorForces
(
sol
)
dv
=
tsid
.
formulatio
n
.
getAccelerations
(
sol
)
com_pos
[:,
i
]
=
robot
.
com
(
invdy
n
.
data
())
com_vel
[:,
i
]
=
robot
.
com_vel
(
invdy
n
.
data
())
com_acc
[:,
i
]
=
comTask
.
getAcceleration
(
dv
)
com_pos
[:,
i
]
=
tsid
.
robot
.
com
(
tsid
.
formulatio
n
.
data
())
com_vel
[:,
i
]
=
tsid
.
robot
.
com_vel
(
tsid
.
formulatio
n
.
data
())
com_acc
[:,
i
]
=
tsid
.
comTask
.
getAcceleration
(
dv
)
com_pos_ref
[:,
i
]
=
sampleCom
.
pos
()
com_vel_ref
[:,
i
]
=
sampleCom
.
vel
()
com_acc_ref
[:,
i
]
=
sampleCom
.
acc
()
com_acc_des
[:,
i
]
=
comTask
.
getDesiredAcceleration
com_acc_des
[:,
i
]
=
tsid
.
comTask
.
getDesiredAcceleration
if
i
%
PRINT_N
==
0
:
if
i
%
conf
.
PRINT_N
==
0
:
print
"Time %.3f"
%
(
t
)
if
invdy
n
.
checkContact
(
contactRF
.
name
,
sol
):
f
=
invdy
n
.
getContactForce
(
contactRF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
contactRF
.
name
.
ljust
(
20
,
'.'
),
contactRF
.
getNormalForce
(
f
))
if
tsid
.
formulatio
n
.
checkContact
(
tsid
.
contactRF
.
name
,
sol
):
f
=
tsid
.
formulatio
n
.
getContactForce
(
tsid
.
contactRF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
tsid
.
contactRF
.
name
.
ljust
(
20
,
'.'
),
tsid
.
contactRF
.
getNormalForce
(
f
))
if
invdy
n
.
checkContact
(
contactLF
.
name
,
sol
):
f
=
invdy
n
.
getContactForce
(
contactLF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
contactLF
.
name
.
ljust
(
20
,
'.'
),
contactLF
.
getNormalForce
(
f
))
if
tsid
.
formulatio
n
.
checkContact
(
tsid
.
contactLF
.
name
,
sol
):
f
=
tsid
.
formulatio
n
.
getContactForce
(
tsid
.
contactLF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
tsid
.
contactLF
.
name
.
ljust
(
20
,
'.'
),
tsid
.
contactLF
.
getNormalForce
(
f
))
print
"
\t
tracking err %s: %.3f"
%
(
comTask
.
name
.
ljust
(
20
,
'.'
),
norm
(
comTask
.
position_error
,
2
))
print
"
\t
tracking err %s: %.3f"
%
(
tsid
.
comTask
.
name
.
ljust
(
20
,
'.'
),
norm
(
tsid
.
comTask
.
position_error
,
2
))
print
"
\t
||v||: %.3f
\t
||dv||: %.3f"
%
(
norm
(
v
,
2
),
norm
(
dv
))
v_mean
=
v
+
0.5
*
dt
*
dv
v
+=
dt
*
dv
q
=
se3
.
integrate
(
robot
.
model
(),
q
,
dt
*
v_mean
)
t
+=
dt
q
,
v
=
tsid
.
integrate_dv
(
q
,
v
,
dv
,
conf
.
dt
)
t
+=
conf
.
dt
if
i
%
DISPLAY_N
==
0
:
robot_display
.
display
(
q
)
if
i
%
conf
.
DISPLAY_N
==
0
:
tsid
.
robot_display
.
display
(
q
)
time_spent
=
time
.
time
()
-
time_start
if
(
time_spent
<
dt
):
time
.
sleep
(
dt
-
time_spent
)
if
(
time_spent
<
conf
.
dt
):
time
.
sleep
(
conf
.
dt
-
time_spent
)
# PLOT STUFF
time
=
np
.
arange
(
0.0
,
N
_SIMULATION
*
dt
,
dt
)
time
=
np
.
arange
(
0.0
,
N
*
conf
.
dt
,
conf
.
dt
)
(
f
,
ax
)
=
plut
.
create_empty_figure
(
3
,
1
)
for
i
in
range
(
3
):
...
...
exercizes/ex_2.py
View file @
e8642f31
import
pinocchio
as
se3
import
tsid
import
numpy
as
np
import
numpy.matlib
as
matlib
from
numpy
import
nan
from
numpy.linalg
import
norm
as
norm
import
os
import
matplotlib.pyplot
as
plt
import
plot_utils
as
plut
import
gepetto.corbaserver
import
time
import
commands
np
.
set_printoptions
(
precision
=
3
,
linewidth
=
200
,
suppress
=
True
)
LINE_WIDTH
=
60
print
""
.
center
(
LINE_WIDTH
,
'#'
)
print
" Test Task Space Inverse Dynamics "
.
center
(
LINE_WIDTH
,
'#'
)
print
""
.
center
(
LINE_WIDTH
,
'#'
),
'
\n
'
lxp
=
0.14
# foot length in positive x direction
lxn
=
0.077
# foot length in negative x direction
lyp
=
0.069
# foot length in positive y direction
lyn
=
0.069
# foot length in negative y direction
lz
=
0.105
# foot sole height with respect to ankle joint
mu
=
0.3
# friction coefficient
fMin
=
5.0
# minimum normal force
fMax
=
1000.0
# maximum normal force
rf_frame_name
=
"RAnkleRoll"
# right foot frame name
lf_frame_name
=
"LAnkleRoll"
# left foot frame name
contactNormal
=
np
.
matrix
([
0.
,
0.
,
1.
]).
T
# direction of the normal to the contact surface
w_com
=
1.0
# weight of center of mass task
w_posture
=
1e-3
# weight of joint posture task
w_forceRef
=
1e-5
# weight of force regularization task
kp_contact
=
10.0
# proportional gain of contact constraint
kp_com
=
10.0
# proportional gain of center of mass task
kp_posture
=
10.0
# proportional gain of joint posture task
dt
=
0.001
# controller time step
PRINT_N
=
500
# print every PRINT_N time steps
DISPLAY_N
=
25
# update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION
=
4000
# number of time steps simulated
filename
=
str
(
os
.
path
.
dirname
(
os
.
path
.
abspath
(
__file__
)))
path
=
filename
+
'/../models/romeo'
urdf
=
path
+
'/urdf/romeo.urdf'
vector
=
se3
.
StdVec_StdString
()
vector
.
extend
(
item
for
item
in
path
)
robot
=
tsid
.
RobotWrapper
(
urdf
,
vector
,
se3
.
JointModelFreeFlyer
(),
False
)
srdf
=
path
+
'/srdf/romeo_collision.srdf'
# for gepetto viewer
robot_display
=
se3
.
RobotWrapper
.
BuildFromURDF
(
urdf
,
[
path
,
],
se3
.
JointModelFreeFlyer
())
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
)
cl
=
gepetto
.
corbaserver
.
Client
()
gui
=
cl
.
gui
robot_display
.
initDisplay
(
loadModel
=
True
)
q
=
se3
.
getNeutralConfiguration
(
robot
.
model
(),
srdf
,
False
)
q
[
2
]
+=
0.84
v
=
np
.
matrix
(
np
.
zeros
(
robot
.
nv
)).
T
robot_display
.
displayCollisions
(
False
)
robot_display
.
displayVisuals
(
True
)
robot_display
.
display
(
q
)
assert
robot
.
model
().
existFrame
(
rf_frame_name
)
assert
robot
.
model
().
existFrame
(
lf_frame_name
)
t
=
0.0
# time
invdyn
=
tsid
.
InverseDynamicsFormulationAccForce
(
"tsid"
,
robot
,
False
)
invdyn
.
computeProblemData
(
t
,
q
,
v
)
data
=
invdyn
.
data
()
contact_Point
=
np
.
matrix
(
np
.
ones
((
3
,
4
))
*
lz
)
contact_Point
[
0
,
:]
=
[
-
lxn
,
-
lxn
,
lxp
,
lxp
]
contact_Point
[
1
,
:]
=
[
-
lyn
,
lyp
,
-
lyn
,
lyp
]
contactRF
=
tsid
.
Contact6d
(
"contact_rfoot"
,
robot
,
rf_frame_name
,
contact_Point
,
contactNormal
,
mu
,
fMin
,
fMax
)
contactRF
.
setKp
(
kp_contact
*
matlib
.
ones
(
6
).
T
)
contactRF
.
setKd
(
2.0
*
np
.
sqrt
(
kp_contact
)
*
matlib
.
ones
(
6
).
T
)
H_rf_ref
=
robot
.
position
(
data
,
robot
.
model
().
getJointId
(
rf_frame_name
))
contactRF
.
setReference
(
H_rf_ref
)
invdyn
.
addRigidContact
(
contactRF
,
w_forceRef
)
contactLF
=
tsid
.
Contact6d
(
"contact_lfoot"
,
robot
,
lf_frame_name
,
contact_Point
,
contactNormal
,
mu
,
fMin
,
fMax
)
contactLF
.
setKp
(
kp_contact
*
matlib
.
ones
(
6
).
T
)
contactLF
.
setKd
(
2.0
*
np
.
sqrt
(
kp_contact
)
*
matlib
.
ones
(
6
).
T
)
H_lf_ref
=
robot
.
position
(
data
,
robot
.
model
().
getJointId
(
lf_frame_name
))
contactLF
.
setReference
(
H_lf_ref
)
invdyn
.
addRigidContact
(
contactLF
,
w_forceRef
)
comTask
=
tsid
.
TaskComEquality
(
"task-com"
,
robot
)
comTask
.
setKp
(
kp_com
*
matlib
.
ones
(
3
).
T
)
comTask
.
setKd
(
2.0
*
np
.
sqrt
(
kp_com
)
*
matlib
.
ones
(
3
).
T
)
invdyn
.
addMotionTask
(
comTask
,
w_com
,
1
,
0.0
)
postureTask
=
tsid
.
TaskJointPosture
(
"task-posture"
,
robot
)
postureTask
.
setKp
(
kp_posture
*
matlib
.
ones
(
robot
.
nv
-
6
).
T
)
postureTask
.
setKd
(
2.0
*
np
.
sqrt
(
kp_posture
)
*
matlib
.
ones
(
robot
.
nv
-
6
).
T
)
invdyn
.
addMotionTask
(
postureTask
,
w_posture
,
1
,
0.0
)
com_ref
=
robot
.
com
(
data
)
trajCom
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_com"
,
com_ref
)
sampleCom
=
trajCom
.
computeNext
()
q_ref
=
q
[
7
:]
trajPosture
=
tsid
.
TrajectoryEuclidianConstant
(
"traj_joint"
,
q_ref
)
solver
=
tsid
.
SolverHQuadProgFast
(
"qp solver"
)
solver
.
resize
(
invdyn
.
nVar
,
invdyn
.
nEq
,
invdyn
.
nIn
)
com_pos
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_vel
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_acc
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_pos_ref
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_vel_ref
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_acc_ref
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
com_acc_des
=
matlib
.
empty
((
3
,
N_SIMULATION
))
*
nan
# acc_des = acc_ref - Kp*pos_err - Kd*vel_err
offset
=
robot
.
com
(
data
)
import
romeo_conf
as
conf
from
tsid_biped
import
TsidBiped
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
" Test Task Space Inverse Dynamics "
.
center
(
conf
.
LINE_WIDTH
,
'#'
)
print
""
.
center
(
conf
.
LINE_WIDTH
,
'#'
),
'
\n
'
tsid
=
TsidBiped
(
conf
)
N
=
conf
.
N_SIMULATION
com_pos
=
matlib
.
empty
((
3
,
N
))
*
nan
com_vel
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc
=
matlib
.
empty
((
3
,
N
))
*
nan
com_pos_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
com_vel_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc_ref
=
matlib
.
empty
((
3
,
N
))
*
nan
com_acc_des
=
matlib
.
empty
((
3
,
N
))
*
nan
# acc_des = acc_ref - Kp*pos_err - Kd*vel_err
offset
=
tsid
.
robot
.
com
(
tsid
.
formulation
.
data
())
amp
=
np
.
matrix
([
0.0
,
0.05
,
0.0
]).
T
two_pi_f
=
2
*
np
.
pi
*
np
.
matrix
([
0.0
,
0.5
,
0.0
]).
T
two_pi_f_amp
=
np
.
multiply
(
two_pi_f
,
amp
)
two_pi_f_squared_amp
=
np
.
multiply
(
two_pi_f
,
two_pi_f_amp
)
for
i
in
range
(
0
,
N_SIMULATION
):
sampleCom
=
tsid
.
trajCom
.
computeNext
()
samplePosture
=
tsid
.
trajPosture
.
computeNext
()
t
=
0.0
q
,
v
=
tsid
.
q
,
tsid
.
v
for
i
in
range
(
0
,
N
):
time_start
=
time
.
time
()
sampleCom
.
pos
(
offset
+
np
.
multiply
(
amp
,
matlib
.
sin
(
two_pi_f
*
t
)))
sampleCom
.
vel
(
np
.
multiply
(
two_pi_f_amp
,
matlib
.
cos
(
two_pi_f
*
t
)))
sampleCom
.
acc
(
np
.
multiply
(
two_pi_f_squared_amp
,
-
matlib
.
sin
(
two_pi_f
*
t
)))
comTask
.
setReference
(
sampleCom
)
samplePosture
=
trajPosture
.
computeNext
()
postureTask
.
setReference
(
samplePosture
)
tsid
.
comTask
.
setReference
(
sampleCom
)
tsid
.
postureTask
.
setReference
(
samplePosture
)
HQPData
=
invdy
n
.
computeProblemData
(
t
,
q
,
v
)
HQPData
=
tsid
.
formulatio
n
.
computeProblemData
(
t
,
q
,
v
)
# if i == 0: HQPData.print_all()
sol
=
solver
.
solve
(
HQPData
)
sol
=
tsid
.
solver
.
solve
(
HQPData
)
if
(
sol
.
status
!=
0
):
print
"QP problem could not be solved! Error code:"
,
sol
.
status
break
tau
=
invdy
n
.
getActuatorForces
(
sol
)
dv
=
invdy
n
.
getAccelerations
(
sol
)
tau
=
tsid
.
formulatio
n
.
getActuatorForces
(
sol
)
dv
=
tsid
.
formulatio
n
.
getAccelerations
(
sol
)
com_pos
[:,
i
]
=
robot
.
com
(
invdy
n
.
data
())
com_vel
[:,
i
]
=
robot
.
com_vel
(
invdy
n
.
data
())
com_acc
[:,
i
]
=
comTask
.
getAcceleration
(
dv
)
com_pos
[:,
i
]
=
tsid
.
robot
.
com
(
tsid
.
formulatio
n
.
data
())
com_vel
[:,
i
]
=
tsid
.
robot
.
com_vel
(
tsid
.
formulatio
n
.
data
())
com_acc
[:,
i
]
=
tsid
.
comTask
.
getAcceleration
(
dv
)
com_pos_ref
[:,
i
]
=
sampleCom
.
pos
()
com_vel_ref
[:,
i
]
=
sampleCom
.
vel
()
com_acc_ref
[:,
i
]
=
sampleCom
.
acc
()
com_acc_des
[:,
i
]
=
comTask
.
getDesiredAcceleration
com_acc_des
[:,
i
]
=
tsid
.
comTask
.
getDesiredAcceleration
if
i
%
PRINT_N
==
0
:
if
i
%
conf
.
PRINT_N
==
0
:
print
"Time %.3f"
%
(
t
)
if
invdy
n
.
checkContact
(
contactRF
.
name
,
sol
):
f
=
invdy
n
.
getContactForce
(
contactRF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
contactRF
.
name
.
ljust
(
20
,
'.'
),
contactRF
.
getNormalForce
(
f
))
if
tsid
.
formulatio
n
.
checkContact
(
tsid
.
contactRF
.
name
,
sol
):
f
=
tsid
.
formulatio
n
.
getContactForce
(
tsid
.
contactRF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
tsid
.
contactRF
.
name
.
ljust
(
20
,
'.'
),
tsid
.
contactRF
.
getNormalForce
(
f
))
if
invdy
n
.
checkContact
(
contactLF
.
name
,
sol
):
f
=
invdy
n
.
getContactForce
(
contactLF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
contactLF
.
name
.
ljust
(
20
,
'.'
),
contactLF
.
getNormalForce
(
f
))
if
tsid
.
formulatio
n
.
checkContact
(
tsid
.
contactLF
.
name
,
sol
):
f
=
tsid
.
formulatio
n
.
getContactForce
(
tsid
.
contactLF
.
name
,
sol
)
print
"
\t
normal force %s: %.1f"
%
(
tsid
.
contactLF
.
name
.
ljust
(
20
,
'.'
),
tsid
.
contactLF
.
getNormalForce
(
f
))
print
"
\t
tracking err %s: %.3f"
%
(
comTask
.
name
.
ljust
(
20
,
'.'
),
norm
(
comTask
.
position_error
,
2
))
print
"
\t
tracking err %s: %.3f"
%
(
tsid
.
comTask
.
name
.
ljust
(
20
,
'.'
),
norm
(
tsid
.
comTask
.
position_error
,
2
))
print
"
\t
||v||: %.3f
\t
||dv||: %.3f"
%
(
norm
(
v
,
2
),
norm
(
dv
))
v_mean
=
v
+
0.5
*
dt
*
dv
v
+=
dt
*
dv
q
=
se3
.
integrate
(
robot
.
model
(),
q
,
dt
*
v_mean
)
t
+=
dt
q
,
v
=
tsid
.
integrate_dv
(
q
,
v
,
dv
,
conf
.
dt
)
t
+=
conf
.
dt
if
i
%
DISPLAY_N
==
0
:
robot_display
.
display
(
q
)
if
i
%
conf
.
DISPLAY_N
==
0
:
tsid
.
robot_display
.
display
(
q
)
time_spent
=
time
.
time
()
-
time_start
if
(
time_spent
<
dt
):
time
.
sleep
(
dt
-
time_spent
)
if
(
time_spent
<
conf
.
dt
):
time
.
sleep
(
conf
.
dt
-
time_spent
)
# PLOT STUFF
time
=
np
.
arange
(
0.0
,
N
_SIMULATION
*
dt
,
dt
)
time
=
np
.
arange
(
0.0
,
N
*
conf
.
dt
,
conf
.
dt
)
(
f
,
ax
)
=
plut
.
create_empty_figure
(
3
,
1
)
for
i
in
range
(
3
):
...
...
exercizes/ex_3_biped_balance_with_gui.py
0 → 100644
View file @
e8642f31
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 17 22:31:22 2019
@author: student
"""
import
numpy
as
np
import
numpy.matlib
as
matlib
import
pinocchio
as
pin
import
Tkinter
as
tk
from
Tkinter
import
Scale
,
Button
,
Frame
,
Entry
,
Label
,
Tk
,
mainloop
,
HORIZONTAL
import
threading
class
Scale3d
:
def
__init__
(
self
,
master
,
name
,
from_
,
to
,
tickinterval
,
length
,
orient
,
command
):
self
.
s
=
3
*
[
None
]
axes
=
[
'X'
,
'Y'
,
'Z'
]
for
i
in
range
(
3
):
self
.
s
[
i
]
=
Scale
(
master
,
label
=
name
+
' '
+
axes
[
i
],
from_
=
from_
[
i
],
to
=
to
[
i
],
tickinterval
=
tickinterval
[
i
],
orient
=
orient
[
i
],
length
=
length
[
i
],
command
=
command
)
self
.
s
[
i
].
pack
()
separator
=
Frame
(
height
=
2
,
bd
=
1
,
relief
=
tk
.
SUNKEN
)
separator
.
pack
(
fill
=
tk
.
X
,
padx
=
5
,
pady
=
5
)
def
get
(
self
):
return
self
.
s
[
0
].
get
(),
self
.
s
[
1
].
get
(),
self
.
s
[
2
].
get
()
class
Entry3d
:
def
__init__
(
self
,
master
,
name
):
self
.
s
=
3
*
[
None
]
axes
=
[
'X'
,
'Y'
,
'Z'
]
for
i
in
range
(
3
):
Label
(
master
,
text
=
name
+
" "
+
axes
[
i
]).
pack
()
#side=tk.TOP)
self
.
s
[
i
]
=
Entry
(
master
,
width
=
5
)
self
.
s
[
i
].
pack
()
#side=tk.BOTTOM)
separator
=
Frame
(
height
=
1
,
bd
=
1
,
relief
=
tk
.
SUNKEN
)
separator
.
pack
(
fill
=
tk
.
X
,
padx
=
2
,
pady
=
2
)
#, side=tk.BOTTOM)
def
get
(
self
):
try
:
return
[
float
(
self
.
s
[
i
].
get
())
for
i
in
range
(
3
)]
except
:
print
"could not convert string to float"
,
[
self
.
s
[
i
].
get
()
for
i
in
range
(
3
)]
return
3
*
[
0.0
]
scale_com
,
scale_RF
,
scale_LF
=
None
,
None
,
None
button_contact_RF
,
button_contact_LF
=
None
,
None
push_robot_active
,
push_robot_com_vel
,
com_vel_entry
=
False
,
3
*
[
0.0
],
None
def
update_com_ref_scale
(
value
):
x
,
y
,
z
=
scale_com
.
get
()
tsid
.
trajCom
.
setReference
(
com_0
+
np
.
matrix
([
1e-2
*
x
,
1e-2
*
y
,
1e-2
*
z
]).
T
)
def
update_RF_ref_scale
(
value
):
x
,
y
,
z
=
scale_RF
.
get
()
H_rf_ref
=
H_rf_0
.
copy
()
H_rf_ref
.
translation
+=
+
np
.
matrix
([
1e-2
*
x
,
1e-2
*
y
,
1e-2
*
z
]).
T
tsid
.
trajRF
.
setReference
(
H_rf_ref
)
def
update_LF_ref_scale
(
value
):
x
,
y
,
z
=
scale_LF
.
get
()
H_lf_ref
=
H_lf_0
.
copy
()
H_lf_ref
.
translation
+=
+
np
.
matrix
([
1e-2
*
x
,
1e-2
*
y
,
1e-2
*
z
]).
T
tsid
.
trajLF
.
setReference
(
H_lf_ref
)
def
switch_contact_RF
():
if
(
tsid
.
contact_RF_active
):
tsid
.
remove_contact_RF
()
button_contact_RF
.
config
(
text
=
'Make contact right foot'
)
else
:
tsid
.
add_contact_RF
()
button_contact_RF
.
config
(
text
=
'Break contact right foot'
)
def
switch_contact_LF
():
if
(
tsid
.
contact_LF_active
):
tsid
.
remove_contact_LF
()
button_contact_LF
.
config
(
text
=
'Make contact left foot'
)
else
:
tsid
.
add_contact_LF
()
button_contact_LF
.
config
(
text
=
'Break contact left foot'
)
def
toggle_wireframe_mode
():
tsid
.
gui
.
setWireFrameMode
(
'world'
,
'WIREFRAME'
)
def
push_robot
():
global
push_robot_com_vel
,
push_robot_active
push_robot_com_vel
=
com_vel_entry
.
get
()
push_robot_active
=
True
def
create_gui
():
"""thread worker function"""
global
scale_com
,
scale_RF
,
scale_LF
,
button_contact_RF
,
button_contact_LF
,
com_vel_entry
master
=
Tk
(
className
=
'TSID GUI'
)
scale_com
=
Scale3d
(
master
,
'CoM'
,
[
-
10
,
-
15
,
-
40
],
[
10
,
15
,
40
],
[
5
,
5
,
10
],
[
200
,
250
,
300
],
3
*
[
HORIZONTAL
],
update_com_ref_scale
)
scale_RF
=
Scale3d
(
master
,
'Right foot'
,
3
*
[
-
30
],
3
*
[
30
],
3
*
[
10
],
3
*
[
300
],
3
*
[
HORIZONTAL
],
update_RF_ref_scale
)
scale_LF
=
Scale3d
(
master
,
'Left foot'
,
3
*
[
-
30
],
3
*
[
30
],
3
*
[
10
],
3
*
[
300
],
3
*
[
HORIZONTAL
],
update_LF_ref_scale
)
button_contact_RF
=
Button
(
master
,
text
=
'Break contact right foot'
,
command
=
switch_contact_RF
)
button_contact_RF
.
pack
(
side
=
tk
.
LEFT
)
button_contact_LF
=
Button
(
master
,
text
=
'Break contact left foot'
,
command
=
switch_contact_LF
)
button_contact_LF
.
pack
(
side
=
tk
.
LEFT
)
Button
(
master
,
text
=
'Toggle wireframe'
,
command
=
toggle_wireframe_mode
).
pack
(
side
=
tk
.
LEFT
)
# Frame(height=2, bd=1, relief=tk.SUNKEN).pack(fill=tk.X, padx=5, pady=5)
Button
(
master
,
text
=
'Push robot CoM'
,
command
=
push_robot
).
pack
()