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
Guilhem Saurel
HQP
Commits
77c15555
Commit
77c15555
authored
Apr 17, 2019
by
Melya Boukheddimi
Browse files
Melya's updates
parent
2dac689a
Changes
4
Hide whitespace changes
Inline
Side-by-side
hqp/abstract_solver.py
View file @
77c15555
...
...
@@ -3,14 +3,7 @@ from scipy.optimize import fmin_slsqp
from
scipy.optimize
import
approx_fprime
from
scipy.optimize.slsqp
import
approx_jacobian
from
scipy.optimize
import
line_search
from
qpoases
import
PySQProblem
as
SQProblem
from
qpoases
import
PyOptions
as
Options
from
qpoases
import
PyPrintLevel
as
PrintLevel
from
qpoases
import
PyReturnValue
from
qpoases
import
PySolutionAnalysis
as
SolutionAnalysis
from
qpoases
import
PyHessianType
as
HessianType
# 'IDENTITY', 'INDEF', 'POSDEF', 'POSDEF_NULLSPACE', 'SEMIDEF', 'UNKNOWN', 'ZERO'
from
qpoases
import
PySubjectToStatus
as
SubjectToStatus
from
qpoases
import
PyBooleanType
as
BooleanType
import
time
class
AbstractSolver
(
object
):
...
...
@@ -482,4 +475,4 @@ class AbstractSolver (object):
print
"[%s] ERROR Qp oases UNKNOWN_BUG"
%
solver_name
;
# 9
else
:
print
"[%s] ERROR Qp oases %d "
%
(
solver_name
,
imode
);
\ No newline at end of file
hqp/derivative_filters.py
View file @
77c15555
...
...
@@ -42,6 +42,7 @@ def computeSecondOrderPolynomialFitting(x, dt, window_length):
assert
len
(
x
.
shape
)
==
2
,
"x must be a matrix"
assert
window_length
>
2
,
"window length must be at least 3"
assert
window_length
%
2
==
1
,
"window length must be an odd number"
print
(
x
,
dt
,
window_length
)
N
=
x
.
shape
[
0
];
T
=
x
.
shape
[
1
];
w
=
window_length
;
...
...
hqp/solvers.py
View file @
77c15555
import
numpy
as
np
from
wrapper
import
Wrapper
import
scipy
from
abstract_solver
import
AbstractSolver
class
NProjections
():
def
reset
(
self
,
q
,
v
,
dt
):
...
...
@@ -169,31 +169,5 @@ class NProjections():
class
StandardQpSolver
(
AbstractSolver
):
"""
Nonrobust inverse dynamics solver for the problem:
min ||D*x - d||^2
s.t. lbA <= A*x <= ubA
"""
def
__init__
(
self
,
n
,
m_in
,
solver
=
'slsqp'
,
accuracy
=
1e-6
,
maxIter
=
100
,
verb
=
0
):
AbstractSolver
.
__init__
(
self
,
n
,
m_in
,
solver
,
accuracy
,
maxIter
,
verb
);
self
.
name
=
"Classic TSID"
;
def
f_cost
(
self
,
x
):
e
=
np
.
dot
(
self
.
D
,
x
)
-
self
.
d
;
return
0.5
*
np
.
dot
(
e
.
T
,
e
);
def
f_cost_grad
(
self
,
x
):
return
np
.
dot
(
self
.
H
,
x
)
-
self
.
dD
;
def
f_cost_hess
(
self
,
x
):
return
self
.
H
;
def
get_linear_inequality_matrix
(
self
):
return
self
.
A
;
def
get_linear_inequality_vectors
(
self
):
return
(
self
.
lbA
,
self
.
ubA
);
hqp/wrapper.py
View file @
77c15555
...
...
@@ -3,6 +3,7 @@ import numpy as np
import
time
import
os
from
models
import
osim_parser
from
ospi
import
model_parser
from
pinocchio.utils
import
XYZQUATToViewerConfiguration
,
zero
,
se3ToXYZQUAT
from
bmtools.algebra
import
quaternion_from_matrix
,
euler_matrix
from
bmtools.filters
import
*
...
...
@@ -12,7 +13,7 @@ class Wrapper():
self
.
name
=
name
if
model_path
is
None
:
model_path
=
'/local/gmaldona/devel/biomechatronics/models/GX.osim'
r
=
osim
_parser
.
Osim2Pinocchio
Model
()
r
=
model
_parser
.
parse
Model
()
self
.
mesh_path
=
mesh_path
self
.
model_path
=
model_path
r
.
parseModel
(
self
.
model_path
,
self
.
mesh_path
)
...
...
@@ -350,7 +351,14 @@ class Wrapper():
costly wrt a dedicated call. Use only with update_geometry for prototyping.
'''
def
frameJacobian
(
self
,
q
,
index
,
update_geometry
=
True
,
local_frame
=
True
):
return
se3
.
frameJacobian
(
self
.
model
,
self
.
data
,
q
,
index
,
local_frame
,
update_geometry
)
if
local_frame
:
return
se3
.
frameJacobian
(
self
.
model
,
self
.
data
,
index
,
q
)
else
:
pass
# idx = get parent joint index
# jointJacobian(q, idx)
#return se3.frameJacobian(self.model, self.data, q, index, local_frame, update_geometry)
def
dof2pinocchio
(
self
,
dof
):
...
...
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