def scaleToBounds(val, bmin, bmax, origin=None): """Cap the ray starting origin in the direction val against bounding box limits. """ if origin != None: if isinstance(bmin, np.ndarray): bmin -= origin else: bmin = vectorops.sub(bmin, origin) if isinstance(bmax, np.ndarray): bmax -= origin else: bmax = vectorops.sub(bmax, origin) return scaleToBounds(val, bmin, bmax) umax = 1.0 for vali, ai, bi in zip(val, bmin, bmax): assert bi >= ai if vali < ai: umax = ai / vali if vali > bi: umax = bi / vali if umax == 1.0: return val assert umax >= 0 print "Scaling to velocity maximum, factor", umax if isinstance(val, np.ndarray): return val * umax else: return vectorops.mul(val, umax)
def segment_triangle_intersection(seg, tri, umin=0, umax=1): """Given a 3D segment (p,q) and a triangle (a,b,c), returns the point of intersection if the region of the segment in interval [umin,umax] intersects the triangle. Otherwise returns None""" p, q = seg a, b, c = tri d = vectorops.sub(b, a) e = vectorops.sub(c, a) n = vectorops.cross(d, e) if vectorops.norm(n) < 1e-7: #degenerate return None n = vectorops.unit(n) ofs = vectorops.dot(n, a) #Solve for n^T(p + u(q-p)) = ofs denom = vectorops.dot(n, vectorops.sub(q, p)) numer = ofs - vectorops.dot(n, p) if abs(denom) < 1e-7: #near parallel return None u = numer / denom if u < umin or u > umax: return None #now check whether point of intersection lies in triangle x = vectorops.madd(p, vectorops.sub(q, p), u) xloc = vectorops.sub(x, a) #solve for coordinates [r,s,t] such that xloc = r*n + s*d + t*e try: rst = np.dot(np.linalg.inv(np.array([n, d, e]).T), xloc) except np.linalg.linalg.LinAlgError: return None r, s, t = rst assert abs(r) < 1e-4 if s >= 0 and t >= 0 and s + t <= 1: return x return None
def segment_segment_intersection(seg1, seg2, umin=0, umax=1, vmin=0, vmax=1): """Given 2 2D segments (a1,b1) and (a2,b2) returns whether the portion of seg1 in the range [umin,umax] overlaps seg2 in the range [vmin,vmax]. Returns the point of intersection or None if no intersection exists """ a1, b1 = seg1 a2, b2 = seg2 assert len(a1) == 2 assert len(b1) == 2 assert len(a2) == 2 assert len(b2) == 2 d = vectorops.sub(b1, a1) a, b = vectorops.sub(a2, a1), vectorops.sub(b2, a1) #now it's centered at a1 #u*d = a + v*(b-a) e = vectorops.sub(b2, a2) A = np.array([d, vectorops.mul(e, -1)]).T try: uv = np.dot(np.linalg.inv(A), a) except np.linalg.linalg.LinAlgError: return None u, v = uv if u < umin or u > umax: return None if v < umin or v > umax: return None return vectorops.interpolate(a1, b1, u)
def predict_next_location(self, x, v, t, gravity): # update estimated x value x_temp = vectorops.sub(vectorops.madd(x, v, t), [0, 0, 0.95 * gravity * t * t]) # if x_temp[2] is less than 0, then extrapolate exact time where t would've hit 0.03 if (x_temp[2] < 0.03): # extrapolate exact time where t would've hit t = t * ((x[2] - 0.03) / (x[2] - x_temp[2])) # update x position x = vectorops.sub(vectorops.madd(x, v, t), [0, 0, 0.95 * gravity * t * t]) # set the z coordinate to 0 (to be sure) x[2] = 0.03 # update velocity value val = 0.7 v[0] = v[0] * val v[1] = v[1] * val v[2] = (v[2] * -1 * val) - .95 * gravity * t else: x = x_temp v = vectorops.sub(v, [0, 0, 0.95 * gravity * t ]) # t is updated based on above computations return (x, v)
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def draw_hull(hull,scale=0.01): """Draws a ConvexHull with 3D texture coordinates""" if len(hull.simplices)==0: print "Hull with no simplices?" return centroid = sum([hull.points[v] for v in hull.vertices],[0.0]*3) / len(hull.vertices) * scale vmin = [min([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)] vmax = [max([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)] uvwscale = [1.0/(b-a) if b != a else 1.0 for (a,b) in zip(vmin,vmax)] for simplex in hull.simplices: ia,ib,ic = simplex a = vectorops.mul(hull.points[ia].tolist(),scale) b = vectorops.mul(hull.points[ib].tolist(),scale) c = vectorops.mul(hull.points[ic].tolist(),scale) n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a)) if vectorops.dot(n,vectorops.sub(centroid,a)) > 0: b,c = c,b n = vectorops.mul(n,-1) try: n = vectorops.mul(n,1.0/vectorops.norm(n)) glNormal3f(*n) except ZeroDivisionError: pass glBegin(GL_TRIANGLES) glTexCoord3f(uvwscale[0]*(a[0]-vmin[0]),uvwscale[1]*(a[1]-vmin[1]),uvwscale[2]*(a[2]-vmin[2])) glVertex3f(*a) glTexCoord3f(uvwscale[0]*(b[0]-vmin[0]),uvwscale[1]*(b[1]-vmin[1]),uvwscale[2]*(b[2]-vmin[2])) glVertex3f(*b) glTexCoord3f(uvwscale[0]*(c[0]-vmin[0]),uvwscale[1]*(c[1]-vmin[1]),uvwscale[2]*(c[2]-vmin[2])) glVertex3f(*c) glEnd()
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP, eP) vcmd = vP #D term vcur = self.getSensedVelocity(q, dq, dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD, eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI, self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt) print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm) print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd return vcmd
def df_dz(self, x, y, z): n = len(y) m = len(z) // n result = np.zeros((6, len(z))) T = self.env_geom.getTransform() CoM = se3.apply(T, self.env_obj.getMass().getCom()) for i in range(n): point = y[i][1] Normal_normal = normal(self.env_geom, point) n1 = so3.canonical(Normal_normal)[3:6] n2 = so3.canonical(Normal_normal)[6:9] Normal_friction = [] for j in range(6): n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)).tolist() Normal_friction.append(vectorops.unit(n_tmp)) Cross_Normal = list( vectorops.cross(vectorops.sub(point, CoM), Normal_normal)) Cross_Normal_friction = [] for j in range(6): cross_Normal_v = list( vectorops.cross(vectorops.sub(point, CoM), Normal_friction[j])) Cross_Normal_friction.append(cross_Normal_v) result[0:3, i * m:(i + 1) * m - 1] = np.asarray(Normal_friction).T result[0:3, (i + 1) * m - 1:(i + 1) * m] = np.asarray(Normal_normal).reshape((3, 1)) result[3:6, i * m:(i + 1) * m - 1] = np.asarray(Cross_Normal_friction).T result[3:6, (i + 1) * m - 1:(i + 1) * m] = np.asarray(Cross_Normal).reshape((3, 1)) return result
def scaleToBounds( val, bmin, bmax, origin=None): """Cap the ray starting origin in the direction val against bounding box limits. """ if origin != None: if isinstance(bmin,np.ndarray): bmin -= origin else: bmin = vectorops.sub(bmin,origin) if isinstance(bmax,np.ndarray): bmax -= origin else: bmax = vectorops.sub(bmax,origin) return scaleToBounds(val,bmin,bmax) umax = 1.0 for vali,ai,bi in zip(val,bmin,bmax): assert bi >= ai if vali < ai: umax = ai/vali if vali > bi: umax = bi/vali if umax==1.0: return val assert umax >= 0 print "Scaling to velocity maximum, factor",umax if isinstance(val,np.ndarray): return val*umax else: return vectorops.mul(val,umax)
def apply_tendon_forces(self,i,link1,link2,rest_length): tendon_c2 = 30000.0 tendon_c1 = 10000.0 b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3)) b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p0w = se3.apply(b1.getTransform(),self.tendon0_local) p1w = se3.apply(b1.getTransform(),self.tendon1_local) p2w = se3.apply(b2.getTransform(),self.tendon2_local) d = vectorops.distance(p1w,p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w,p1w)) f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length) #print d,rest_length #print "Force magnitude",self.model.robot.link(link1).getName(),":",f f = min(f,100) #tendon routing force straight = vectorops.unit(vectorops.sub(p2w,p0w)) pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w)) pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0)) tangential_axis = vectorops.cross(pulley_axis,pulley_direction) cosangle = vectorops.dot(straight,tangential_axis) #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle base_direction = so3.apply(b0.getTransform()[0],[0,0,-1]) b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04)) b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local) b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local) b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local) self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f) self.forces[i][2] = vectorops.mul(direction,f) else: self.forces[i] = [None,None,None] return
def opt_error_fun(est_input, *args): """ :param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle :param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix :return: error: {float} -- the error between the true transformation and estimated transformation """ Roct = so3.from_rpy(est_input[0:3]) toct = est_input[3:6] Rn = so3.from_rpy(est_input[6:9]) tn = est_input[9:12] Tlink_set = args[0] Tneedle2oct_icp = args[1] fun_list = np.array([]) for i in range(len(Tlink_set)): fun_list = np.append( fun_list, np.multiply( so3.error(so3.inv(Tneedle2oct_icp[i][0]), so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]), Rn)), 1)) fun_list = np.append( fun_list, np.multiply( vectorops.sub( vectorops.sub([0., 0., 0.], so3.apply(so3.inv(Tneedle2oct_icp[i][0]), Tneedle2oct_icp[i][1])), so3.apply( so3.inv(Roct), vectorops.add(vectorops.sub(Tlink_set[i][1], toct), so3.apply(Tlink_set[i][0], tn)))), 1000)) return fun_list
def segment_point_distance(a, b, x): """Returns (distance, parameter) pair between segment (a,b) and point x""" d = vectorops.sub(b, a) u = vectorops.dot(vectorops.sub(x, a), d) / vectorops.dot(d, d) u = min(1.0, max(u, 0.0)) d = vectorops.distance(x, vectorops.interpolate(a, b, u)) return (d, u)
def substep(self, dt): twist = self.twist #compute the angular velocity of the shell in the motor frame motorBody = self.sim.body(self.robot.link(5)) shellBody = self.sim.body(self.robot.link(8)) motorTwist = motorBody.getVelocity() shellTwist = shellBody.getVelocity() motorXform = motorBody.getTransform() shellXform = shellBody.getTransform() shellRelativeAvel = so3.apply( so3.inv(motorXform[0]), vectorops.sub(shellTwist[0], motorTwist[0])) #print "Relative angular vel",shellRelativeAvel desiredTurnSpeed = twist[2] * self.velocityLimits[0] desiredDriveSpeed = 0 if twist[0] == 0 or twist[0] * self.motorSpeeds[1] < 0: #stop desiredDriveSpeed = 0 else: desiredDriveSpeed = self.motorSpeeds[ 1] + twist[0] * self.accelLimits[1] * self.dt #print "Turn des",desiredTurnSpeed, "drive des",desiredDriveSpeed #clamp speeds to limits desiredTurnSpeed = max(-self.velocityLimits[0], min(desiredTurnSpeed, self.velocityLimits[0])) desiredDriveSpeed = max(-self.velocityLimits[1], min(desiredDriveSpeed, self.velocityLimits[1])) terr = desiredTurnSpeed - self.motorSpeeds[0] derr = desiredDriveSpeed - self.motorSpeeds[1] #clamp desired accelerations to limits terr = max(-self.accelLimits[0] * self.dt, min(terr, self.accelLimits[0] * self.dt)) derr = max(-self.accelLimits[1] * self.dt, min(derr, self.accelLimits[1] * self.dt)) self.motorSpeeds[0] += terr self.motorSpeeds[1] += derr #apply locked velocity control to bring shell up to relative speed #this is the desired angular velocity of the shell in the motor #coordinates desiredShellAvel = [self.motorSpeeds[1], 0, self.motorSpeeds[0]] #print "Desired angular vel",desiredShellAvel relativeVelError = vectorops.sub(desiredShellAvel, shellRelativeAvel) wrenchlocal = vectorops.mul(relativeVelError, self.velocityLockGain) #local wrench is k*(wdes-wrel) wrench = so3.apply(motorXform[0], wrenchlocal) #print "Wrench to shell",wrench motorBody.applyWrench([0, 0, 0], vectorops.mul(wrench, -1.0)) shellBody.applyWrench([0, 0, 0], wrench) #disable PID controllers self.controller.setTorque([0, 0, 0]) #apply rolling friction forces shellBody.applyWrench([0, 0, 0], vectorops.mul(shellTwist[0], -self.rollingFrictionCoeff)) return
def look_at(self, pos, tgt, scale=None): if self.lockX: tgt[0] = pos[0] if self.lockY: tgt[1] = pos[1] cam = self.view.camera if scale is not None: cam.dist = op.norm(op.sub(tgt, pos)) * scale cam.rot = self.get_camera_rot(op.sub(pos, tgt)) cam.tgt = tgt
def config_to_opening(self,qfinger): """Estimates far qfinger is from closed_config to open_config. Only meaningful if qfinger is close to being along the straight C-space line between the two. """ if self.closed_config is None or self.open_config is None: raise ValueError("Can't estimate opening width to") assert len(qfinger) == len(self.closed_config) b = vectorops.sub(qfinger,self.closed_config) a = vectorops.sub(self.open_config,self.closed_config) return min(1,max(0,vectorops.dot(a,b)/vectorops.normSquared(a)))
def calculate_workspace_axis(robot, obstacles, end_effector, point_local, axis_local, axis_world): global lower_corner, upper_corner resolution = (15, 15, 15) cellsize = vectorops.div(vectorops.sub(upper_corner, lower_corner), resolution) invcellsize = vectorops.div(resolution, vectorops.sub(upper_corner, lower_corner)) reachable = np.zeros(resolution) #TODO: your code here return reachable
def distMetric(self, n1, n2): dist = self.env.n * vectorops.norm( vectorops.sub([n1.x, n1.y, n1.z], [n2.x, n2.y, n2.z])) dist += self.env.sumDist * math.fabs(n1.sc - n2.sc) dist += 1 * self.env.sumDist * math.fabs( n1.sc + n2.sc) / 2 * math.fabs(n1.tht - n2.tht) return dist
def bbox(bmin, bmax, R=None, t=None, world=None, name=None, mass=float('inf'), type='TriangleMesh'): """Makes a box from bounds [bmin,bmax]. Args: bmin (list of 3 floats): the lower corner of the box center (list of 3 floats): the upper corner of the box R,t (se3 transform, optional): if given, the box's world coordinates will be rotated and shifted by this transform. world (WorldModel, optional): If given, then the box will be a RigidObjectModel or TerrainModel will be created in this world name (str, optional): If world is given, this is the name of the object. Default is 'box'. mass (float, optional): If world is given and this is inf, then a TerrainModel will be created. Otherwise, a RigidObjectModel will be created with automatically determined inertia. type (str, optional): the geometry type. Defaults to 'TriangleMesh', but also 'GeometricPrimitive' and 'VolumeGrid' are accepted. Returns: Geometry3D, RigidObjectModel, or TerrainModel: A representation of the box. If a world is given, then either a RigidObjectModel or TerrainModel is added to the world and returned. """ w, d, h = vectorops.sub(bmax, bmin) center = vectorops.interpolate(bmin, bmax, 0.5) return box(w, d, h, center, R, t, world, name, mass, type)
def updateVis(self): """This gets called every control loop. TODO: You may consider visually debugging some of your code here, along with initVis(). For example, to draw a ghost robot at a given configuration q, you can call: kviz.add_ghost() (in initVis) kviz.set_ghost(q) (in updateVis) The current code draws gravity-inflenced arcs leading from all the object position / velocity estimates from your state estimator. Event C folks should set gravity=0 in the following code. """ if self.objectEstimates: for o in self.objectEstimates.objects: #draw a point kviz.update_sphere("object_est"+str(o.name),o.x[0],o.x[1],o.x[2],0.03) kviz.set_color("object_est"+str(o.name),o.name[0],o.name[1],o.name[2]) #draw an arc trace = [] x = [o.x[0],o.x[1],o.x[2]] v = [o.x[3],o.x[4],o.x[5]] if event=='C': gravity = 0 else: gravity = 9.8 for i in range(20): t = i*0.05 trace.append(vectorops.sub(vectorops.madd(x,v,t),[0,0,0.5*gravity*t*t])) kviz.update_polyline("object_trace"+str(o.name),trace); kviz.set_color("object_trace"+str(o.name),o.name[0],o.name[1],o.name[2])
def sample_object_pose_table(obj, stable_fs, bmin, bmax): """Samples a transform of the object so that it lies on in the given bounding box bmin,bmax. Args: obj (RigidObjectModel) stable_fs (list of lists): giving the stable faces of the object, as in MP2. bmin,bmax (3-vectors): the bounding box of the area in which the objects should lie. """ table_height = bmin[2] + 0.001 face = random.choice(stable_fs) normal = np.cross(face[1] - face[0], face[2] - face[0]) normal = normal / np.linalg.norm(normal) centroid = np.sum(face, axis=0) / len(face) x = random.uniform(bmin[0], bmax[0]) y = random.uniform(bmin[1], bmax[1]) z = table_height + np.dot(centroid, normal) Rnormal = so3.canonical((-normal).tolist()) Rx = so3.rotation((1, 0, 0), random.uniform(0, math.pi * 2)) Rxz = so3.rotation((0, 1, 0), -math.pi * 0.5) R = so3.mul(Rxz, so3.mul(Rx, so3.inv(Rnormal))) #R*com + t = [x,y,_] t = vectorops.sub([x, y, z], so3.apply(R, obj.getMass().getCom())) t[2] = z obj.setTransform(R, t)
def updateVis(objectEstimates): """This gets called by update() TODO: You may consider visually debugging some of your code here, along with initVis(). The current code draws gravity-inflenced arcs leading from all the object position / velocity estimates from your state estimator. """ gravity = GRAVITY if objectEstimates: for o in objectEstimates.objects: #draw a point kviz.update_sphere("object_est" + str(o.name), o.x[0], o.x[1], o.x[2], 0.03) kviz.set_color("object_est" + str(o.name), o.name[0], o.name[1], o.name[2]) #draw an arc trace = [] x = [o.x[0], o.x[1], o.x[2]] v = [o.x[3], o.x[4], o.x[5]] for i in range(20): t = i * 0.05 trace.append( vectorops.sub(vectorops.madd(x, v, t), [0, 0, 0.5 * gravity * t * t])) if trace[-1][2] < 0: break kviz.remove("object_trace" + str(o.name)) kviz.add_polyline("object_trace" + str(o.name), trace) kviz.set_color("object_trace" + str(o.name), o.name[0], o.name[1], o.name[2])
def output_and_advance(self, **inputs): try: q = inputs['q'] dq = inputs['dq'] u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt']) except KeyError: print "Warning, cannot debug motion model, dq or dqcmd not in input" return None if self.dqpredlast != None: if self.activeDofs != None: dq = dq[:] for i in [ i for i in range(len(q)) if i not in self.activeDofs ]: dq[i] = self.dqpredlast[i] #compare motion model to dq print "Motion model error:", np.linalg.norm(self.dqpredlast - np.array(dq)) (v, i) = max( zip( np.abs(self.dqpredlast - np.array(dq)).tolist(), range(len(dq)))) print " Max error:", v, "at", i, if self.robot != None: print self.robot.link(i).getName() else: print print " Command:", self.ulast[i], "Predicted:", self.dqpredlast[ i], "Actual:", dq[i] print " pred:", self.Alast[i, i], "*u +", self.blast[i]
def grasp_from_contacts(contact1,contact2): """Helper: if you have two contacts, this returns an AntipodalGrasp""" d = vectorops.unit(vectorops.sub(contact2.x,contact1.x)) grasp = AntipodalGrasp(vectorops.interpolate(contact1.x,contact2.x,0.5),d) grasp.finger_width = vectorops.distance(contact1.x,contact2.x) grasp.contact1 = contact1 grasp.contact2 = contact2 return grasp
def simulateForceSpring(self, kP=10.0): if not self.forceApplicationMode: return self.sim.updateWorld() body = self.sim.body(self.forceObject) T = self.forceObject.getTransform() wp = se3.apply(T, self.forceLocalPoint) f = vectorops.mul(vectorops.sub(self.forceAnchorPoint, wp), kP) body.applyForceAtLocalPoint(f, self.forceLocalPoint)
def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio): reactive_vel = [0.0, 0.0, 0.0] pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand)) pos_diff_norm = vectorops.norm(pos_diff) if pos_diff_norm >= 0.02: vel = vectorops.mul(pos_diff, vel_ratio) reactive_vel = list(vel) return reactive_vel
def taskDifference(self, a, b): if self.taskType == 'po': return se3.error(a, b) elif self.taskType == 'position': return vectorops.sub(a, b) elif self.taskType == 'orientation': return so3.error(a, b) else: raise ValueError("Invalid taskType " + self.taskType)
def make_object_arrangement(world, container, objects, container_wall_thickness=0.01, max_iterations=100, remove_failures=False): """For a given container and a list of objects in the world, places the objects inside the container with randomized x-y locations and z orientations so that they are initially collision free and on the bottom of the container. Args: world (WorldModel): the world containing the objects and obstacles container: the container RigidObject / Terrain in world into which objects should be spawned. Assumed axis-aligned. objects (list of RigidObject): a list of RigidObjects in the world, at arbitrary locations. They are placed in order. container_wall_thickness (float, optional): a margin subtracted from the container's outer dimensions into which the objects are spawned. max_iterations (int, optional): the maximum number of iterations used for sampling object initial poses remove_failures (bool): if True, then instead of returning None on failure, the objects that fail placement are removed from the world. Returns: (WorldModel): if successful, the positions of objects in world are modified and world is returned. On failure, None is returned. Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy() to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances. """ container_outer_bb = _get_bound(container) container_inner_bb = (vectorops.add(container_outer_bb[0], [container_wall_thickness] * 3), vectorops.sub(container_outer_bb[1], [container_wall_thickness] * 3)) collision_margin = 0.0025 for object in objects: #make sure the bottom of the object touches the bottom of the container obb = _get_bound(object) zmin = obb[0][2] R, t = object.getTransform() t[2] = container_inner_bb[2] - zmin + collision_margin object.setTransform(R, t) failures = xy_jiggle(world, rigid_objects, [container], container_inner_bb[0], container_inner_bb[1], max_iterations) if len(failures) > 0: if remove_failures: removeIDs = [objects[i].index for i in failures] for i in sorted(removeIDs)[::-1]: world.remove(world.rigidObject(i)) else: return None return world
def taskDifference(self,a,b): if self.taskType == 'po': return se3.error(a,b) elif self.taskType == 'position': return vectorops.sub(a,b) elif self.taskType == 'orientation': return so3.error(a,b) else: raise ValueError("Invalid taskType "+self.taskType)
def get_camera_dir(self, zeroZ=False): cam = self.view.camera dir = op.sub(cam.tgt, self.get_camera_pos()) if zeroZ: dir = (dir[0], dir[1], 0) if op.norm(dir) > 1e-6: dir = op.mul(dir, 1 / op.norm(dir)) dir[1] *= -1 return dir
def apply_tendon_forces(self, i, link1, link2, rest_length): tendon_c2 = 30000.0 tendon_c1 = 10000.0 b0 = self.sim.body( self.model.robot.link(self.model.proximal_links[0] - 3)) b1 = self.sim.body(self.model.robot.link(link1)) b2 = self.sim.body(self.model.robot.link(link2)) p0w = se3.apply(b1.getTransform(), self.tendon0_local) p1w = se3.apply(b1.getTransform(), self.tendon1_local) p2w = se3.apply(b2.getTransform(), self.tendon2_local) d = vectorops.distance(p1w, p2w) if d > rest_length: #apply tendon force direction = vectorops.unit(vectorops.sub(p2w, p1w)) f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d - rest_length) #print d,rest_length #print "Force magnitude",self.model.robot.link(link1).getName(),":",f f = min(f, 100) #tendon routing force straight = vectorops.unit(vectorops.sub(p2w, p0w)) pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w)) pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0)) tangential_axis = vectorops.cross(pulley_axis, pulley_direction) cosangle = vectorops.dot(straight, tangential_axis) #print "Cosine angle",self.model.robot.link(link1).getName(),cosangle base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1]) b0.applyForceAtLocalPoint( vectorops.mul(base_direction, -f), vectorops.madd(p0w, base_direction, 0.04)) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, cosangle * f), self.tendon1_local) b1.applyForceAtLocalPoint( vectorops.mul(tangential_axis, -cosangle * f), self.tendon0_local) b2.applyForceAtLocalPoint(vectorops.mul(direction, -f), self.tendon2_local) self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f) self.forces[i][2] = vectorops.mul(direction, f) else: self.forces[i] = [None, None, None] return
def value(self, x, y, z): result = np.zeros(6) if len(y) == 0: result[0:3] += self.mass * self.gravity_coefficient * np.asarray( self.gravity_direction) return result n = len(y) m = len(z) // n assert m == 7 T = self.env_geom.getTransform() CoM = se3.apply(T, self.env_obj.getMass().getCom()) for i in range(n): point = y[i][1] f_normal = z[m * (i + 1) - 1] f_friction = z[m * i:m * (i + 1) - 1] Normal_normal = normal(self.env_geom, point) n1 = so3.canonical(Normal_normal)[3:6] n2 = so3.canonical(Normal_normal)[6:9] Normal_friction = [] for j in range(6): n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin( (math.pi / 3) * j) * np.array(n2)).tolist() Normal_friction.append(vectorops.unit(n_tmp)) Cross_Normal = list( vectorops.cross(vectorops.sub(point, CoM), Normal_normal)) Cross_Normal_friction = [] for j in range(6): cross_Normal_v = list( vectorops.cross(vectorops.sub(point, CoM), Normal_friction[j])) Cross_Normal_friction.append(cross_Normal_v) result[0:3] += np.asarray(Normal_normal) * f_normal + np.asarray( f_friction) @ np.asarray(Normal_friction) result[3:6] += np.asarray(Cross_Normal) * f_normal + np.asarray( f_friction) @ np.asarray(Cross_Normal_friction) result[0:3] += self.mass * self.gravity_coefficient * np.asarray( self.gravity_direction) return result
def findOptPrepareLoc(self, hit_xyz, prep_loc_dict): best_loc = None min_dist = float('inf') for each_key in prep_loc_dict.keys(): cur_prep = prep_loc_dict[each_key] cur_dist = vectorops.norm(vectorops.sub(hit_xyz, cur_prep)) if cur_dist < min_dist: best_loc = each_key min_dist = cur_dist return best_loc
def error_analysis(self): """ :return: None """ Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6]) Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12]) trans_error_list = [] trans_error_mat = [] rot_error_list = [] rot_error_mat = [] redraw_list = [] for i in range(len(self.Tlink_set)): Toct_needle_est = se3.mul( se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle) trans_error = vectorops.sub( se3.inv(self.trans_list[i])[1], Toct_needle_est[1]) rot_error = so3.error( se3.inv(self.trans_list[i])[0], Toct_needle_est[0]) trans_error_list.append(vectorops.normSquared(trans_error)) trans_error_mat.append(np.absolute(trans_error)) rot_error_list.append(vectorops.normSquared(rot_error)) rot_error_mat.append(np.absolute(rot_error)) redraw_list.append( redraw_pcd(self.pcd_list[i], Toct_needle_est)[0]) redraw_list.append( redraw_pcd(self.pcd_list[i], Toct_needle_est)[1]) redraw_list.append( create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0])) draw_geometries(redraw_list) trans_error_list = np.array(trans_error_list) trans_error_mat = np.array(trans_error_mat) rot_error_list = np.array(rot_error_list) rot_error_mat = np.array(rot_error_mat) rmse_trans = np.sqrt(np.mean(trans_error_list)) rmse_rot = np.sqrt(np.mean(rot_error_list)) print("The squared translation error list is:\n ", np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans) print("The mean error in XYZ directions is: ", np.mean(trans_error_mat, axis=0)) print("The squared rotation error list is:\n ", np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot) print("The mean error in three rotation vectors is: ", np.mean(rot_error_mat, axis=0)) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list), # np.sqrt(rot_error_list)]), allow_pickle=True) # np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num + # "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0), # np.mean(rot_error_mat, axis=0)]), allow_pickle=True) trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world): global lower_corner,upper_corner resolution = (15,15,15) cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution) invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner)) reachable = np.zeros(resolution) #TODO: your code here for i in range(resolution[0]): for j in range(resolution[1]): for k in range(resolution[2]): orig_config = robot.getConfig() point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner) world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world) local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local) point_goal = ik.objective(end_effector, local=point_local, world=point) if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10): reachable[i,j,k] = 1.0 robot.setConfig(orig_config) return reachable
def mahalanobis_distance2(u,v,A=None): if A is None: return vectorops.distanceSquared(u,v) else: z = np.array(vectorops.sub(u,v)) return np.dot(z.T,np.dot(A,z))
def taskDifference(self,a,b): """Default: assumes a Cartesian space""" return vectorops.sub(a,b)
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True, visualize=False,verbose=0): """For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable. Args: world (WorldModel): the world containing the objects and obstacles container: the container RigidObject / Terrain in world into which objects should be spawned. Assumed axis-aligned. objects (list of RigidObject): a list of RigidObjects in the world, at arbitrary locations. They are placed in order. container_wall_thickness (float, optional): a margin subtracted from the container's outer dimensions into which the objects are spawned. randomize_orientation (bool or str, optional): if True, the orientation of the objects are completely randomized. If 'z', only the z orientation is randomized. If False or None, the orientation is unchanged visualize (bool, optional): if True, pops up a visualization window to show the progress of the pile verbose (int, optional): if > 0, prints progress of the pile. Side effect: the positions of objects in world are modified Returns: (tuple): (world,sim), containing - world (WorldModel): the original world - sim (Simulator): the Simulator instance at the state used to obtain the stable placement of the objects. Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy() to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances. """ container_outer_bb = _get_bound(container) container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3)) spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:]) collision_margin = 0.0025 if visualize: from klampt import vis from klampt.model import config import time oldwindow = vis.getWindow() newwindow = vis.createWindow("make_object_pile dynamic visualization") vis.setWindow(newwindow) vis.show() visworld = world.copy() vis.add("world",visworld) sim = Simulator(world) sim.setSetting("maxContacts","20") sim.setSetting("adaptiveTimeStepping","0") Tfar = (so3.identity(),[0,0,-100000]) for object in objects: R,t = object.getTransform() object.setTransform(R,Tfar[1]) sim.body(object).setTransform(*Tfar) sim.body(object).enable(False) if verbose: print("Spawn area",spawn_area) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() for index in range(len(objects)): #always spawn above the current height of the pile if index > 0: objects_bound = _get_bound(objects[:index]) if verbose: print("Existing objects bound:",objects_bound) zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2]) spawn_area[0][2] += zshift spawn_area[1][2] += zshift object = objects[index] obb = _get_bound(object) zmin = obb[0][2] R0,t0 = object.getTransform() feasible = False for sample in range(1000): R,t = R0[:],t0[:] if randomize_orientation == True: R = so3.sample() t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin object.setTransform(R,t) xy_randomize(object,spawn_area[0],spawn_area[1]) if verbose: print("Sampled position of",object.getName(),object.getTransform()[1]) if not randomize_orientation: _,t = object.getTransform() object.setTransform(R,t) #object spawned, now settle sobject = sim.body(object) sobject.enable(True) sobject.setTransform(*object.getTransform()) res = sim.checkObjectOverlap() if len(res[0]) == 0: feasible = True #get it low as possible without overlapping R,t = object.getTransform() for lower in range(100): sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01])) res = sim.checkObjectOverlap() if len(res[0]) != 0: if verbose: print("Terminated lowering at",lower,"cm lower") sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01])) res = sim.checkObjectOverlap() break sim.updateWorld() break if not feasible: if verbose: print("Failed to place object",object.getName()) return None if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(0.1) if verbose: print("Beginning to simulate") #start letting everything fall for firstfall in range(10): sim.simulate(0.01) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(0.01) maxT = 5.0 dt = 0.01 t = 0.0 wdamping = -0.01 vdamping = -0.1 while t < maxT: settled = True for object in objects: sobject = sim.body(object) w,v = sobject.getVelocity() sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping)) if vectorops.norm(w) + vectorops.norm(v) > 1e-4: #settled settled=False break if settled: break if visualize: t0 = time.time() sim.simulate(dt) if visualize: vis.lock() config.setConfig(visworld,config.getConfig(world)) vis.unlock() time.sleep(max(0.0,dt-(time.time()-t0))) t += dt if visualize: vis.show(False) vis.setWindow(oldwindow) return (world,sim)
def make_object_arrangement(world,container,objects,container_wall_thickness=0.01,max_iterations=100,remove_failures=False): """For a given container and a list of objects in the world, places the objects inside the container with randomized x-y locations and z orientations so that they are initially collision free and on the bottom of the container. Args: world (WorldModel): the world containing the objects and obstacles container: the container RigidObject / Terrain in world into which objects should be spawned. Assumed axis-aligned. objects (list of RigidObject): a list of RigidObjects in the world, at arbitrary locations. They are placed in order. container_wall_thickness (float, optional): a margin subtracted from the container's outer dimensions into which the objects are spawned. max_iterations (int, optional): the maximum number of iterations used for sampling object initial poses remove_failures (bool): if True, then instead of returning None on failure, the objects that fail placement are removed from the world. Returns: (WorldModel): if successful, the positions of objects in world are modified and world is returned. On failure, None is returned. Note: Since world is modified in-place, if you wish to make multiple worlds with piles of the same objects, you should use world.copy() to store the configuration of the objects. You may also wish to randomize the object ordering using random.shuffle(objects) between instances. """ container_outer_bb = _get_bound(container) container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3)) collision_margin = 0.0025 for object in objects: #make sure the bottom of the object touches the bottom of the container obb = _get_bound(object) zmin = obb[0][2] R,t = object.getTransform() t[2] = container_inner_bb[2] - zmin + collision_margin object.setTransform(R,t) failures = xy_jiggle(world,rigid_objects,[container],container_inner_bb[0],container_inner_bb[1],max_iterations) if len(failures) > 0: if remove_failures: removeIDs = [objects[i].index for i in failures] for i in sorted(removeIDs)[::-1]: world.remove(world.rigidObject(i)) else: return None return world