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
412788bb
Commit
412788bb
authored
Feb 23, 2018
by
Pierre Fernbach
Browse files
[transition test] add a method to retrieve the Bezier curve in python from the path index
parent
44394474
Changes
4
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
412788bb
...
...
@@ -717,6 +717,12 @@ module hpp
///
Throw
an
error
if
there
is
no
trajectory
computed
for
the
given
id
/
name
floatSeqSeqSeq
getEffectorTrajectoryWaypoints
(
in
unsigned
short
pathId
,
in
string
effectorName
)
raises
(
Error
)
;
///
Return
the
waypoints
of
the
bezier
curve
at
given
index
///
\
param
pathId
the
path
index
///
\
return
a
list
of
lenght
degree
+
2
:
the
first
item
is
the
time
of
the
curve
,
the
others
are
the
waypoints
floatSeqSeq
getPathAsBezier
(
in
unsigned
short
pathId
)
raises
(
Error
)
;
boolean
areKinematicsConstraintsVerified
(
in
floatSeq
point
)
raises
(
Error
)
;
boolean
areKinematicsConstraintsVerifiedForState
(
in
unsigned
short
stateFrom
,
in
floatSeq
point
)
raises
(
Error
)
;
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
412788bb
...
...
@@ -1080,6 +1080,15 @@ class FullBody (object):
res
+=
[
curve
]
return
res
## Return the bezier curve corresponding to a given path index
def
getPathAsBezier
(
self
,
pathId
):
l
=
self
.
client
.
rbprm
.
rbprm
.
getPathAsBezier
(
pathId
)
t
=
l
[
0
][
0
]
wps
=
matrix
(
l
[
1
:]).
transpose
()
curve
=
bezier
(
wps
,
t
)
return
curve
## return the contacts variation between two states
# \param stateIdFrom : index of the first state
# \param stateIdTo : index of the second state
...
...
src/rbprmbuilder.impl.cc
View file @
412788bb
...
...
@@ -1589,6 +1589,46 @@ namespace hpp {
}
}
hpp
::
floatSeqSeq
*
RbprmBuilder
::
getPathAsBezier
(
unsigned
short
pathId
)
throw
(
hpp
::
Error
){
try
{
if
(
!
fullBodyLoaded_
)
throw
std
::
runtime_error
(
"No Fullbody loaded"
);
if
(
problemSolver
()
->
paths
().
size
()
<=
pathId
)
throw
std
::
runtime_error
(
"No path at index "
+
boost
::
lexical_cast
<
std
::
string
>
(
pathId
));
PathVectorPtr_t
pathVector
=
problemSolver
()
->
paths
()[
pathId
];
PathPtr_t
path
=
pathVector
->
pathAtRank
(
0
);
// try to cast path as BezierPath :
BezierPathPtr_t
bezierPath
=
boost
::
dynamic_pointer_cast
<
BezierPath
>
(
path
);
if
(
!
bezierPath
)
throw
std
::
runtime_error
(
"Not a bezier path at index "
+
boost
::
lexical_cast
<
std
::
string
>
(
pathId
));
const
bezier_t
::
t_point_t
waypoints
=
bezierPath
->
getWaypoints
();
// build the floatSeqSeq : first value is the time, the others are the waypoints
hpp
::
floatSeqSeq
*
res
;
res
=
new
hpp
::
floatSeqSeq
();
_CORBA_ULong
size
=
(
_CORBA_ULong
)
waypoints
.
size
()
+
1
;
res
->
length
(
size
);
// +1 because the first value is the length (time)
{
// add the time at the first index :
double
*
dofArray
=
hpp
::
floatSeq
::
allocbuf
(
1
);
hpp
::
floatSeq
floats
(
1
,
1
,
dofArray
,
true
);
dofArray
[
0
]
=
bezierPath
->
length
();
(
*
res
)
[(
_CORBA_ULong
)
0
]
=
floats
;
// Always assume the curve start at 0. There isn't any ways to create it otherwise in python
}
// now add the waypoints :
std
::
size_t
i
=
1
;
for
(
bezier_t
::
t_point_t
::
const_iterator
wit
=
waypoints
.
begin
();
wit
!=
waypoints
.
end
();
++
wit
,
++
i
)
{
(
*
res
)
[(
_CORBA_ULong
)
i
]
=
vectorToFloatseq
(
*
wit
);
}
return
res
;
}
catch
(
std
::
runtime_error
&
e
)
{
std
::
cout
<<
"ERROR "
<<
e
.
what
()
<<
std
::
endl
;
throw
Error
(
e
.
what
());
}
}
...
...
src/rbprmbuilder.impl.hh
View file @
412788bb
...
...
@@ -347,6 +347,8 @@ namespace hpp {
virtual
hpp
::
floatSeq
*
computeTargetTransform
(
const
char
*
limbName
,
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
p
,
const
hpp
::
floatSeq
&
n
)
throw
(
hpp
::
Error
);
virtual
Names_t
*
getEffectorsTrajectoriesNames
(
unsigned
short
pathId
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeqSeq
*
getEffectorTrajectoryWaypoints
(
unsigned
short
pathId
,
const
char
*
effectorName
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
getPathAsBezier
(
unsigned
short
pathId
)
throw
(
hpp
::
Error
);
virtual
bool
areKinematicsConstraintsVerified
(
const
hpp
::
floatSeq
&
point
)
throw
(
hpp
::
Error
);
virtual
bool
areKinematicsConstraintsVerifiedForState
(
unsigned
short
stateId
,
const
hpp
::
floatSeq
&
point
)
throw
(
hpp
::
Error
);
...
...
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