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
b74d1b76
Commit
b74d1b76
authored
Apr 22, 2021
by
Guilhem Saurel
Browse files
update to changes in hpp-core
parent
a97a5976
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/rbprmbuilder.impl.cc
View file @
b74d1b76
...
...
@@ -832,9 +832,11 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
rotConstraints
.
push_back
(
true
);
const
pinocchio
::
Frame
effectorFrame
=
fullBody
()
->
device_
->
getFrameByName
(
limb
->
effector_
.
name
());
pinocchio
::
JointPtr_t
effectorJoint
=
limb
->
effector_
.
joint
();
proj
->
add
(
core
::
NumericalC
on
s
tr
aint
::
create
(
constraints
::
Position
::
create
(
const
constraints
::
DifferentiableFuncti
on
P
tr
_t
&
function
=
constraints
::
Position
::
create
(
""
,
fullBody
()
->
device_
,
effectorJoint
,
effectorFrame
.
pinocchio
().
placement
*
globalFrame
,
localFrame
,
posConstraints
)));
posConstraints
);
constraints
::
ComparisonTypes_t
comp
(
function
->
outputDerivativeSize
(),
constraints
::
EqualToZero
);
proj
->
add
(
constraints
::
Implicit
::
create
(
function
,
comp
));
if
(
limb
->
contactType_
==
hpp
::
rbprm
::
_6_DOF
)
{
// rotation matrix around z
value_type
theta
=
2
*
(
value_type
(
rand
())
/
value_type
(
RAND_MAX
)
-
0.5
)
*
M_PI
;
...
...
@@ -843,9 +845,10 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
fcl
::
Matrix3f
rotation
=
r
*
effectorFrame
.
pinocchio
().
placement
.
rotation
().
transpose
();
// * limb->effector_->initialPosition
// ().getRotation();
proj
->
add
(
core
::
NumericalConstraint
::
create
(
constraints
::
Orientation
::
create
(
""
,
fullBody
()
->
device_
,
effectorJoint
,
pinocchio
::
Transform3f
(
rotation
,
fcl
::
Vec3f
::
Zero
()),
rotConstraints
)));
const
constraints
::
DifferentiableFunctionPtr_t
&
orientation_function
=
constraints
::
Orientation
::
create
(
""
,
fullBody
()
->
device_
,
effectorJoint
,
pinocchio
::
Transform3f
(
rotation
,
fcl
::
Vec3f
::
Zero
()),
rotConstraints
);
constraints
::
ComparisonTypes_t
orientation_comp
(
orientation_function
->
outputDerivativeSize
(),
constraints
::
EqualToZero
);
proj
->
add
(
constraints
::
Implicit
::
create
(
orientation_function
,
orientation_comp
));
}
}
shooter
->
shoot
(
config
);
...
...
src/rbprmbuilder.impl.hh
View file @
b74d1b76
...
...
@@ -51,10 +51,10 @@ struct BindShooter {
:
shootLimit_
(
shootLimit
),
displacementLimit_
(
displacementLimit
)
{}
hpp
::
rbprm
::
RbPrmShooterPtr_t
create
(
/*const hpp::pinocchio::DevicePtr_t& robot,*/
const
hpp
::
core
::
Problem
&
problem
)
{
/*const hpp::pinocchio::DevicePtr_t& robot,*/
const
hpp
::
core
::
Problem
ConstPtr_t
&
problem
)
{
affMap_
=
problemSolver_
->
affordanceObjects
;
hpp
::
pinocchio
::
RbPrmDevicePtr_t
robotcast
=
std
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problem
.
robot
());
std
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problem
->
robot
());
if
(
affMap_
.
map
.
empty
())
{
throw
hpp
::
Error
(
"No affordances found. Unable to create shooter object."
);
}
...
...
@@ -62,8 +62,8 @@ struct BindShooter {
hpp
::
rbprm
::
RbPrmShooter
::
create
(
robotcast
,
problemSolver_
->
problem
()
->
collisionObstacles
(),
affMap_
,
romFilter_
,
affFilter_
,
shootLimit_
,
displacementLimit_
);
if
(
!
so3Bounds_
.
empty
())
shooter
->
BoundSO3
(
so3Bounds_
);
shooter
->
sampleExtraDOF
(
problem
.
getParameter
(
"ConfigurationShooter/sampleExtraDOF"
).
boolValue
());
shooter
->
ratioWeighted
(
problem
.
getParameter
(
"RbprmShooter/ratioWeighted"
).
floatValue
());
shooter
->
sampleExtraDOF
(
problem
->
getParameter
(
"ConfigurationShooter/sampleExtraDOF"
).
boolValue
());
shooter
->
ratioWeighted
(
problem
->
getParameter
(
"RbprmShooter/ratioWeighted"
).
floatValue
());
return
shooter
;
}
...
...
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