Commit 77c15555 authored by Melya Boukheddimi's avatar Melya Boukheddimi
Browse files

Melya's updates

parent 2dac689a
......@@ -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):
......
......@@ -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;
......
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);
......@@ -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.Osim2PinocchioModel()
r = model_parser.parseModel()
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):
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment