### update

parent 99f36c37
hqp/#algebra.py# 0 → 100644
 def quaternion_from_matrix(matrix, isprecise=False): """Return quaternion from rotation matrix. If isprecise is True, the input matrix is assumed to be a precise rotation matrix and a faster algorithm is used. >>> q = quaternion_from_matrix(np.identity(4), True) >>> np.allclose(q, [1, 0, 0, 0]) True >>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1])) >>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0]) True >>> R = rotation_matrix(0.123, (1, 2, 3)) >>> q = quaternion_from_matrix(R, True) >>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) True >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] >>> q = quaternion_from_matrix(R) >>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) True >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] >>> q = quaternion_from_matrix(R) >>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
hqp/#wrapper.py# 0 → 100644
This diff is collapsed.
 ... ... @@ -2,7 +2,7 @@ import numpy as np from numpy.linalg import norm from numpy.random import random from polytope_conversion_utils import cone_span_to_face from wrapper import Wrapper from hqp.wrapper import Wrapper import pinocchio as se3 from pinocchio.utils import zero as zeros from acc_bounds_util_multi_dof import computeAccLimits ... ... @@ -195,9 +195,9 @@ class InvDynFormulation (object): def __init__(self, name, q, v, dt, mesh_dir, urdfFileName, freeFlyer=True): if(freeFlyer): self.r = RobotWrapper(urdfFileName, mesh_dir, root_joint=se3.JointModelFreeFlyer()); self.r = Wrapper(urdfFileName, mesh_dir,'robot',True)# root_joint=se3.JointModelFreeFlyer()); else: self.r = RobotWrapper(urdfFileName, mesh_dir, None); self.r = Wrapper(urdfFileName, mesh_dir, None); self.freeFlyer = freeFlyer; self.nq = self.r.nq; self.nv = self.r.nv; ... ... @@ -404,7 +404,8 @@ class InvDynFormulation (object): self.setPositions(q, updateConstraintReference=False); self.setVelocities(v); self.r.computeAllTerms(q, v); #self.r.computeAllTerms(q, v); self.r.update(q); self.r.framesKinematics(q); self.x_com = self.r.com(q, update_kinematics=False); self.J_com = self.r.Jcom(q, update_kinematics=False); ... ...
 ... ... @@ -11,6 +11,105 @@ class Simulator(object): COM_SPHERE_RADIUS = 0.01 COM_SPHERE_COLOR = (1, 0, 0, 1) ''' name = '' q = None; # current positions v = None; # current velocities LCP = None; # approximate LCP solver using staggered projections USE_LCP_SOLVER = True; DETECT_CONTACT_POINTS = True; #'' True: detect collisions between feet and ground, False: collision is specified by the user '' GROUND_HEIGHT = 0.0; LOW_PASS_FILTER_INPUT_TORQUES = False; ENABLE_FORCE_LIMITS = True; ENABLE_TORQUE_LIMITS = True; ENABLE_JOINT_LIMITS = True; ACCOUNT_FOR_ROTOR_INERTIAS = False; VIEWER_DT = 0.05; DISPLAY_COM = True; DISPLAY_CAPTURE_POINT = True; COM_SPHERE_RADIUS = 0.01; CAPTURE_POINT_SPHERE_RADIUS = 0.01; CONTACT_FORCE_ARROW_RADIUS = 0.01; COM_SPHERE_COLOR = (1, 0, 0, 1); # red, green, blue, alpha CAPTURE_POINT_SPHERE_COLOR = (0, 1, 0, 1); CONTACT_FORCE_ARROW_COLOR = (1, 0, 0, 1); CONTACT_FORCE_ARROW_SCALE = 1e-3; contact_force_arrow_names = []; # list of names of contact force arrows SLIP_VEL_THR = 0.1; SLIP_ANGVEL_THR = 0.2; NORMAL_FORCE_THR = 5.0; JOINT_LIMITS_DQ_THR = 1e-1; #1.0; TORQUE_VIOLATION_THR = 1.0; DQ_MAX = 9.14286; ENABLE_WALL_DRILL_CONTACT = False; wall_x = 0.5; wall_damping = np.array([30, 30, 30, 0.3, 0.3, 0.3]); k=0; # number of contact constraints (i.e. size of contact force vector) na=0; # number of actuated DoFs nq=0; # number of position DoFs nv=0; # number of velocity DoFs r=[]; # robot mu=[]; # friction coefficient (force, moment) fMin = 0; # minimum normal force dt = 0; # time step used to compute joint acceleration bounds qMin = []; # joint lower bounds qMax = []; # joint upper bounds #'' Mapping between y and tau: y = C*tau+c '' C = []; c = []; M = []; # mass matrix Md = []; # rotor inertia h = []; # dynamic drift q = []; dq = []; x_com = []; # com 3d position dx_com = []; # com 3d velocity ddx_com = []; # com 3d acceleration cp = None; # capture point # H_lankle = []; # left ankle homogeneous matrix # H_rankle = []; # right ankle homogeneous matrix J_com = []; # com Jacobian Jc = []; # contact Jacobian Minv = []; # inverse of the mass matrix Jc_Minv = []; # Jc*Minv Lambda_c = []; # task-space mass matrix (Jc*Minv*Jc^T)^-1 Jc_T_pinv = []; # Lambda_c*Jc_Minv Nc_T = []; # I - Jc^T*Jc_T_pinv S_T = []; # selection matrix dJc_v = []; # product of contact Jacobian time derivative and velocity vector: dJc*v candidateContactConstraints = []; rigidContactConstraints = []; # constr_rfoot = None; # constr_lfoot = None; # constr_rhand = None; # f_rh = []; # force right hand # w_rf = []; # wrench right foot # w_lf = []; # wrench left foot #'' debug variables '' x_c = []; # contact points dx_c = []; # contact points velocities x_c_init = []; # initial position of constrained bodies viewer = None; ''' def reset(self, t, q, v, dt, nv): n = nv self.robot.dt = 0.0025 #dt ... ... @@ -31,9 +130,11 @@ class Simulator(object): self.viewer = Viewer(self.robot.name, self.robot) self.updateRobotConfig(self.robot.q0) self.viewer.display(self.robot.q0,robot.name) #self.fMin = 0.001 #self.mu = np.array([ 0.3, 0.1]) #self.nq = self.robot.nq #self.nv = self.robot.nv #self.na = self.nv-6 #self.na = self.nv #if(self.DISPLAYCOM): # self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, mat_zeros(3), mat_zeros(3), self.COM_SPHERE_COLOR, 'OFF') ... ...
 import numpy as np from wrapper import Wrapper import scipy from abstract_solver import AbstractSolver class NProjections(): def reset(self,q,v,dt): ... ... @@ -166,3 +166,34 @@ class NProjections(): Z += self.null(Jstack[k]/robot.M) * Z[k-1] return Z 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);