diff --git a/python/pinocchio_inv_dyn/plot_utils.py b/python/pinocchio_inv_dyn/plot_utils.py index 59c92e6..3b01a56 100644 --- a/python/pinocchio_inv_dyn/plot_utils.py +++ b/python/pinocchio_inv_dyn/plot_utils.py @@ -19,7 +19,7 @@ DEFAULT_FIGURE_FACE_COLOR = 'white' # figure facecolor; 0.75 is scalar gray DEFAULT_LEGEND_FONT_SIZE = DEFAULT_FONT_SIZE; DEFAULT_AXES_LABEL_SIZE = DEFAULT_FONT_SIZE; # fontsize of the x any y labels -DEFAULT_TEXT_USE_TEX = True; +DEFAULT_TEXT_USE_TEX = False; LINE_ALPHA = 0.9; SAVE_FIGURES = False; FILE_EXTENSIONS = ['png']; #,'eps']; @@ -247,4 +247,4 @@ def grayify_cmap(cmap): def saveFigure(title): if(SAVE_FIGURES): for ext in FILE_EXTENSIONS: - plt.gcf().savefig(FIGURE_PATH+title.replace(' ', '_')+'.'+ext, format=ext, dpi=FIGURES_DPI, bbox_inches='tight'); \ No newline at end of file + plt.gcf().savefig(FIGURE_PATH+title.replace(' ', '_')+'.'+ext, format=ext, dpi=FIGURES_DPI, bbox_inches='tight'); diff --git a/python/pinocchio_inv_dyn/simulator.py b/python/pinocchio_inv_dyn/simulator.py index f84a1b5..f63bdb9 100644 --- a/python/pinocchio_inv_dyn/simulator.py +++ b/python/pinocchio_inv_dyn/simulator.py @@ -3,7 +3,7 @@ from numpy.random import random from pinocchio_inv_dyn.robot_wrapper import RobotWrapper import pinocchio as se3 -from staggered_projections import StaggeredProjections +#~ from staggered_projections import StaggeredProjections from constraint_violations import ForceConstraintViolation, PositionConstraintViolation, VelocityConstraintViolation, TorqueConstraintViolation, ConstraintViolationType from first_order_low_pass_filter import FirstOrderLowPassFilter import time @@ -180,7 +180,7 @@ def __init__(self, name, q, v, fMin, mu, dt, mesh_dir, urdfFileName, freeFlyer=T self.reset(0, q, v, dt); - self.LCP = StaggeredProjections(self.nv, self.mu[0], accuracy=EPS, verb=0); + #~ self.LCP = StaggeredProjections(self.nv, self.mu[0], accuracy=EPS, verb=0); if(self.DISPLAY_COM): self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, zeros(3), zeros(3), self.COM_SPHERE_COLOR, 'OFF'); @@ -770,4 +770,4 @@ def updateContactForcesInViewerFromConstraints(self, contact_constraints, f): contact_forces = [ f[ii:ii+3] for ii in contact_size_cum]; return self.updateContactForcesInViewer(contact_names, contact_points, contact_forces); - \ No newline at end of file + diff --git a/python/pinocchio_inv_dyn/sot_utils.py b/python/pinocchio_inv_dyn/sot_utils.py index 9935ce7..01830b6 100644 --- a/python/pinocchio_inv_dyn/sot_utils.py +++ b/python/pinocchio_inv_dyn/sot_utils.py @@ -11,12 +11,13 @@ from pinocchio.rpy import rotate from pinocchio import Quaternion from pinocchio.utils import matrixToRpy, rpyToMatrix -from polytope_conversion_utils import * -from qpoases import PyQProblemB as QProblemB # QP with simple bounds only -from qpoases import PySQProblem as SQProblem -from qpoases import PyOptions as Options -from qpoases import PyPrintLevel as PrintLevel -from qpoases import PyReturnValue +from numpy import array +#~ from polytope_conversion_utils import * +#~ from qpoases import PyQProblemB as QProblemB # QP with simple bounds only +#~ from qpoases import PySQProblem as SQProblem +#~ from qpoases import PyOptions as Options +#~ from qpoases import PyPrintLevel as PrintLevel +#~ from qpoases import PyReturnValue EPS = 1e-6 @@ -186,6 +187,7 @@ def compute6dContactInequalities(contact_points, contact_normals, mu): G_centr4[3:,cg*i:cg*i+cg] = np.dot(crossMatrix(p[i,:]), G4[i,:,:]); ''' convert generators to inequalities ''' + from polytope_conversion_utils import * H = cone_span_to_face(G_centr4); return H; @@ -218,6 +220,7 @@ def computeRectangularContactInequalities(lxp, lxn, lyp, lyn, mu): G_centr4[:3,cg*i:cg*i+cg] = G4; G_centr4[3:,cg*i:cg*i+cg] = np.dot(crossMatrix(p[i,:]), G4); ''' convert generators to inequalities ''' + from polytope_conversion_utils import * H = cone_span_to_face(G_centr4); return H; @@ -269,6 +272,11 @@ def solveHierarchicalLeastSquares(A_list, b_list, damping=1e-4, zero_thr=1e-5): return x; def qpOasesSolverMsg(imode): + from qpoases import PyQProblemB as QProblemB # QP with simple bounds only + from qpoases import PySQProblem as SQProblem + from qpoases import PyOptions as Options + from qpoases import PyPrintLevel as PrintLevel + from qpoases import PyReturnValue if(imode==PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY): return "HOTSTART_STOPPED_INFEASIBILITY"; elif(imode==PyReturnValue.MAX_NWSR_REACHED): @@ -446,4 +454,4 @@ def hrp2_jointId_2_name(jid): if(np.linalg.norm(q-qSot) > 1e-6): print "Error in conversion sot-pino-sot" print q - print qSot \ No newline at end of file + print qSot