Melya's updates

hqp/abstract_solver.py

@@ -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

@@ -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

 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

@@ -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):