Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
P
polympc
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Devakumar Thammisetty
polympc
Commits
6cd9be1c
Commit
6cd9be1c
authored
5 years ago
by
Petr Listov
Browse files
Options
Downloads
Patches
Plain Diff
+ path reset bug fixed;
parent
4f484d79
Branches
master
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/nmpf.hpp
+13
-5
13 additions, 5 deletions
src/nmpf.hpp
with
13 additions
and
5 deletions
src/nmpf.hpp
+
13
−
5
View file @
6cd9be1c
...
...
@@ -130,6 +130,7 @@ private:
bool
WARM_START
;
bool
_initialized
;
bool
scale
;
double
reset_path_after
;
/** TRACE FUNCTIONS */
casadi
::
Function
DynamicsFunc
;
...
...
@@ -209,6 +210,13 @@ nmpf<System, Path, NX, NU, NumSegments, PolyOrder>::nmpf(const double &tf, const
assert
(
1
==
W
.
size2
());
}
reset_path_after
=
2
*
M_PI
;
if
(
mpc_options
.
find
(
"mpc.reset_path_after"
)
!=
mpc_options
.
end
())
{
casadi
::
DM
tmp
=
mpc_options
.
find
(
"mpc.reset_path_after"
)
->
second
;
reset_path_after
=
tmp
.
nonzeros
()[
0
];
}
/** assume unconstrained problem */
LBX
=
-
casadi
::
DM
::
inf
(
nx
+
2
);
UBX
=
casadi
::
DM
::
inf
(
nx
+
2
);
...
...
@@ -391,17 +399,17 @@ void nmpf<System, Path, NX, NU, NumSegments, PolyOrder>::computeControl(const ca
{
int
N
=
NUM_COLLOCATION_POINTS
;
/** rectify virtual state */
/** rectify
/ reset
virtual state */
casadi
::
DM
X0
=
_X0
;
bool
rectify
=
false
;
if
(
X0
(
nx
).
nonzeros
()[
0
]
>
2
*
M_PI
)
if
(
X0
(
nx
).
nonzeros
()[
0
]
>
reset_path_after
)
{
X0
(
nx
)
-=
2
*
M_PI
;
X0
(
nx
)
-=
reset_path_after
;
rectify
=
true
;
}
else
if
(
X0
(
nx
).
nonzeros
()[
0
]
<
-
2
*
M_PI
)
else
if
(
X0
(
nx
).
nonzeros
()[
0
]
<
-
reset_path_after
)
{
X0
(
nx
)
+=
2
*
M_PI
;
X0
(
nx
)
+=
reset_path_after
;
rectify
=
true
;
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment