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
curves
Commits
727b52ea
Commit
727b52ea
authored
Nov 30, 2016
by
Steve Tonneau
Browse files
ongoing: instead of inverting whole matrix solving problems one by one
parent
e9882b85
Changes
1
Hide whitespace changes
Inline
Side-by-side
include/spline/exact_cubic_vel_acc_cons.h
View file @
727b52ea
...
...
@@ -68,8 +68,8 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
point_t
init_acc
;
point_t
end_vel
;
point_t
end_acc
;
point_t
init_normal
;
point_t
end_normal
;
point_t
init_normal
;
//TODO
point_t
end_normal
;
//TODO
};
/* Constructors - destructors */
...
...
@@ -84,7 +84,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
///\brief Destructor
~
cubic_zero_vel
(){}
private:
/*
private:
MatrixX setVelConstraintsAndComputeB(const spline_constraints& constraints,
const Eigen::Ref<MatrixX> x,
Eigen::Ref<MatrixX> h1, Eigen::Ref<MatrixX> h2) const
...
...
@@ -102,6 +102,41 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
MatrixX h1inv = h1.inverse();
b = h1inv * (h2 *x + cons); //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
return b;
}*/
private:
template
<
typename
In
>
void
compute_one_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
{
const
point_t
&
a0
=
wayPointsBegin
->
second
,
a1
=
wayPointsNext
->
second
;
const
point_t
&
b0
=
constraints
.
init_vel
,
c0
=
constraints
.
init_acc
;
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
-
wayPointsNext
->
first
;
const
num_t
dt
=
end_t
-
init_t
,
dt_2
=
dt
*
dt
,
dt_3
=
dt_2
*
dt
;
const
point_t
d0
=
(
a1
-
a0
-
b0
*
dt
-
c0
*
dt_2
)
/
dt_3
;
subSplines
.
push_back
(
create_cubic
<
Time
,
Numeric
,
Dim
,
Safe
,
Point
,
T_Point
>
(
a0
,
b0
,
c0
,
d0
,
init_t
,
end_t
));
constraints
.
init_vel
=
subSplines
.
back
().
derivate
(
end_t
,
1
);
constraints
.
init_acc
=
subSplines
.
back
().
derivate
(
end_t
,
2
);
}
template
<
typename
In
>
void
compute_end_spline
(
In
wayPointsBegin
,
In
wayPointsNext
,
spline_constraints
&
constraints
,
t_spline_t
&
subSplines
)
{
const
point_t
&
a0
=
wayPointsBegin
->
second
,
a1
=
wayPointsNext
->
second
;
const
point_t
&
b0
=
constraints
.
init_vel
,
b1
=
constraints
.
end_vel
,
c0
=
constraints
.
init_acc
,
c1
=
constraints
.
end_acc
;
const
num_t
&
init_t
=
wayPointsBegin
->
first
,
end_t
=
-
wayPointsNext
->
first
;
const
num_t
dt
=
end_t
-
init_t
,
dt_2
=
dt
*
dt
,
dt_3
=
dt_2
*
dt
;
//solving a system of four linear eq with 4 unknows: d0, e0
const
num_t
alpha_0
=
a1
-
a0
+
b0
*
dt
+
c0
*
dt_2
;
const
num_t
alpha_1
=
b1
-
b0
+
b0
*
dt
+
c0
*
dt_2
;
/*subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
(a0,b0,c0,d0,e0, init_t, end_t));
constraints.init_vel = subSplines.back().derivate(end_t, 1);
constraints.init_acc = subSplines.back().derivate(end_t, 2);*/
//solving
}
template
<
typename
In
>
...
...
@@ -112,9 +147,20 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
{
throw
;
// TODO
}
t_spline_t
subSplines
;
subSplines
.
reserve
(
size
);
t_spline_t
subSplines
;
subSplines
.
reserve
(
size
-
1
);
spline_constraints
cons
=
constraints
;
// refer to the paper to understand all this.
In
it
(
wayPointsBegin
),
next
(
wayPointsBegin
),
end
(
wayPointsEnd
-
1
);
++
next
;
for
(
std
::
size_t
i
(
0
);
next
!=
end
;
++
next
,
++
it
,
++
i
)
{
compute_one_spline
<
In
>
(
it
,
next
,
cons
,
subSplines
);
}
compute_end_spline
<
In
>
(
it
,
next
,
cons
,
subSplines
);
return
subSplines
;
// then solving last step
/*// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
...
...
@@ -173,7 +219,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
}
//subSplines.push_back(
// create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
return
subSplines
;
return subSplines;
*/
}
...
...
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