def createJointAccInequalitiesViability(self): n = self.na B = zeros((2 * n, n)) b = zeros(2 * n) B[:n, :] = np.matlib.identity(n) B[n:, :] = -np.matlib.identity(n) # Take the most conservative limit for each joint dt = max(self.JOINT_POS_PREVIEW, self.JOINT_VEL_PREVIEW) * self.dt self.ddqMax[:, 0] = self.MAX_JOINT_ACC self.ddqStop[:, 0] = self.MAX_MIN_JOINT_ACC (ddqLB, ddqUB) = computeAccLimits( self.q[7:], self.v[6:], self.qMin[7:], self.qMax[7:], self.dqMax[6:], self.ddqMax, dt, False, self.ddqStop, self.IMPOSE_POSITION_BOUNDS, self.IMPOSE_VELOCITY_BOUNDS, self.IMPOSE_VIABILITY_BOUNDS, self.IMPOSE_ACCELERATION_BOUNDS) self.ddqMinFinal = ddqLB self.ddqMaxFinal = ddqUB b[:n] = -self.ddqMinFinal b[n:] = self.ddqMaxFinal if (np.isnan(b).any()): print " ****** ERROR ***** Joint acceleration limits contain nan" return (B, b)
def createContactForceInequalities(self, fMin, mu, contact_points, contact_normals, oRi): if (contact_points.shape[1] > 1): B = -1 * compute6dContactInequalities(contact_points.T, contact_normals.T, mu[0]) B[:, :3] = np.dot(B[:, :3], oRi.T) B[:, 3:] = np.dot(B[:, 3:], oRi.T) b = zeros(B.shape[0]) elif (norm(contact_points) < EPS): B = zeros((5, 6)) b = zeros(B.shape[0]) B[0, 0] = -1 B[1, 0] = 1 B[2, 1] = -1 B[3, 1] = 1 B[:, 2] = mu[0] # minimum normal force B[-1, 2] = 1 b[-1] = -fMin else: raise ValueError( "Contact with only one point that is not at the origin of the frame: NOT SUPPORTED" ) return (B, b)
def computeCostFunction(self, t): n_tasks = len(self.tasks) dims = np.empty(n_tasks, np.int) J = n_tasks * [ None, ] drift = n_tasks * [ None, ] a_des = n_tasks * [ None, ] dim = 0 for k in range(n_tasks): J[k], drift[k], a_des[k] = self.tasks[k].dyn_value( t, self.q, self.v) dims[k] = a_des[k].shape[0] dim += dims[k] A = zeros((dim, self.nv + self.k + self.na)) a = zeros(dim) i = 0 for k in range(n_tasks): A[i:i + dims[k], :self.nv] = self.task_weights[k] * J[k] a[i:i + dims[k]] = self.task_weights[k] * (a_des[k] - drift[k]) i += dims[k] D = np.dot(A, self.C) d = a - np.dot(A, self.c) return (D, d)
def updateSupportPolygon(self): ''' Compute contact points and contact normals in world frame ''' ncp = int(np.sum([p.shape[1] for p in self.rigidContactConstraints_p])) self.contact_points = zeros((3, ncp)) self.contact_normals = zeros((3, ncp)) mu_s = zeros(ncp) i = 0 for (constr, P, N, mu) in zip(self.rigidContactConstraints, self.rigidContactConstraints_p, self.rigidContactConstraints_N, self.rigidContactConstraints_mu): oMi = self.r.framePosition(constr._frame_id) for j in range(P.shape[1]): self.contact_points[:, i] = oMi.act(P[:, j]) self.contact_normals[:, i] = oMi.rotation * N[:, j] mu_s[i, 0] = mu[0] i += 1 if (ncp == 0 or not self.COMPUTE_SUPPORT_POLYGON): self.B_sp = zeros((0, 2)) self.b_sp = zeros(0) else: avg_z = np.mean(self.contact_points[2, :]) if (np.max(np.abs(self.contact_points[2, :] - avg_z)) < 1e-3): ''' Contact points are coplanar so I can simply compute the convex hull of vertical projection of contact points''' (self.B_sp, self.b_sp) = compute_convex_hull(self.contact_points[:2, :].A) else: (H, h) = compute_GIWC(self.contact_points.T, self.contact_normals.T, mu_s) (self.B_sp, self.b_sp) = compute_support_polygon( H, h, self.M[0, 0], np.array([0., 0., -9.81]), eliminate_redundancies=False) self.B_sp *= -1.0 # normalize inequalities for i in range(self.B_sp.shape[0]): tmp = np.linalg.norm(self.B_sp[i, :]) if (tmp > 1e-6): self.B_sp[i, :] /= tmp self.b_sp[i] /= tmp # self.plotSupportPolygon(); self.B_sp = np.matrix(self.B_sp) self.b_sp = np.matrix(self.b_sp).T self.support_polygon_computed = True
def setNewSensorData(self, t, q, v): self.t = t self.setPositions(q, updateConstraintReference=False) self.setVelocities(v) self.r.computeAllTerms(q, v) self.r.framesKinematics(q) self.x_com = self.r.com(q, update_kinematics=False) self.J_com = self.r.Jcom(q, update_kinematics=False) self.M = self.r.mass(q, update_kinematics=False) self.Ag = self.r.momentumJacobian(q, v) if (self.ACCOUNT_FOR_ROTOR_INERTIAS): if (self.freeFlyer): self.M[6:, 6:] += self.Md else: self.M += self.Md self.h = self.r.bias(q, v, update_kinematics=False) # self.h += self.JOINT_FRICTION_COMPENSATION_PERCENTAGE*np.dot(np.array(JOINT_VISCOUS_FRICTION), self.v); self.dx_com = np.dot(self.J_com, self.v) com_z = self.x_com[2] #-np.mean(self.contact_points[:,2]); if (com_z > 0.0): self.cp = self.x_com[:2] + self.dx_com[:2] / np.sqrt(9.81 / com_z) else: self.cp = zeros(2) self.updateConstrainedDynamics()
def createForceRegularizationTask(self, w_f): n = self.n; # number of joints k = self.k; A = zeros((12,2*n+6+k)); A[:,n+6:n+6+12] = np.diag(w_f); D = np.dot(A,self.C); d = - np.dot(A,self.c); return (D,d);
def computeCostFunction(self, t): n_tasks = len(self.tasks); dims = np.empty(n_tasks, np.int); J = n_tasks*[None,]; drift = n_tasks*[None,]; a_des = n_tasks*[None,]; dim = 0; for k in range(n_tasks): J[k], drift[k], a_des[k] = self.tasks[k].dyn_value(t, self.q, self.v); dims[k] = a_des[k].shape[0]; dim += dims[k]; A = zeros((dim, self.nv+self.k+self.na)); a = zeros(dim); i = 0; for k in range(n_tasks): A[i:i+dims[k],:self.nv] = self.task_weights[k]*J[k]; a[i:i+dims[k]] = self.task_weights[k]*(a_des[k] - drift[k]); i += dims[k]; D = np.dot(A,self.C); d = a - np.dot(A,self.c); return (D,d);
def __init__(self, name, q, v, dt, mesh_dir, urdfFileName, freeFlyer=True): if(freeFlyer): self.r = RobotWrapper(urdfFileName, mesh_dir, root_joint=se3.JointModelFreeFlyer()); else: self.r = RobotWrapper(urdfFileName, mesh_dir, None); self.freeFlyer = freeFlyer; self.nq = self.r.nq; self.nv = self.r.nv; self.na = self.nv-6 if self.freeFlyer else self.nv; self.k = 0; # number of constraints self.dt = dt; self.t = 0.0; self.name = name; self.Md = zeros((self.na,self.na)); #np.diag([ g*g*i for (i,g) in zip(INERTIA_ROTOR,GEAR_RATIO) ]); # rotor inertia ''' create low-pass filter for base velocities ''' self.baseVelocityFilter = FirstOrderLowPassFilter(dt, self.BASE_VEL_FILTER_CUT_FREQ , zeros(6)); if(freeFlyer): self.S_T = zeros((self.nv,self.na)); self.S_T[6:, :] = np.matlib.eye(self.na); else: self.S_T = np.matlib.eye(self.na); self.Nc_T = np.matlib.eye(self.nv); self.qMin = self.r.model.lowerPositionLimit; self.qMax = self.r.model.upperPositionLimit; self.dqMax = self.r.model.velocityLimit; self.ddqMax = zeros(self.na); self.ddqStop = zeros(self.na); if(self.freeFlyer): self.qMin[:6] = -1e100; # set bounds for the floating base self.qMax[:6] = +1e100; self.tauMax = self.r.model.effortLimit[6:]; else: self.tauMax = self.r.model.effortLimit; self.contact_points = zeros((0,3)); self.updateInequalityData(updateConstrainedDynamics=False); self.setNewSensorData(0, q, v);
def __init__(self, name, q, v, dt, robot): self.tasks = [] self.task_weights = [] self.r = robot if robot.model.joints[1].shortname() == "JointModelFreeFlyer": self.freeFlyer = True else: self.freeFlyer = False self.nq = self.r.nq self.nv = self.r.nv self.na = self.nv - 6 if self.freeFlyer else self.nv self.xdim = self.nv self.k = 0 # number of constraints self.dt = dt self.t = 0.0 self.name = name self.Md = zeros((self.na, self.na)) # np.diag([ g*g*i for (i,g) in zip(INERTIA_ROTOR,GEAR_RATIO) ]); # rotor inertia ''' create low-pass filter for base velocities ''' self.baseVelocityFilter = FirstOrderLowPassFilter( dt, self.BASE_VEL_FILTER_CUT_FREQ, zeros(6)) if (self.freeFlyer): self.S_T = zeros((self.nv, self.na)) self.S_T[6:, :] = np.matlib.eye(self.na) else: self.S_T = np.matlib.eye(self.na) self.Nc_T = np.matlib.eye(self.nv) self.qMin = self.r.model.lowerPositionLimit self.qMax = self.r.model.upperPositionLimit self.dqMax = self.r.model.velocityLimit self.ddqMax = zeros(self.na) self.ddqStop = zeros(self.na) if (self.freeFlyer): self.qMin[:7] = -1e100 # set bounds for the floating base self.qMax[:7] = +1e100 self.tauMax = self.r.model.effortLimit[6:] else: self.tauMax = self.r.model.effortLimit self.contact_points = zeros((0, 3)) self.updateInequalityData(updateConstrainedDynamics=False) self.setNewSensorData(0, q, v)
def getZmp(self, f_l, f_r): return zeros(2)
def updateInequalityData(self, updateConstrainedDynamics=True): self.updateSupportPolygon() self.m_in = 0 # number of inequalities c = len(self.rigidContactConstraints) # number of unilateral contacts self.k = int(np.sum([con.dim for con in self.rigidContactConstraints])) self.k += int( np.sum([con.dim for con in self.bilateralContactConstraints])) if (self.ENABLE_FORCE_LIMITS): self.rigidContactConstraints_m_in = np.zeros(c, np.int) Bf = zeros((0, self.k)) bf = zeros(0) ii = 0 for i in range(c): (Bfi, bfi) = self.createContactForceInequalities(self.rigidContactConstraints_fMin[i], self.rigidContactConstraints_mu[i], \ self.rigidContactConstraints_p[i], self.rigidContactConstraints_N[i], \ self.rigidContactConstraints[i].framePosition().rotation) self.rigidContactConstraints_m_in[i] = Bfi.shape[0] tmp = zeros((Bfi.shape[0], self.k)) dim = self.rigidContactConstraints[i].dim mask = self.rigidContactConstraints[i]._mask tmp[:, ii:ii + dim] = Bfi[:, mask] ii += dim Bf = np.vstack((Bf, tmp)) bf = np.vstack((bf, bfi)) self.ind_force_in = range( self.m_in, self.m_in + np.sum(self.rigidContactConstraints_m_in)) self.m_in += np.sum(self.rigidContactConstraints_m_in) else: self.ind_force_in = [] if (self.ENABLE_JOINT_LIMITS): self.ind_acc_in = range(self.m_in, self.m_in + 2 * self.na) self.m_in += 2 * self.na else: self.ind_acc_in = [] if (self.ENABLE_TORQUE_LIMITS): self.lb = -self.tauMax self.ub = self.tauMax else: self.lb = zeros(self.na) - 1e100 self.ub = zeros(self.na) + 1e100 if (self.ENABLE_CAPTURE_POINT_LIMITS): self.ind_cp_in = range(self.m_in, self.m_in + self.b_sp.size) self.m_in += self.b_sp.size else: self.ind_cp_in = [] # resize all data that depends on k self.B = zeros((self.m_in, self.nv + self.k + self.na)) self.b = zeros(self.m_in) self.Jc = zeros((self.k, self.nv)) self.dJc_v = zeros(self.k) self.dx_c = zeros(self.k) self.ddx_c_des = zeros(self.k) self.Jc_Minv = zeros((self.k, self.nv)) self.Lambda_c = zeros((self.k, self.k)) self.Jc_T_pinv = zeros((self.k, self.nv)) self.C = zeros((self.nv + self.k + self.na, self.na)) self.c = zeros(self.nv + self.k + self.na) if (self.ENABLE_FORCE_LIMITS and self.k > 0): self.B[self.ind_force_in, self.nv:self.nv + self.k] = Bf self.b[self.ind_force_in] = bf #print "Contact inequality constraints:\n", self.B[self.ind_force_in, self.nv:self.nv+self.k], "\n", bf.T; if (updateConstrainedDynamics): self.updateConstrainedDynamics()