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()
