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
f177a0de
Commit
f177a0de
authored
May 24, 2017
by
t steve
Browse files
added state helper class (finally)
parent
b2f3fea7
Changes
6
Expand all
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
f177a0de
...
...
@@ -275,6 +275,19 @@ module hpp
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsAtStateForLimb(in unsigned short stateId, in unsigned short isIntermediate, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state
/// \param stateId iD of the considered state
/// \param isIntermediate whether the intermediate state should be considerred rather than this one
/// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsAtState(in unsigned short stateId, in unsigned short isIntermediate) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
/// to this function. If these conditions are not met an error is raised.
...
...
src/CMakeLists.txt
View file @
f177a0de
...
...
@@ -101,6 +101,7 @@ INSTALL(
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/client.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmbuilder.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmfullbody.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmstate.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm
)
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
f177a0de
This diff is collapsed.
Click to expand it.
src/hpp/corbaserver/rbprm/rbprmstate.py
0 → 100644
View file @
f177a0de
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Steve Tonneau
#
# This file is part of hpp-rbprm-corba.
# hpp-rbprm-corba is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-manipulation-corba is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>.
from
hpp.corbaserver.rbprm
import
Client
as
RbprmClient
from
hpp.corbaserver
import
Client
as
BasicClient
from
numpy
import
array
## Creates a state given an Id pointing to an existing c++ state
#
# A RbprmDevice robot is a set of two robots. One for the
# trunk of the robot, one for the range of motion
class
State
(
object
):
## Constructor
def
__init__
(
self
,
fullBody
,
sId
,
isIntermediate
=
False
):
self
.
cl
=
fullBody
.
client
.
rbprm
.
rbprm
self
.
sId
=
sId
self
.
isIntermediate
=
isIntermediate
self
.
fullBody
=
fullBody
## assert for case where functions can't be used with intermediate state
def
_cni
(
self
):
assert
not
self
.
isIntermediate
,
"method can't be called with intermediate state"
## Get the state configuration
def
q
(
self
):
self
.
_cni
()
return
self
.
cl
.
getConfigAtState
(
self
.
sId
)
## Set the state configuration
# \param q configuration of the robot
# \return whether or not the configuration was successfully set
def
setQ
(
self
,
q
):
self
.
_cni
()
return
self
.
cl
.
setConfigAtState
(
self
.
sId
,
q
)
>
0
# \param state1 current state considered
# \param limb name of the limb for which the request aplies
# \return whether the limb is in contact at this state
def
isLimbInContact
(
self
,
limbname
):
if
(
self
.
isIntermediate
):
return
self
.
cl
.
isLimbInContactIntermediary
(
limbname
,
self
.
sId
)
>
0
else
:
return
self
.
cl
.
isLimbInContact
(
limbname
,
self
.
sId
)
>
0
#
# \param state1 current state considered
# \param limb name of the limb for which the request aplies
# \return all limbs in contact at this state
def
getLimbsInContact
(
self
):
return
[
limbName
for
limbName
in
self
.
fullBody
.
limbNames
if
self
.
isLimbInContact
(
limbName
)]
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def
getEffectorPosition
(
self
,
limbName
):
self
.
_cni
()
return
self
.
cl
.
getEffectorPosition
(
limbName
,
self
.
q
())
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def
getContactPosAndNormals
(
self
):
if
(
self
.
isIntermediate
):
rawdata
=
self
.
cl
.
computeContactPointsAtState
(
self
.
sId
,
1
)
else
:
rawdata
=
self
.
cl
.
computeContactPointsAtState
(
self
.
sId
,
0
)
return
[[
b
[
i
:
i
+
3
]
for
i
in
range
(
0
,
len
(
b
),
6
)]
for
b
in
rawdata
],
[[
b
[
i
+
3
:
i
+
6
]
for
i
in
range
(
0
,
len
(
b
),
6
)]
for
b
in
rawdata
]
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def
getContactPosAndNormalsForLimb
(
self
,
limbName
):
assert
self
.
isLimbInContact
(
limbname
),
"in getContactPosAndNormals: limb "
+
limbName
+
"is not in contact at state"
+
str
(
stateId
)
if
(
self
.
isIntermediate
):
rawdata
=
cl
.
computeContactPointsAtStateForLimb
(
self
.
sId
,
1
)
else
:
rawdata
=
cl
.
computeContactPointsAtStateForLimb
(
self
.
sId
,
0
)
return
[[
b
[
i
:
i
+
3
]
for
i
in
range
(
0
,
len
(
b
),
6
)]
for
b
in
rawdata
],
[[
b
[
i
+
3
:
i
+
6
]
for
i
in
range
(
0
,
len
(
b
),
6
)]
for
b
in
rawdata
]
## Get position of center of mass
def
getCenterOfMass
(
self
):
q0
=
fullBody
.
client
.
basic
.
robot
.
getCurrentConfig
()
fullBody
.
client
.
basic
.
robot
.
setCurrentConfig
(
self
.
q
())
c
=
fullBody
.
client
.
basic
.
robot
.
getComPosition
()
fullBody
.
client
.
basic
.
robot
.
setCurrentConfig
(
q0
)
return
c
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def
getContactCone
(
self
,
friction
):
if
(
self
.
isIntermediate
):
rawdata
=
self
.
cl
.
getContactIntermediateCone
(
self
.
sId
,
friction
)
else
:
rawdata
=
self
.
cl
.
getContactCone
(
self
.
sId
,
friction
)
H_h
=
array
(
rawdata
)
return
H_h
[:,:
-
1
],
H_h
[:,
-
1
]
src/rbprmbuilder.impl.cc
View file @
f177a0de
...
...
@@ -1473,6 +1473,53 @@ namespace hpp {
return
res
;
}
floatSeqSeq
*
RbprmBuilder
::
computeContactPointsAtState
(
unsigned
short
cId
,
unsigned
short
isIntermediate
)
throw
(
hpp
::
Error
)
{
if
(
lastStatesComputed_
.
size
()
<=
cId
+
isIntermediate
)
{
throw
std
::
runtime_error
(
"Unexisting state "
+
std
::
string
(
""
+
(
cId
)));
}
State
state
=
lastStatesComputed_
[
cId
];
if
(
isIntermediate
>
0
)
{
const
State
&
thirdState
=
lastStatesComputed_
[
cId
+
1
];
bool
success
(
false
);
State
interm
=
intermediary
(
state
,
thirdState
,
cId
,
success
);
if
(
success
)
state
=
interm
;
}
std
::
vector
<
std
::
vector
<
fcl
::
Vec3f
>
>
allStates
;
allStates
.
push_back
(
computeRectangleContact
(
fullBody_
,
state
));
hpp
::
floatSeqSeq
*
res
;
res
=
new
hpp
::
floatSeqSeq
();
// compute array of contact positions
res
->
length
((
_CORBA_ULong
)
allStates
.
size
());
std
::
size_t
i
=
0
;
for
(
std
::
vector
<
std
::
vector
<
fcl
::
Vec3f
>
>::
const_iterator
cit
=
allStates
.
begin
();
cit
!=
allStates
.
end
();
++
cit
,
++
i
)
{
const
std
::
vector
<
fcl
::
Vec3f
>&
positions
=
*
cit
;
_CORBA_ULong
size
=
(
_CORBA_ULong
)
positions
.
size
()
*
3
;
double
*
dofArray
=
hpp
::
floatSeq
::
allocbuf
(
size
);
hpp
::
floatSeq
floats
(
size
,
size
,
dofArray
,
true
);
//convert the config in dofseq
for
(
std
::
size_t
h
=
0
;
h
<
positions
.
size
();
++
h
)
{
for
(
std
::
size_t
k
=
0
;
k
<
3
;
++
k
)
{
model
::
size_type
j
(
h
*
3
+
k
);
dofArray
[
j
]
=
positions
[
h
][
k
];
}
}
(
*
res
)
[(
_CORBA_ULong
)
i
]
=
floats
;
}
return
res
;
}
floatSeqSeq
*
RbprmBuilder
::
computeContactPointsForLimb
(
unsigned
short
cId
,
const
char
*
limbName
)
throw
(
hpp
::
Error
)
{
if
(
lastStatesComputed_
.
size
()
<=
cId
+
1
)
...
...
@@ -1528,6 +1575,53 @@ namespace hpp {
return
res
;
}
floatSeqSeq
*
RbprmBuilder
::
computeContactPointsAtStateForLimb
(
unsigned
short
cId
,
unsigned
short
isIntermediate
,
const
char
*
limbName
)
throw
(
hpp
::
Error
)
{
if
(
lastStatesComputed_
.
size
()
<=
cId
+
isIntermediate
)
{
throw
std
::
runtime_error
(
"Unexisting state "
+
std
::
string
(
""
+
(
cId
)));
}
std
::
string
limb
(
limbName
);
State
state
=
lastStatesComputed_
[
cId
];
if
(
isIntermediate
>
0
)
{
const
State
&
thirdState
=
lastStatesComputed_
[
cId
+
1
];
bool
success
(
false
);
State
interm
=
intermediary
(
state
,
thirdState
,
cId
,
success
);
if
(
success
)
state
=
interm
;
}
std
::
vector
<
std
::
vector
<
fcl
::
Vec3f
>
>
allStates
;
allStates
.
push_back
(
computeRectangleContact
(
fullBody_
,
state
,
limb
));
hpp
::
floatSeqSeq
*
res
;
res
=
new
hpp
::
floatSeqSeq
();
// compute array of contact positions
res
->
length
((
_CORBA_ULong
)
allStates
.
size
());
std
::
size_t
i
=
0
;
for
(
std
::
vector
<
std
::
vector
<
fcl
::
Vec3f
>
>::
const_iterator
cit
=
allStates
.
begin
();
cit
!=
allStates
.
end
();
++
cit
,
++
i
)
{
const
std
::
vector
<
fcl
::
Vec3f
>&
positions
=
*
cit
;
_CORBA_ULong
size
=
(
_CORBA_ULong
)
positions
.
size
()
*
3
;
double
*
dofArray
=
hpp
::
floatSeq
::
allocbuf
(
size
);
hpp
::
floatSeq
floats
(
size
,
size
,
dofArray
,
true
);
//convert the config in dofseq
for
(
std
::
size_t
h
=
0
;
h
<
positions
.
size
();
++
h
)
{
for
(
std
::
size_t
k
=
0
;
k
<
3
;
++
k
)
{
model
::
size_type
j
(
h
*
3
+
k
);
dofArray
[
j
]
=
positions
[
h
][
k
];
}
}
(
*
res
)
[(
_CORBA_ULong
)
i
]
=
floats
;
}
return
res
;
}
floatSeqSeq
*
RbprmBuilder
::
interpolate
(
double
timestep
,
double
path
,
double
robustnessTreshold
,
unsigned
short
filterStates
)
throw
(
hpp
::
Error
)
{
try
...
...
src/rbprmbuilder.impl.hh
View file @
f177a0de
...
...
@@ -154,7 +154,9 @@ namespace hpp {
virtual
void
setEndState
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
Names_t
&
contactLimbs
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
computeContactForConfig
(
const
hpp
::
floatSeq
&
configuration
,
const
char
*
limbNam
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeContactPoints
(
unsigned
short
cId
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeContactPointsAtState
(
unsigned
short
cId
,
unsigned
short
isIntermediate
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeContactPointsForLimb
(
unsigned
short
cId
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeContactPointsAtStateForLimb
(
unsigned
short
cId
,
unsigned
short
isIntermediate
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
interpolate
(
double
timestep
,
double
path
,
double
robustnessTreshold
,
unsigned
short
filterStates
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
interpolateConfigs
(
const
hpp
::
floatSeqSeq
&
configs
,
double
robustnessTreshold
,
unsigned
short
filterStates
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
getContactCone
(
unsigned
short
stateId
,
double
friction
)
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