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
Pierre Fernbach
curves
Commits
5f14887a
Unverified
Commit
5f14887a
authored
Feb 11, 2020
by
Fernbach Pierre
Committed by
GitHub
Feb 11, 2020
Browse files
Merge pull request #29 from wxmerkt/wxm-replace-assert-issue-28
Replace assert with exceptions - addresses #28
parents
e360fb3a
ce872cf6
Changes
10
Hide whitespace changes
Inline
Side-by-side
include/curves/bernstein.h
View file @
5f14887a
...
...
@@ -41,7 +41,9 @@ struct Bern {
~
Bern
()
{}
Numeric
operator
()(
const
Numeric
u
)
const
{
assert
(
u
>=
0.
&&
u
<=
1.
);
if
(
!
(
u
>=
0.
&&
u
<=
1.
))
{
throw
std
::
invalid_argument
(
"u needs to be betwen 0 and 1."
);
}
return
bin_m_i_
*
(
pow
(
u
,
i_
))
*
pow
((
1
-
u
),
m_minus_i
);
}
...
...
include/curves/bezier_curve.h
View file @
5f14887a
...
...
@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
size_
(
std
::
distance
(
PointsBegin
,
PointsEnd
)),
degree_
(
size_
-
1
),
bernstein_
(
curves
::
makeBernstein
<
num_t
>
((
unsigned
int
)
degree_
))
{
assert
(
bernstein_
.
size
()
==
size_
);
if
(
bernstein_
.
size
()
!=
size_
)
{
throw
std
::
invalid_argument
(
"Invalid size of polynomial"
);
}
In
it
(
PointsBegin
);
if
(
Safe
&&
(
size_
<
1
||
T_max_
<=
T_min_
))
{
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
// TODO
throw
std
::
invalid_argument
(
"can't create bezier min bound is higher than max bound"
);
}
for
(;
it
!=
PointsEnd
;
++
it
)
{
control_points_
.
push_back
(
*
it
);
...
...
include/curves/cubic_hermite_spline.h
View file @
5f14887a
...
...
@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
//
const
Time
dt
=
(
t1
-
t0
);
const
Time
alpha
=
(
t
-
t0
)
/
dt
;
assert
(
0.
<=
alpha
&&
alpha
<=
1.
&&
"alpha must be in [0,1]"
);
if
(
!
(
0.
<=
alpha
&&
alpha
<=
1.
))
{
throw
std
::
runtime_error
(
"alpha must be in [0,1]"
);
}
Numeric
h00
,
h10
,
h01
,
h11
;
evalCoeffs
(
alpha
,
h00
,
h10
,
h01
,
h11
,
degree_derivative
);
// std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<"
...
...
include/curves/helpers/effector_spline.h
View file @
5f14887a
...
...
@@ -37,7 +37,9 @@ typedef exact_cubic_t::spline_t spline_t;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint
compute_offset
(
const
Waypoint
&
source
,
const
Point
&
normal
,
const
Numeric
offset
,
const
Time
time_offset
)
{
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
if
(
norm
<
0.
)
{
throw
std
::
runtime_error
(
"Norm of normal is less than 0!"
);
}
return
std
::
make_pair
(
source
.
first
+
time_offset
,
(
source
.
second
+
normal
/
norm
*
offset
));
}
...
...
@@ -46,7 +48,9 @@ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numer
spline_t
make_end_spline
(
const
Point
&
normal
,
const
Point
&
from
,
const
Numeric
offset
,
const
Time
init_time
,
const
Time
time_offset
)
{
Numeric
norm
=
normal
.
norm
();
assert
(
norm
>
0.
);
if
(
norm
<
0.
)
{
throw
std
::
runtime_error
(
"Norm of normal is less than 0!"
);
}
Point
n
=
normal
/
norm
;
Point
d
=
offset
/
(
time_offset
*
time_offset
*
time_offset
)
*
-
n
;
Point
c
=
-
3
*
d
*
time_offset
;
...
...
include/curves/optimization/OptimizeSpline.h
View file @
5f14887a
...
...
@@ -38,7 +38,9 @@ struct SplineOptimizer {
///\brief Initializes optimizer environment.
SplineOptimizer
()
{
MSKrescodee
r_
=
MSK_makeenv
(
&
env_
,
NULL
);
assert
(
r_
==
MSK_RES_OK
);
if
(
r_
!=
MSK_RES_OK
)
{
throw
std
::
runtime_error
(
"Issue initializing MSK_makeenv"
);
}
}
///\brief Destructor.
...
...
include/curves/optimization/details.h
View file @
5f14887a
...
...
@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
// add remaining variables (only if no end_pos constraints)
for
(;
i
<
numControlPoints
;
++
i
)
variables_
.
push_back
(
var_t
::
Zero
(
pDef
.
dim_
));
assert
(
numControlPoints
>
numConstants
);
assert
(
numControlPoints
==
variables_
.
size
());
if
(
numControlPoints
<=
numConstants
)
{
throw
std
::
runtime_error
(
"numControlPoints < numConstants"
);
}
if
(
numControlPoints
!=
variables_
.
size
())
{
throw
std
::
runtime_error
(
"numControlPoints != variables_.size()"
);
}
problemData
.
numControlPoints
=
numControlPoints
;
problemData
.
numVariables
=
numControlPoints
-
numConstants
;
...
...
@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
// compute sub-bezier curves
T_bezier_t
beziers
=
split
<
Point
,
Numeric
>
(
pDef
,
pData
);
assert
(
pDef
.
inequalityMatrices_
.
size
()
==
pDef
.
inequalityVectors_
.
size
());
assert
(
pDef
.
inequalityMatrices_
.
size
()
==
beziers
.
size
());
if
(
pDef
.
inequalityMatrices_
.
size
()
!=
pDef
.
inequalityVectors_
.
size
())
{
throw
std
::
invalid_argument
(
"The sizes of the inequality matrices and vectors do not match."
);
}
if
(
pDef
.
inequalityMatrices_
.
size
()
!=
beziers
.
size
())
{
throw
std
::
invalid_argument
(
"The sizes of the inequality matrices and the bezier degree do not match."
);
}
long
currentRowIdx
=
0
;
typename
problem_definition_t
::
CIT_matrix_x_t
cmit
=
pDef
.
inequalityMatrices_
.
begin
();
...
...
@@ -264,7 +272,7 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
currentRowIdx
+=
cmit
->
rows
();
}
}
assert
(
rows
==
currentRowIdx
);
// we filled all the constraints
assert
(
rows
==
currentRowIdx
);
// we filled all the constraints
- NB: leave assert for Debug tests
}
template
<
typename
Point
,
typename
Numeric
,
typename
In
>
...
...
@@ -273,8 +281,9 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In Po
typedef
Eigen
::
Matrix
<
Numeric
,
Eigen
::
Dynamic
,
1
>
vector_x_t
;
unsigned
int
nPoints1
=
(
unsigned
int
)(
std
::
distance
(
PointsBegin1
,
PointsEnd1
)),
nPoints2
=
(
unsigned
int
)(
std
::
distance
(
PointsBegin2
,
PointsEnd2
));
assert
(
nPoints1
>
0
);
assert
(
nPoints2
>
0
);
if
(
nPoints1
<=
0
||
nPoints2
<=
0
)
{
throw
std
::
runtime_error
(
"This should never happen because an unsigned int cannot go negative without underflowing."
);
}
unsigned
int
deg1
=
nPoints1
-
1
,
deg2
=
nPoints2
-
1
;
unsigned
int
newDeg
=
(
deg1
+
deg2
);
// the integral of the primitive will simply be the last control points of the primitive,
...
...
include/curves/quadratic_variable.h
View file @
5f14887a
...
...
@@ -35,7 +35,9 @@ struct quadratic_variable {
}
quadratic_variable
(
const
matrix_x_t
&
A
,
const
point_t
&
b
,
const
Numeric
c
=
0
)
:
c_
(
c
),
b_
(
b
),
A_
(
A
),
zero
(
false
)
{
assert
(
A
.
cols
()
==
b
.
rows
()
&&
A
.
cols
()
==
A
.
rows
());
if
(
A
.
cols
()
!=
b
.
rows
()
||
A
.
cols
()
!=
A
.
rows
())
{
throw
std
::
invalid_argument
(
"The dimensions of A and b are incorrect."
);
}
}
quadratic_variable
(
const
point_t
&
b
,
const
Numeric
c
=
0
)
...
...
@@ -45,7 +47,9 @@ struct quadratic_variable {
// linear evaluation
Numeric
operator
()(
const
Eigen
::
Ref
<
const
point_t
>&
val
)
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
val
.
transpose
()
*
A
()
*
val
+
b
().
transpose
()
*
val
+
c
();
}
...
...
@@ -98,15 +102,21 @@ struct quadratic_variable {
}
const
matrix_x_t
&
A
()
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
A_
;
}
const
point_t
&
b
()
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
b_
;
}
const
Numeric
c
()
const
{
assert
(
!
isZero
());
if
(
isZero
())
{
throw
std
::
runtime_error
(
"Not initialized! (isZero)"
);
}
return
c_
;
}
bool
isZero
()
const
{
return
zero
;
}
...
...
include/curves/serialization/archive.hpp
View file @
5f14887a
...
...
@@ -59,7 +59,9 @@ struct Serializable {
/// \brief Loads a Derived object from an XML file.
template
<
class
Derived
>
void
loadFromXML
(
const
std
::
string
&
filename
,
const
std
::
string
&
tag_name
)
{
assert
(
!
tag_name
.
empty
());
if
(
tag_name
.
empty
())
{
throw
std
::
invalid_argument
(
"tag_name cannot be empty."
);
}
std
::
ifstream
ifs
(
filename
.
c_str
());
if
(
ifs
)
{
boost
::
archive
::
xml_iarchive
ia
(
ifs
);
...
...
@@ -74,7 +76,9 @@ struct Serializable {
/// \brief Saved a Derived object as an XML file.
template
<
class
Derived
>
void
saveAsXML
(
const
std
::
string
&
filename
,
const
std
::
string
&
tag_name
)
const
{
assert
(
!
tag_name
.
empty
());
if
(
tag_name
.
empty
())
{
throw
std
::
invalid_argument
(
"tag_name cannot be empty."
);
}
std
::
ofstream
ofs
(
filename
.
c_str
());
if
(
ofs
)
{
boost
::
archive
::
xml_oarchive
oa
(
ofs
);
...
...
include/curves/so3_linear.h
View file @
5f14887a
...
...
@@ -256,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
theta
=
PI_value
;
// acos((-1-1)/2)
else
theta
=
acos
((
tr
-
Scalar
(
1
))
/
Scalar
(
2
));
assert
(
theta
==
theta
&&
"theta contains some NaN"
);
// theta != NaN
if
(
!
std
::
isfinite
(
theta
))
{
throw
std
::
runtime_error
(
"theta contains some NaN"
);
}
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
if
(
theta
<
PI_value
-
1e-2
)
{
...
...
python/curves/python_variables.cpp
View file @
5f14887a
...
...
@@ -5,7 +5,9 @@
namespace
curves
{
std
::
vector
<
linear_variable_t
>
matrix3DFromEigenArray
(
const
point_list3_t
&
matrices
,
const
point_list3_t
&
vectors
)
{
assert
(
vectors
.
cols
()
*
3
==
matrices
.
cols
());
if
(
vectors
.
cols
()
*
3
!=
matrices
.
cols
())
{
throw
std
::
invalid_argument
(
"vectors.cols() * 3 != matrices.cols()"
);
}
std
::
vector
<
linear_variable_t
>
res
;
for
(
int
i
=
0
;
i
<
vectors
.
cols
();
++
i
)
{
res
.
push_back
(
linear_variable_t
(
matrices
.
block
<
3
,
3
>
(
0
,
i
*
3
),
vectors
.
col
(
i
)));
...
...
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