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
Jason Chemin
hpp-rbprm-corba
Commits
50a3c3c1
Commit
50a3c3c1
authored
Oct 07, 2016
by
Steve Tonneau
Browse files
working on effector trajhectories
parent
49f5ca89
Changes
4
Hide whitespace changes
Inline
Side-by-side
script/scenarios/ground_crouch_hyq_interp.py
View file @
50a3c3c1
import
matplotlib
#~ matplotlib.use('Agg')
import
matplotlib.pyplot
as
plt
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
...
...
@@ -147,32 +150,11 @@ from numpy import matlib
#~ for i in range(6,25):
#~ act(i, 60, optim_effectors = True)
#~ pid = act(8,0,optim_effectors = False)
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
res
pid
=
res
[
0
]
-
2
dt
=
0.001
#~ qs = [ pp.client.problem.configAtParam (pid, i*dt) for i in range(600,700)]
#~ cs = []
#~ res = zeros((3, 100))
#~ for i in range(len(qs)):
#~ q = qs[i]
#~ fullBody.setCurrentConfig(q[:-1])
#~ c = fullBody.getCenterOfMass()
#~ cs += [c[0]]
#~ res[:,i] = c
#~ for i in range(8,9): act(i,0,optim_effectors=False, draw=False)
#~ ddc = [2 *(cs[i-1] - c[i]) for i in range(1, len(c))]
#~ ddcx = [ddci[0] for ddci in ddc]
#~ res = zeros((3, 100))
for
i
in
range
(
7
,
8
):
act
(
i
,
0
,
use_window
=
0
,
verbose
=
True
,
optim_effectors
=
False
,
draw
=
False
)
#~ for i,ci in enumerate(c):
#~ res[:,i] = ci
#~ m = matrix(res)
#~ pid = act(8,0,optim_effectors = False)
from
derivative_filters
import
*
...
...
@@ -188,6 +170,7 @@ import plot_utils
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
trajec_mil
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
trajec
from
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
import
coms
#~ plt.plot(x[0,:].A.squeeze())
#~ plt.plot(dx[0,:].A.squeeze())
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @
50a3c3c1
import
matplotlib
#~ matplotlib.use('Agg')
import
matplotlib.pyplot
as
plt
from
mpl_toolkits.mplot3d
import
Axes3D
from
cwc
import
cone_optimization
from
obj_to_constraints
import
ineq_from_file
,
rotate_inequalities
import
numpy
as
np
...
...
@@ -155,9 +159,6 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
def
draw_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
=
False
,
mu
=
1
,
dt
=
0.2
,
phase_dt
=
[
0.4
,
1
],
reduce_ineq
=
True
,
verbose
=
False
,
limbsCOMConstraints
=
None
,
use_window
=
0
):
var_final
,
params
,
elapsed
=
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
False
,
use_window
=
use_window
)
p
,
N
=
fullBody
.
computeContactPoints
(
state_id
)
from
mpl_toolkits.mplot3d
import
Axes3D
import
matplotlib.pyplot
as
plt
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
'3d'
)
n
=
100
...
...
@@ -179,14 +180,16 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
set_ylabel
(
'Y Label'
)
ax
.
set_zlabel
(
'Z Label'
)
plt
.
show
()
#~ plt.show()
plt
.
savefig
(
'/tmp/c'
+
str
(
state_id
)
+
'.png'
)
print
"plotting speed "
print
"end target "
,
params
[
'x_end'
]
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
)
if
(
use_window
>
0
):
points
=
var_final
[
'dc_old'
]
#~ points = var_final['dc_old']
points
=
var_final
[
'dc'
]
else
:
points
=
var_final
[
'dc'
]
...
...
@@ -196,13 +199,15 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
scatter
(
xs
,
ys
,
c
=
'b'
)
plt
.
show
()
#~ plt.show()
plt
.
savefig
(
'/tmp/dc'
+
str
(
state_id
)
+
'.png'
)
print
"plotting acceleration "
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
)
if
(
use_window
>
0
):
points
=
var_final
[
'ddc_old'
]
#~ points = var_final['ddc_old']
points
=
var_final
[
'ddc'
]
else
:
points
=
var_final
[
'ddc'
]
ys
=
[
norm
(
el
)
*
el
[
0
]
/
abs
(
el
[
0
]
+
_EPS
)
for
el
in
points
]
...
...
@@ -210,7 +215,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
scatter
(
xs
,
ys
,
c
=
'b'
)
plt
.
show
()
#~ plt.show()
plt
.
savefig
(
'/tmp/ddc'
+
str
(
state_id
)
+
'.png'
)
print
"plotting Dl "
fig
=
plt
.
figure
()
...
...
@@ -221,7 +227,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
scatter
(
xs
,
ys
,
c
=
'b'
)
plt
.
show
()
#~ plt.show()
plt
.
savefig
(
'/tmp/dL'
+
str
(
state_id
)
+
'.png'
)
return
var_final
,
params
,
elapsed
def
__cVarPerPhase
(
var
,
dt
,
t
,
final_state
,
addValue
):
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @
50a3c3c1
...
...
@@ -74,11 +74,15 @@ def genPEffperFrame(fullBody, freeEffectorsPerPhase, qs, pp, times, dt_framerate
def
genComPerFrame
(
final_state
,
dt
,
dt_framerate
=
1.
/
1000.
):
num_frames_per_dt
=
int
(
round
(
dt
/
dt_framerate
))
inc
=
1.
/
((
float
)(
num_frames_per_dt
))
c
=
[
array
(
final_state
[
'x_init'
][:
3
])]
+
final_state
[
'c'
]
dc
=
[
array
(
final_state
[
'x_init'
][
3
:])]
+
final_state
[
'dc'
]
ddc
=
final_state
[
'ddc'
]
cs
=
[]
[
cs
.
append
(
c
[
i
]
+
ddt
*
dt
*
dc
[
i
]
+
ddt
*
dt
*
ddt
*
dt
*
0.5
*
ddc
[
i
])
for
ddt
in
range
(
num_frames_per_dt
)
for
i
in
range
(
0
,
len
(
ddc
))]
for
i
in
range
(
0
,
len
(
c
)
-
1
):
for
j
in
range
(
num_frames_per_dt
):
ddt
=
j
*
inc
*
dt
cs
.
append
(
c
[
i
]
+
ddt
*
dc
[
i
]
+
ddt
*
ddt
*
0.5
*
ddc
[
i
])
return
cs
stat_data
=
{
...
...
@@ -164,14 +168,13 @@ trackedEffectors = []):
Ps
,
Ns
,
freeEffectorsPerPhase
=
genPandNperFrame
(
fullBody
,
i
,
limbsCOMConstraints
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
NPeffs
=
genPEffperFrame
(
fullBody
,
freeEffectorsPerPhase
,
new_traj_andrea
,
pp
,
time_per_path
,
frame_rate_andrea
)
com
=
genComPerFrame
(
final_state
,
dt
,
frame_rate_andrea
)
if
(
len
(
trajec
)
>
0
):
new_traj
=
new_traj
[
1
:]
new_traj_andrea
=
new_traj_andrea
[
1
:]
#~ new_contacts = new_contacts[1:]
Ps
=
Ps
[
1
:]
Ns
=
Ns
[
1
:]
com
=
com
[
1
:]
pEffs
=
pEffs
[
1
:]
#~ if(len(trajec) > 0):
#~ new_traj = new_traj[1:]
#~ new_traj_andrea = new_traj_andrea[1:]
#~ Ps = Ps[1:]
#~ Ns = Ns[1:]
#~ com = com[1:]
#~ NPeffs = NPeffs[1:]
trajec
=
trajec
+
new_traj
trajec_mil
+=
new_traj_andrea
#~ global contacts
...
...
src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
View file @
50a3c3c1
...
...
@@ -66,10 +66,8 @@ def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, d
config_size_path
=
len
(
path_player
.
client
.
problem
.
configAtParam
(
path_id
,
0
))
if
(
config_size_path
>
config_size
):
#~ if(i == 1 ):
print
"a traj"
res
+=
follow_trajectory_path
(
robot
,
path_player
,
path_id
,
total_time_per_paths
[
i
],
dt_framerate
)
else
:
print
"a path"
res
+=
linear_interpolate_path
(
robot
,
path_player
,
path_id
,
total_time_per_paths
[
i
],
dt_framerate
)
activeid
+=
1
return
res
...
...
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