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-core
Commits
309ad40a
Commit
309ad40a
authored
Mar 29, 2014
by
Florent Lamiraux
Browse files
Extend use of Eigen::Ref to input/output vectors and configurations.
parent
8d8d4855
Changes
22
Hide whitespace changes
Inline
Side-by-side
include/hpp/core/config-projector.hh
View file @
309ad40a
...
...
@@ -62,8 +62,8 @@ namespace hpp {
/// \textbf{q}_{res} = \textbf{q}_{from} + \left(I_n -
/// J^{+}J(\textbf{q}_{from})\right) (\textbf{q}_{to} - \textbf{q}_{from})
/// \f]
void
projectOnKernel
(
const
Configuration_t
&
from
,
const
Configuration_t
&
to
,
Configuration_t
&
result
);
void
projectOnKernel
(
Configuration
In
_t
from
,
Configuration
In
_t
to
,
Configuration
Out
_t
result
);
protected:
/// Constructor
...
...
@@ -81,15 +81,15 @@ namespace hpp {
weak_
=
self
;
}
/// Numerically solve constraint
virtual
bool
impl_compute
(
Configuration_t
&
configuration
);
virtual
bool
impl_compute
(
Configuration
Out
_t
configuration
);
/// Set locked degrees of freedom to their locked values
void
computeLockedDofs
(
Configuration_t
&
configuration
);
void
computeLockedDofs
(
Configuration
Out
_t
configuration
);
private:
virtual
std
::
ostream
&
print
(
std
::
ostream
&
os
)
const
;
virtual
void
addToConstraintSet
(
const
ConstraintSetPtr_t
&
constraintSet
);
void
smallToNormal
(
const
vector_t
&
small
,
vector_t
&
normal
);
void
normalToSmall
(
const
vector_t
&
normal
,
vector_t
&
small
);
void
smallToNormal
(
vector
In
_t
small
,
vector
Out
_t
normal
);
void
normalToSmall
(
vector
In
_t
normal
,
vector
Out
_t
small
);
struct
FunctionValueAndJacobian_t
{
FunctionValueAndJacobian_t
(
DifferentiableFunctionPtr_t
f
,
vector_t
v
,
matrix_t
j
)
:
function
(
f
),
...
...
@@ -102,7 +102,7 @@ namespace hpp {
};
// struct FunctionValueAndJacobian_t
typedef
std
::
vector
<
FunctionValueAndJacobian_t
>
NumericalConstraints_t
;
void
resize
();
void
computeValueAndJacobian
(
const
Configuration_t
&
configuration
);
void
computeValueAndJacobian
(
Configuration
In
_t
configuration
);
virtual
void
addLockedDof
(
const
LockedDofPtr_t
&
lockedDof
);
void
computeIntervals
();
typedef
std
::
list
<
LockedDofPtr_t
>
LockedDofs_t
;
...
...
include/hpp/core/constraint-set.hh
View file @
309ad40a
...
...
@@ -67,7 +67,7 @@ namespace hpp {
Constraint
::
init
(
self
);
weak_
=
self
;
}
virtual
bool
impl_compute
(
Configuration_t
&
configuration
);
virtual
bool
impl_compute
(
Configuration
Out
_t
configuration
);
private:
virtual
void
addToConstraintSet
(
const
ConstraintSetPtr_t
&
constraintSet
)
{
...
...
include/hpp/core/constraint.hh
View file @
309ad40a
...
...
@@ -37,7 +37,7 @@ namespace hpp {
/// Function that applies the constraint
/// \param configuration initial configuration and result
/// \return true if constraint applied successfully, false if failure.
bool
apply
(
Configuration_t
&
configuration
);
bool
apply
(
Configuration
Out
_t
configuration
);
/// Get name of constraint
const
std
::
string
&
name
()
const
{
...
...
@@ -45,7 +45,7 @@ namespace hpp {
}
protected:
/// User defined implementation of the constraint.
virtual
bool
impl_compute
(
Configuration_t
&
configuration
)
=
0
;
virtual
bool
impl_compute
(
Configuration
Out
_t
configuration
)
=
0
;
/// Constructor
Constraint
(
const
std
::
string
&
name
)
:
name_
(
name
)
{
...
...
include/hpp/core/differentiable-function.hh
View file @
309ad40a
...
...
@@ -33,8 +33,8 @@ namespace hpp {
/// Evaluate the function at a given parameter.
///
/// \note parameters should be of the correct size.
void
operator
()
(
vector_t
&
result
,
const
Configuration_t
&
argument
)
const
void
operator
()
(
vector
Out
_t
result
,
Configuration
In
_t
argument
)
const
{
assert
(
result
.
size
()
==
outputSize
());
assert
(
argument
.
size
()
==
inputSize
());
...
...
@@ -44,7 +44,7 @@ namespace hpp {
///
/// \retval jacobian jacobian will be stored in this argument
/// \param argument point at which the jacobian will be computed
void
jacobian
(
matrix_t
&
jacobian
,
const
Configuration_t
&
argument
)
const
void
jacobian
(
matrix_t
&
jacobian
,
Configuration
In
_t
argument
)
const
{
assert
(
argument
.
size
()
==
inputSize
());
assert
(
jacobian
.
rows
()
==
outputSize
());
...
...
@@ -113,11 +113,11 @@ namespace hpp {
}
/// User implementation of function evaluation
virtual
void
impl_compute
(
vector_t
&
result
,
const
Configuration_t
&
argument
)
const
=
0
;
virtual
void
impl_compute
(
vector
Out
_t
result
,
Configuration
In
_t
argument
)
const
=
0
;
virtual
void
impl_jacobian
(
matrix_t
&
jacobian
,
const
Configuration_t
&
arg
)
const
=
0
;
Configuration
In
_t
arg
)
const
=
0
;
private:
/// Dimension of input vector.
...
...
include/hpp/core/distance.hh
View file @
309ad40a
...
...
@@ -28,8 +28,8 @@ namespace hpp {
/// Abstract class for distance between configurations
class
HPP_CORE_DLLAPI
Distance
{
public:
virtual
value_type
operator
()
(
const
Configuration_t
&
q1
,
const
Configuration_t
&
q2
)
virtual
value_type
operator
()
(
Configuration
In
_t
q1
,
Configuration
In
_t
q2
)
{
return
impl_distance
(
q1
,
q2
);
}
...
...
@@ -42,8 +42,8 @@ namespace hpp {
{
}
/// Derived class should implement this function
virtual
value_type
impl_distance
(
const
Configuration_t
&
q1
,
const
Configuration_t
&
q2
)
=
0
;
virtual
value_type
impl_distance
(
Configuration
In
_t
q1
,
Configuration
In
_t
q2
)
=
0
;
};
// class Distance
}
// namespace core
}
// namespace hpp
...
...
include/hpp/core/fwd.hh
View file @
309ad40a
...
...
@@ -69,6 +69,8 @@ namespace hpp {
BasicConfigurationShooterPtr_t
;
typedef
model
::
CollisionObjectPtr_t
CollisionObjectPtr_t
;
typedef
model
::
Configuration_t
Configuration_t
;
typedef
model
::
ConfigurationIn_t
ConfigurationIn_t
;
typedef
model
::
ConfigurationOut_t
ConfigurationOut_t
;
typedef
boost
::
shared_ptr
<
model
::
Configuration_t
>
ConfigurationPtr_t
;
typedef
std
::
vector
<
ConfigurationPtr_t
>
Configurations_t
;
typedef
Configurations_t
::
iterator
ConfigIterator_t
;
...
...
@@ -120,6 +122,8 @@ namespace hpp {
typedef
std
::
vector
<
PathPtr_t
>
Paths_t
;
typedef
std
::
vector
<
PathVectorPtr_t
>
PathVectors_t
;
typedef
model
::
vector_t
vector_t
;
typedef
model
::
vectorIn_t
vectorIn_t
;
typedef
model
::
vectorOut_t
vectorOut_t
;
typedef
boost
::
shared_ptr
<
WeighedDistance
>
WeighedDistancePtr_t
;
}
// namespace core
}
// namespace hpp
...
...
include/hpp/core/locked-dof.hh
View file @
309ad40a
...
...
@@ -62,7 +62,7 @@ namespace hpp {
Constraint
::
init
(
self
);
weak_
=
self
;
}
bool
impl_compute
(
Configuration_t
&
configuration
)
bool
impl_compute
(
Configuration
Out
_t
configuration
)
{
configuration
[
index_
]
=
value_
;
return
true
;
...
...
include/hpp/core/problem.hh
View file @
309ad40a
...
...
@@ -160,7 +160,7 @@ namespace hpp {
private
:
/// Validate configuration and track validation reports.
bool
validateConfig
(
const
DevicePtr_t
&
device
,
const
Configuration_t
&
inConfig
)
const
;
Configuration
In
_t
inConfig
)
const
;
/// Validate initial configuration
void
validateInitConfig
()
const
;
...
...
include/hpp/core/steering-method-straight.hh
View file @
309ad40a
...
...
@@ -36,8 +36,8 @@ namespace hpp {
{
}
/// create a path between two configurations
virtual
PathPtr_t
impl_compute
(
const
Configuration_t
&
q1
,
const
Configuration_t
&
q2
)
const
virtual
PathPtr_t
impl_compute
(
Configuration
In
_t
q1
,
Configuration
In
_t
q2
)
const
{
value_type
length
=
(
*
distance_
)
(
q1
,
q2
);
return
StraightPath
::
create
(
device_
.
lock
(),
q1
,
q2
,
length
,
...
...
include/hpp/core/steering-method.hh
View file @
309ad40a
...
...
@@ -39,8 +39,8 @@ namespace hpp {
{
public:
/// create a path between two configurations
PathPtr_t
operator
()
(
const
Configuration_t
&
q1
,
const
Configuration_t
&
q2
)
const
PathPtr_t
operator
()
(
Configuration
In
_t
q1
,
Configuration
In
_t
q2
)
const
{
return
impl_compute
(
q1
,
q2
);
}
...
...
@@ -62,8 +62,8 @@ namespace hpp {
protected:
/// create a path between two configurations
virtual
PathPtr_t
impl_compute
(
const
Configuration_t
&
q1
,
const
Configuration_t
&
q2
)
const
=
0
;
virtual
PathPtr_t
impl_compute
(
Configuration
In
_t
q1
,
Configuration
In
_t
q2
)
const
=
0
;
private:
/// Set of constraints to apply on the paths produced
ConstraintSetPtr_t
constraints_
;
...
...
include/hpp/core/straight-path.hh
View file @
309ad40a
...
...
@@ -52,8 +52,8 @@ namespace hpp {
/// \param init, end Start and end configurations of the path
/// \param length Distance between the configurations.
static
StraightPathPtr_t
create
(
const
DevicePtr_t
&
device
,
const
Configuration_t
&
init
,
const
Configuration_t
&
end
,
Configuration
In
_t
init
,
Configuration
In
_t
end
,
value_type
length
,
const
ConstraintSetPtr_t
&
constraints
=
ConstraintSetPtr_t
())
...
...
@@ -72,8 +72,8 @@ namespace hpp {
protected:
/// Constructor
StraightPath
(
const
DevicePtr_t
&
robot
,
const
Configuration_t
&
init
,
const
Configuration_t
&
end
,
value_type
length
,
StraightPath
(
const
DevicePtr_t
&
robot
,
Configuration
In
_t
init
,
Configuration
In
_t
end
,
value_type
length
,
const
ConstraintSetPtr_t
&
constraints
);
/// Copy constructor
...
...
include/hpp/core/weighed-distance.hh
View file @
309ad40a
...
...
@@ -44,8 +44,8 @@ namespace hpp {
WeighedDistance
(
const
WeighedDistance
&
distance
);
void
init
(
WeighedDistanceWkPtr
self
);
/// Derived class should implement this function
virtual
value_type
impl_distance
(
const
Configuration_t
&
q1
,
const
Configuration_t
&
q2
);
virtual
value_type
impl_distance
(
Configuration
In
_t
q1
,
Configuration
In
_t
q2
);
private:
DevicePtr_t
robot_
;
std
::
vector
<
value_type
>
weights_
;
...
...
src/config-projector.cc
View file @
309ad40a
...
...
@@ -118,7 +118,7 @@ namespace hpp {
}
void
ConfigProjector
::
computeValueAndJacobian
(
const
Configuration_t
&
configuration
)
(
Configuration
In
_t
configuration
)
{
size_type
row
=
0
,
nbRows
=
0
;
for
(
NumericalConstraints_t
::
iterator
itConstraint
=
...
...
@@ -147,8 +147,8 @@ namespace hpp {
/// Convert vector of non locked degrees of freedom to vector of
/// all degrees of freedom
void
ConfigProjector
::
smallToNormal
(
const
vector_t
&
small
,
vector_t
&
normal
)
void
ConfigProjector
::
smallToNormal
(
vector
In
_t
small
,
vector
Out
_t
normal
)
{
assert
(
small
.
size
()
+
lockedDofs_
.
size
()
==
robot_
->
numberDof
());
assert
(
normal
.
size
()
-
robot_
->
numberDof
()
==
0
);
...
...
@@ -162,8 +162,8 @@ namespace hpp {
}
}
void
ConfigProjector
::
normalToSmall
(
const
vector_t
&
normal
,
vector_t
&
small
)
void
ConfigProjector
::
normalToSmall
(
vector
In
_t
normal
,
vector
Out
_t
small
)
{
assert
(
small
.
size
()
+
lockedDofs_
.
size
()
==
robot_
->
numberDof
());
assert
(
abs
(
normal
.
size
())
-
robot_
->
numberDof
()
==
0
);
...
...
@@ -177,7 +177,7 @@ namespace hpp {
}
}
bool
ConfigProjector
::
impl_compute
(
Configuration_t
&
configuration
)
bool
ConfigProjector
::
impl_compute
(
Configuration
Out
_t
configuration
)
{
hppDout
(
info
,
"before projection: "
<<
configuration
.
transpose
());
computeLockedDofs
(
configuration
);
...
...
@@ -201,7 +201,8 @@ namespace hpp {
Eigen
::
ComputeThinV
);
dqSmall_
=
svd
.
solve
(
value_
);
smallToNormal
(
dqSmall_
,
dq_
);
model
::
integrate
(
robot_
,
configuration
,
-
alpha
*
dq_
,
configuration
);
vector_t
v
(
-
alpha
*
dq_
);
model
::
integrate
(
robot_
,
configuration
,
v
,
configuration
);
// Increase alpha towards alphaMax
alpha
=
alphaMax
-
.8
*
(
alphaMax
-
alpha
);
squareNorm
=
value_
.
squaredNorm
();
...
...
@@ -221,9 +222,9 @@ namespace hpp {
return
true
;
}
void
ConfigProjector
::
projectOnKernel
(
const
Configuration_t
&
from
,
const
Configuration_t
&
to
,
Configuration_t
&
result
)
void
ConfigProjector
::
projectOnKernel
(
Configuration
In
_t
from
,
Configuration
In
_t
to
,
Configuration
Out
_t
result
)
{
computeValueAndJacobian
(
from
);
model
::
difference
(
robot_
,
to
,
from
,
toMinusFrom_
);
...
...
@@ -240,7 +241,7 @@ namespace hpp {
model
::
integrate
(
robot_
,
from
,
projMinusFrom_
,
result
);
}
void
ConfigProjector
::
computeLockedDofs
(
Configuration_t
&
configuration
)
void
ConfigProjector
::
computeLockedDofs
(
Configuration
Out
_t
configuration
)
{
for
(
LockedDofs_t
::
iterator
itLock
=
lockedDofs_
.
begin
();
itLock
!=
lockedDofs_
.
end
();
itLock
++
)
{
...
...
src/constraint-set.cc
View file @
309ad40a
...
...
@@ -37,14 +37,14 @@ namespace hpp {
:
ConfigProjector
(
robot
,
"trivial ConfigProjector"
,
0
,
0
)
{
}
bool
impl_compute
(
Configuration_t
&
configuration
)
bool
impl_compute
(
Configuration
Out
_t
configuration
)
{
computeLockedDofs
(
configuration
);
return
true
;
}
};
// class ConfigProjectorTrivial
bool
ConstraintSet
::
impl_compute
(
Configuration_t
&
configuration
)
bool
ConstraintSet
::
impl_compute
(
Configuration
Out
_t
configuration
)
{
for
(
Constraints_t
::
iterator
itConstraint
=
constraints_
.
begin
();
itConstraint
!=
constraints_
.
end
();
itConstraint
++
)
{
...
...
src/constraint.cc
View file @
309ad40a
...
...
@@ -20,7 +20,7 @@
namespace
hpp
{
namespace
core
{
bool
Constraint
::
apply
(
Configuration_t
&
configuration
)
bool
Constraint
::
apply
(
Configuration
Out
_t
configuration
)
{
return
impl_compute
(
configuration
);
}
...
...
src/diffusing-planner.cc
View file @
309ad40a
...
...
@@ -31,7 +31,7 @@
namespace
hpp
{
namespace
core
{
extern
std
::
string
displayConfig
(
const
Configuration_t
&
q
);
extern
std
::
string
displayConfig
(
Configuration
In
_t
q
);
DiffusingPlannerPtr_t
DiffusingPlanner
::
createWithRoadmap
(
const
Problem
&
problem
,
const
RoadmapPtr_t
&
roadmap
)
...
...
src/node.cc
View file @
309ad40a
...
...
@@ -23,7 +23,7 @@
namespace
hpp
{
namespace
core
{
extern
std
::
string
displayConfig
(
const
Configuration_t
&
q
);
extern
std
::
string
displayConfig
(
Configuration
In
_t
q
);
Node
::
Node
(
const
ConfigurationPtr_t
&
configuration
,
ConnectedComponentPtr_t
connectedComponent
)
:
configuration_
(
configuration
),
...
...
src/problem.cc
View file @
309ad40a
...
...
@@ -28,7 +28,7 @@
namespace
hpp
{
namespace
core
{
extern
std
::
string
displayConfig
(
const
Configuration_t
&
q
);
extern
std
::
string
displayConfig
(
Configuration
In
_t
q
);
// ======================================================================
...
...
@@ -154,7 +154,7 @@ namespace hpp {
}
bool
Problem
::
validateConfig
(
const
DevicePtr_t
&
device
,
const
Configuration_t
&
config
)
const
Configuration
In
_t
config
)
const
{
device
->
currentConfiguration
(
config
);
device
->
computeForwardKinematics
();
...
...
src/random-shortcut.cc
View file @
309ad40a
...
...
@@ -33,14 +33,16 @@ namespace hpp {
// Compute the length of a vector of paths assuming that each element
// is optimal for the given distance.
static
value_type
pathLength
(
const
PathVectorPtr_t
&
path
,
const
DistancePtr_t
&
distance
)
const
DistancePtr_t
&
distance
)
{
value_type
result
=
0
;
for
(
std
::
size_t
i
=
0
;
i
<
path
->
numberPaths
();
++
i
)
{
const
PathPtr_t
&
element
(
path
->
pathAtRank
(
i
));
const
value_type
&
tmin
(
element
->
timeRange
().
first
);
const
value_type
&
tmax
(
element
->
timeRange
().
second
);
result
+=
(
*
distance
)
((
*
element
)
(
tmin
),
(
*
element
)
(
tmax
));
Configuration_t
q1
((
*
element
)
(
tmax
));
Configuration_t
q2
((
*
element
)
(
tmax
));
result
+=
(
*
distance
)
(
q1
,
q2
);
}
return
result
;
}
...
...
src/roadmap.cc
View file @
309ad40a
...
...
@@ -28,7 +28,7 @@
namespace
hpp
{
namespace
core
{
std
::
string
displayConfig
(
const
Configuration_t
&
q
)
std
::
string
displayConfig
(
Configuration
In
_t
q
)
{
std
::
ostringstream
oss
;
for
(
size_type
i
=
0
;
i
<
q
.
size
();
++
i
)
{
...
...
Prev
1
2
Next
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