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
Humanoid Path Planner
hpp-rbprm-corba
Commits
b54e60d0
Commit
b54e60d0
authored
Nov 14, 2019
by
Pierre Fernbach
Browse files
[tools] surface_from_path : fix issue in mixing of numpy matrix and arrays
parent
5f05f20f
Changes
1
Hide whitespace changes
Inline
Side-by-side
script/tools/surfaces_from_path.py
View file @
b54e60d0
...
...
@@ -8,7 +8,7 @@ import eigenpy
eigenpy
.
switchToNumpyMatrix
()
ROBOT_NAME
=
'talos'
MAX_SURFACE
=
5
.
# if a contact surface is greater than this value, the intersection is used instead of the whole surface
MAX_SURFACE
=
1
.
# if a contact surface is greater than this value, the intersection is used instead of the whole surface
LF
=
0
RF
=
1
...
...
@@ -90,7 +90,7 @@ def getMergedPhases (seqs):
def
computeRootYawAngleBetwwenConfigs
(
q0
,
q1
):
quat0
=
Quaternion
(
q0
[
6
],
q0
[
3
],
q0
[
4
],
q0
[
5
])
quat1
=
Quaternion
(
q1
[
6
],
q1
[
3
],
q1
[
4
],
q1
[
5
])
v_angular
=
log3
(
quat0
.
matrix
().
T
*
quat1
.
matrix
())
v_angular
=
np
.
matrix
(
log3
(
quat0
.
matrix
().
T
*
quat1
.
matrix
())
).
T
#print "q_prev : ",q0
#print "q : ",q1
#print "v_angular = ",v_angular
...
...
@@ -101,7 +101,7 @@ def isYawVariationsInsideBounds(q0,q1,max_yaw = 0.5):
#print "yaw = ",yaw
return
yaw
<
max_yaw
def
getSurfacesFromGuideContinuous
(
rbprmBuilder
,
ps
,
afftool
,
pId
,
viewer
=
None
,
step
=
1.
,
useIntersection
=
False
,
mergeCandidates
=
False
,
max_yaw
=
0.5
):
def
getSurfacesFromGuideContinuous
(
rbprmBuilder
,
ps
,
afftool
,
pId
,
viewer
=
None
,
step
=
1.
,
useIntersection
=
False
,
mergeCandidates
=
False
,
max_yaw
=
0.5
,
max_surface_area
=
MAX_SURFACE
):
pathLength
=
ps
.
pathLength
(
pId
)
#length of the path
discretizationStep
=
0.01
# step at which we check the colliding surfaces
#print "path length = ",pathLength
...
...
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