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 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 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 transform_point_cloud(pc, T, point_channels=[0, 1, 2], normal_channels=[6, 7, 8]): """Given a point cloud `pc` and a transform T, apply the transform to the point cloud (in place). Args: pc (np.ndarray): an N x M numpy array, with N points and M channels. T (klampt se3 element): a Klamp't se3 element representing the transform to apply. point_channels (list of 3 ints): The channel indices (columns) in pc corresponding to the point data. normal_channels (list of 3 ints): The channels indices(columns) in pc corresponding to the normal data. If this is None or an index is >= M, just ignore. """ N, M = pc.shape assert len(point_channels) == 3 for i in point_channels: assert i < M, "Invalid point_channel" for i in range(N): point_data = pc[i, :] point = point_data[point_channels] point_data[point_channels] = se3.apply(T, point) if normal_channels is not None and normal_channels[0] < M: for i in normal_channels: assert i < M, "Invalid normal_channel" for i in range(N): point_data = pc[i, :] point = point_data[normal_channels] point_data[normal_channels] = so3.apply(T[0], point)
def display(self): T1 = self.taskGen.getDesiredCartesianPose('left',0) T2 = self.taskGen.getDesiredCartesianPose('right',1) baseTransform = self.taskGen.world.robot(0).link(2).getTransform() glEnable(GL_LIGHTING) if T1 is not None: gldraw.xform_widget(se3.mul(baseTransform,T1),0.2,0.03,fancy=True,lighting=True) if T2 is not None: gldraw.xform_widget(se3.mul(baseTransform,T2),0.2,0.03,fancy=True,lighting=True) dstate = self.taskGen.serviceThread.hapticupdater.deviceState if T1 is not None and dstate[0] != None: R = dstate[0].widgetCurrentTransform[0] T = se3.mul(baseTransform,(R,T1[1])) self.draw_wand(T) if T2 is not None and dstate[1] != None: R = dstate[1].widgetCurrentTransform[0] T = se3.mul(baseTransform,(R,T2[1])) self.draw_wand(T) if debugHapticTransform: for deviceState in dstate: #debugging #this is the mapping from the handle to the user frame Traw = (so3.from_moment(deviceState.rotationMoment),deviceState.position) Tuser = se3.mul(se3.inv(handleToUser),Traw) T = se3.mul(userToWorld,se3.mul(Tuser,worldToUser)) T = (T[0],so3.apply([0,-1,0, 0,0,1, -1,0,0],T[1])) T = deviceToViewTransform(Traw[0],Traw[1]) gldraw.xform_widget(se3.mul(se3translation([-1,0,0]),Traw),0.5,0.05,lighting=True,fancy=True) gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),Tuser),0.5,0.05,lighting=True,fancy=True) #gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),se3.mul(Traw,worldToHandle)),0.5,0.05,lighting=True,fancy=True) gldraw.xform_widget(T,0.5,0.05,lighting=True,fancy=True) break
def axis_rotation_magnitude(R, a): """Given a rotation matrix R and a unit axis a, determines how far R rotates about a. Specifically, if R = from_axis_angle((a,v)) this should return v (modulo pi). """ cterm = so3.trace(R) - vectorops.dot(a, so3.apply(R, a)) mR = so3.matrix(R) sterm = -(a[0] * (mR[1][2] - mR[2][1]) + a[1] * (mR[2][0] - mR[0][2]) + a[2] * (mR[0][1] - mR[1][0])) return math.atan2(sterm, cterm)
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 getSpheres(self): for i in range(len(self.obsList)): obsDim = self.obsDimList[i] pos = self.obsList[i] rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]]) obsSphere = so3.apply( rotMat, [obsDim[0] / 2, obsDim[1] / 2, obsDim[2] / 2]) obsSphere = np.array(obsSphere) obsSphere = obsSphere + [pos[0], pos[1], pos[2]] self.spherePosList.append(obsSphere) return self.spherePosList
def checkCollision(self, q): collisionFlag = False # Hardcoded robot dimensions robDim = [0.05, 0.05, 0.05] robSphereRad = self.getRadius(robDim) rotMat = mathUtils.euler_zyx_mat([q[3], q[4], q[5]]) # position is half of the robots dimension - Center of the approximated robot when placed at [0,0,0] robSphere = so3.apply(rotMat, [robDim[0] / 2, robDim[1] / 2, robDim[2] / 2]) robSphere = np.array(robSphere) robSphere = robSphere + [q[0], q[1], q[2]] if len(self.spherePosList) == len(self.obsList): for i in range(len(self.spherePosList)): obsDim = self.obsDimList[i] obsSphereRad = self.getRadius(obsDim) obsSphere = self.spherePosList[i] dist = np.sqrt((obsSphere[0] - robSphere[0])**2 + (obsSphere[1] - robSphere[1])**2 + (obsSphere[2] - robSphere[2])**2) if (dist <= robSphereRad + obsSphereRad + self.epsilon): collisionFlag = True break return collisionFlag for i in range(len(self.obsList)): obsDim = self.obsDimList[i] obsSphereRad = self.getRadius(obsDim) pos = self.obsList[i] rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]]) obsSphere = so3.apply( rotMat, [obsDim[0] / 2, obsDim[1] / 2, obsDim[2] / 2]) obsSphere = np.array(obsSphere) obsSphere = obsSphere + [pos[0], pos[1], pos[2]] dist = np.sqrt((obsSphere[0] - robSphere[0])**2 + (obsSphere[1] - robSphere[1])**2 + (obsSphere[2] - robSphere[2])**2) if (dist <= robSphereRad + obsSphereRad): collisionFlag = True break return collisionFlag
def so3_grid_test(N=5, staggered=True): from klampt import vis from klampt.model import trajectory if staggered: G = so3_staggered_grid(N) else: G = so3_grid(N) vispt = [1, 0, 0] for n in G.nodes(): R = G.node[n]['params'] trans = so3.apply(R, vispt) #trans = so3.axis_angle(R)[0] #trans = vectorops.unit(so3.quaternion(R)[:3]) vis.add(str(n), [R, trans]) vis.hideLabel(str(n)) #draw edges? minDist = float('inf') maxDist = 0.0 for i, j in G.edges(): Ri = G.node[i]['params'] Rj = G.node[j]['params'] tmax = 9 times = range(tmax + 1) milestones = [] for t in times: u = float(t) / tmax trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt) milestones.append(trans) vis.add( str(i) + '-' + str(j), trajectory.Trajectory(times, milestones)) vis.hideLabel(str(i) + '-' + str(j)) dist = so3.distance(Ri, Rj) if dist > maxDist: maxDist = dist print "Max dispersion at", i, j, ":", dist if dist < minDist: minDist = dist print "Number of points:", G.number_of_nodes() print "Min/max dispersion:", minDist, maxDist vis.run()
def euler_zyx_mat(theta): """For the zyx euler angles theta=(rz,ry,rx), produces a matrix A such that A*dtheta is the angular velocities when dtheta is the rate of change of the euler angles""" eu = [0, 0, 1] ev = [0, 1, 0] ew = [1, 0, 0] Ru = so3.rotation([0, 0, 1], theta[0]) Rv = so3.rotation([0, 1, 0], theta[1]) col1 = eu col2 = so3.apply(Ru, ev) col3 = so3.apply(Ru, so3.apply(Rv, ew)) #col1 = [0,0,1] #col2 = [c0 -s0 0] [0] = [-s0] # [s0 c0 0]*[1] [c0 ] # [0 0 1] [0] [0 ] #col3 = Ru*[c1 0 s1] [1] = Ru*[c1 ] = [c1c0] # [0 1 0 ]*[0] [0 ] [c1s0] # [-s1 0 c1] [0] [-s1] [-s1 ] #A = [ 0 -s0 c1c0] # [ 0 c0 c1s0] # [ 1 0 -s1 ] #return [col1[2], col2[2], col3[2], col1[1], col2[1], col3[1], col1[0], col2[0], col3[0]] phi = theta[0] tht = theta[1] psi = theta[2] cphi = math.cos(phi) sphi = math.sin(phi) ctht = math.cos(tht) stht = math.sin(tht) cpsi = math.cos(psi) spsi = math.sin(psi) rotMat = [ cphi * ctht, sphi * cpsi + cphi * stht * spsi, sphi * spsi - cphi * stht * cpsi, -sphi * ctht, cphi * cpsi - sphi * stht * spsi, cphi * spsi + sphi * stht * cpsi, stht, -spsi * ctht, cpsi * ctht ] return rotMat
def euler_zyx_moments(theta): """For the zyx euler angles theta=(rz,ry,rx), produces a matrix A such that A*dtheta is the angular velocities when dtheta is the rate of change of the euler angles""" eu = [0, 0, 1] ev = [0, 1, 0] ew = [1, 0, 0] Ru = so3.rotation([0, 0, 1], theta[0]) Rv = so3.rotation([0, 1, 0], theta[1]) col1 = eu col2 = so3.apply(Ru, ev) col3 = so3.apply(Ru, so3.apply(Rv, ew)) #col1 = [0,0,1] #col2 = [c0 -s0 0] [0] = [-s0] # [s0 c0 0]*[1] [c0 ] # [0 0 1] [0] [0 ] #col3 = Ru*[c1 0 s1] [1] = Ru*[c1 ] = [c1c0] # [0 1 0 ]*[0] [0 ] [c1s0] # [-s1 0 c1] [0] [-s1] [-s1 ] #A = [ 0 -s0 c1c0] # [ 0 c0 c1s0] # [ 1 0 -s1 ] return zip(col1, col2, col3)
def solve_placement(self): """Implemented for you: come up with a collision-free target placement""" obmin,obmax = self.objbb ozmin = obmin[2] min_dims = min(obmax[0]-obmin[0],obmax[1]-obmin[1]) center = [0.5*(obmax[0]+obmin[0]),0.5*(obmax[1]-obmin[1])] xmin,ymin,zmin = self.goal_bounds[0] xmax,ymax,zmax = self.goal_bounds[1] center_sampling_range = [(xmin+min_dims/2,xmax-min_dims/2),(ymin+min_dims/2,ymax-min_dims/2)] Tobj_feasible = [] for iters in range(20): crand = [random.uniform(b[0],b[1]) for b in center_sampling_range] Robj = so3.rotation((0,0,1),random.uniform(0,math.pi*2)) tobj = vectorops.add(so3.apply(Robj,[-center[0],-center[1],0]),[crand[0],crand[1],zmin-ozmin+0.002]) self.object.setTransform(Robj,tobj) feasible = True for i in range(self.world.numTerrains()): if self.object.geometry().collides(self.world.terrain(i).geometry()): feasible=False break if not feasible: bmin,bmax = self.object.geometry().getBBTight() if bmin[0] < xmin: tobj[0] += xmin-bmin[0] if bmax[0] > xmax: tobj[0] -= bmin[0]-xmax if bmin[1] < ymin: tobj[1] += ymin-bmin[1] if bmax[1] > ymax: tobj[1] -= bmin[1]-ymax self.object.setTransform(Robj,tobj) feasible = True for i in range(self.world.numTerrains()): if self.object.geometry().collides(self.world.terrain(i).geometry()): feasible=False break if not feasible: continue for i in range(self.world.numRigidObjects()): if i == self.object.index: continue if self.object.geometry().collides(self.world.rigidObject(i).geometry()): #raise it up a bit bmin,bmax = self.world.rigidObject(i).geometry().getBB() tobj[2] = bmax[2]-ozmin+0.002 self.object.setTransform(Robj,tobj) Tobj_feasible.append((Robj,tobj)) print("Found",len(Tobj_feasible),"valid object placements") return Tobj_feasible
def retract(robot,gripper,amount,local=True): """Retracts the robot's gripper by a vector `amount`. if local=True, amount is given in local coordinates. Otherwise, its given in world coordinates. """ if not isinstance(gripper,(int,str)): gripper = gripper.base_link link = robot.link(gripper) Tcur = link.getTransform() if local: amount = so3.apply(Tcur[0],amount) obj = ik.objective(link,R=Tcur[0],t=vectorops.add(Tcur[1],amount)) res = ik.solve(obj) if not res: return None return robot.getConfig()
def next(self): """Returns the next Grasp from the database.""" if self.options is None: return None if self.index >= len(self.options): self.options = None return None c, n, score = self.options(self.index) self.index += 1 cworld = se3.apply(self.pc_xform, c) nworld = so3.apply(self.pc_xform[0], n) objective = IKObjective() objective.setLinks(self.gripper.link) objective.setFixedPoint(self.gripper.center, cworld) objective.setAxialRotConstraint(self.gripper.primary_axis, vectorops.mul(nworld, -1)) return Grasp(objective, score=score)
def plan_pick_one(world,robot,object,gripper,grasp, robot0=True): """ Plans a picking motion for a given object and a specified grasp. Arguments: world (WorldModel): the world, containing robot, object, and other items that will need to be avoided. robot (RobotModel): the robot in its current configuration object (RigidObjectModel): the object to pick. gripper (GripperInfo): the gripper. grasp (Grasp): the desired grasp. See common/grasp.py for more information. Returns: None or (transit,approach,lift): giving the components of the pick motion. Each element is a RobotTrajectory. (Note: to convert a list of milestones to a RobotTrajectory, use RobotTrajectory(robot,milestones=milestones) Tip: vis.debug(q,world=world) will show a configuration. """ qstart = robot.getConfig() qstartopen = gripper.set_finger_config(qstart, gripper.partway_open_config(1)) # open the fingers of the start to match qpregrasp robot.setConfig(qstartopen) grasp.ik_constraint.robot = robot #this makes it more convenient to use the ik module def feasible(): return is_collision_free_grasp(world, robot, object) # return True solved = ik.solve_global(objectives=grasp.ik_constraint, iters=50, numRestarts=5, activeDofs=[1, 2, 3, 4, 5, 6, 7],feasibilityCheck=feasible) print('ik status:', solved) if not solved: return None qpregrasp = robot.getConfig() robot.setConfig(qpregrasp) if not feasible(): return None qtransit = retract(robot=robot, gripper=gripper, amount=list(-0.1*np.array(gripper.primary_axis)), local=True) # rotate the gripper along its primary axis to make its secondary axis perpendicular to the object # so that it can grasp the object easily secondary_axis_gripper = gripper.secondary_axis if not isinstance(gripper,(int,str)): gripper1 = gripper.base_link else: gripper1 = gripper link = robot.link(gripper1) R_gripper_w, _ = link.getTransform() secondary_axis_world = so3.apply(R_gripper_w, secondary_axis_gripper) secondary_axis_world_2d = np.array(secondary_axis_world)[:-1] angle = np.arccos(np.dot(secondary_axis_world_2d, [0, 1])) q_rotate = copy.deepcopy(qtransit) q_rotate[7] = clip_angle(q_rotate[7] - angle) qpregrasp[7] = clip_angle(qpregrasp[7] - angle) if qtransit is None: return None print(qtransit) robot.setConfig(qtransit) if not feasible(): return None robot.setConfig(qpregrasp) qopen = gripper.set_finger_config(qpregrasp, gripper.partway_open_config(1)) # open the fingers further robot.setConfig(qopen) if not feasible(): return None if robot0: close_amount = 0.1 else: close_amount = 0.8 qgrasp = gripper.set_finger_config(qopen, gripper.partway_open_config(close_amount)) # close the gripper robot.setConfig(qgrasp) qlift = retract(robot=robot, gripper=gripper, amount=[0, 0, 0.1], local=False) robot.setConfig(qlift) if not feasible(): return None robot.setConfig(qstart) transit = feasible_plan(world, robot, object, qtransit) # decide whether to use feasible_plan or optimizing_plan if not transit: return None return RobotTrajectory(robot,milestones=[qstart]+transit),\ RobotTrajectory(robot,milestones=[qtransit, q_rotate, qpregrasp, qopen, qgrasp]),\ RobotTrajectory(robot,milestones=[qgrasp,qlift])
def integrate_velocities(controller, sim, dt): """The make() function returns a 1-argument function that takes a SimRobotController and performs whatever processing is needed when it is invoked.""" global start_time global now_dur global syn_curr global syn_next global entered_once global got_syn if not entered_once: syn_vec = controller.getCommandedConfig() if not syn_vec: got_syn = False entered_once = False else: got_syn = True entered_once = True syn_curr = syn_vec[34] start_time = sim.getTime() else: syn_curr = syn_next now_dur = sim.getTime() - start_time # print 'The current simulation duration is', now_dur rob = sim.controller(0).model() palm_curr = mv_el.get_moving_base_xform(rob) R = palm_curr[0] t = palm_curr[1] if DEBUG: print 'The current palm rotation is ', R print 'The current palm translation is ', t print 'The simulation dt is ', dt # Converting to rpy euler = so3.rpy(R) # Checking if list empty and returning false if not got_syn: return False, None, None if DEBUG: print 'The integration time is ', dt print 'The adaptive velocity of hand is ', global_vars.hand_command print 'The adaptive twist of palm is \n', global_vars.arm_command # print 'The commanded position of the hand encoder (in memory) is ', syn_curr # print 'The present position of the hand encoder (sensed) is ', controller.getCommandedConfig()[34] v = rob.getVelocity() print 'The actual velocity of the robot is ', v # print 'The present pose of the palm is \n', palm_curr # print 'The present position of the palm is ', t, 'and its orientation is', euler # Getting linear and angular velocities lin_vel_vec = global_vars.arm_command.linear ang_vel_vec = global_vars.arm_command.angular lin_vel = [lin_vel_vec.x, lin_vel_vec.y, lin_vel_vec.z] ang_vel = [ang_vel_vec.x, ang_vel_vec.y, ang_vel_vec.z] ang_vel_loc = so3.apply(so3.inv(R), ang_vel) # rotating ang vel to local frame # Transform ang vel into rpy vel euler_vel = transform_ang_vel(euler, ang_vel_loc) palm_vel = [] palm_vel.extend(lin_vel) palm_vel.extend([euler_vel[0], euler_vel[1], euler_vel[2]]) # appending syn_vel = global_vars.hand_command # Integrating syn_next = syn_curr + syn_vel * int_const_syn * dt t_next = vectorops.madd(t, lin_vel, int_const_t * dt) euler_next = vectorops.madd(euler, euler_vel, int_const_eul * dt) # Saturating synergy if syn_next >= 1.0: syn_next = 1.0 # Convert back for send xform palm_R_next = so3.from_rpy(euler_next) palm_t_next = t_next palm_next = (palm_R_next, palm_t_next) if DEBUG or True: print 'euler is ', euler, ' and is of type ', type(euler) print 'euler_vel is ', euler_vel, ' and is of type ', type(euler_vel) print 'euler_next is ', euler_next, ' and is of type ', type( euler_next) # # print 't is ', t, ' and is of type ', type(t) # print 't_next is ', t_next, ' and is of type ', type(t_next) # # print 'R is ', R, ' and is of type ', type(R) # print 'palm_R_next is ', palm_R_next, ' and is of type ', type(palm_R_next) # # print 'palm_curr is ', palm_curr, ' and is of type ', type(palm_curr) # print 'palm_next is ', palm_next, ' and is of type ', type(palm_next) return True, syn_next, palm_next, syn_vel, palm_vel
def stable_faces(obj, com=None, stability_tol=0.0, merge_tol=0.0): """ Returns a list of support polygons on the object that are likely to be stable on a planar surface. Args: obj (RigidObjectModel or Geometry3D): the object. com (3-list, optional): sets the local center of mass. If not given, the default RigidObjectModel's COM will be used, or (0,0,0) will be used for a Geometry3D. stability_tol (float,optional): if > 0, then only faces that are stable with all perturbed "up" directions (dx,dy,1) with ||(dx,dy)||<= normal_tol will be outputted (robust stability). If < 0, then all faces that are stable from some "up" direction (dx,dy,1) with ||(dx,dy)||<= |normal_tol| will be outputted (non-robust stability) merge_tol (float, optional): if > 0, then adjacent faces with normals whose angles are within this tolerance (in rads) will be merged together. Returns: list of list of 3-vectors: The set of all polygons that could form stable sides. Each polygon is convex and listed in counterclockwise order (i.e., the outward normal can be obtained via: (poly[2]-poly[0]) x (poly[1]-poly[0]) """ if isinstance(obj, RigidObjectModel): geom = obj.geometry() if com is None: com = obj.getMass().getCom() else: geom = obj if com is None: com = (0, 0, 0) assert len(com) == 3, "Need to provide a 3D COM" ch_trimesh = geom.convert('ConvexHull').convert('TriangleMesh') xform, (verts, tris) = numpy_convert.to_numpy(ch_trimesh) trinormals = get_triangle_normals(verts, tris) edges = dict() tri_neighbors = np.full(tris.shape, -1, dtype=np.int32) for ti, tri in enumerate(tris): for ei, e in enumerate([(tri[0], tri[1]), (tri[1], tri[2]), (tri[2], tri[0])]): if (e[1], e[0]) in edges: tn, tne = edges[(e[1], e[0])] if tri_neighbors[tn][tne] >= 0: print("Warning, triangle", ti, "neighbors two triangles on edge", tne, "?") tri_neighbors[ti][ei] = tn tri_neighbors[tn][tne] = ti else: edges[e] = ti, ei num_empty_edges = 0 for ti, tri in enumerate(tris): for e in range(3): if tri_neighbors[tn][e] < 0: num_empty_edges += 1 if num_empty_edges > 0: print("Info: boundary of mesh has", num_empty_edges, "edges") visited = [False] * len(tris) cohesive_faces = dict() for ti, tri in enumerate(tris): if visited[ti]: continue face = [ti] visited[ti] = True myvisit = set() myvisit.add(ti) q = deque() q.append(ti) while q: tvisit = q.popleft() for tn in tri_neighbors[tvisit]: if tn >= 0 and tn not in myvisit: if math.acos(trinormals[ti].dot( trinormals[tn])) <= merge_tol: face.append(tn) myvisit.add(tn) q.append(tn) for t in myvisit: visited[t] = True cohesive_faces[ti] = face output = [] for t, face in cohesive_faces.items(): n = trinormals[t] R = so3.canonical(n) if len(face) > 1: #project face onto the canonical basis faceverts = set() for t in face: faceverts.add(tris[t][0]) faceverts.add(tris[t][1]) faceverts.add(tris[t][2]) faceverts = list(faceverts) xypts = [so3.apply(so3.inv(R), verts[v])[1:3] for v in faceverts] try: ch = ConvexHull(xypts) face = [faceverts[v] for v in ch.vertices] except Exception: print("Error computing convex hull of", xypts) print("Vertex indices", faceverts) print("Vertices", [verts[v] for v in faceverts]) else: face = tris[face[0]] comproj = np.array(so3.apply(so3.inv(R), com)[1:3]) stable = True for vi in range(len(face)): vn = (vi + 1) % len(face) a, b = face[vi], face[vn] pa = np.array(so3.apply(so3.inv(R), verts[a])[1:3]) pb = np.array(so3.apply(so3.inv(R), verts[b])[1:3]) #check distance from com elen = np.linalg.norm(pb - pa) if elen == 0: continue sign = np.cross(pb - pa, comproj - pa) / elen if sign < stability_tol: stable = False break if stable: output.append([verts[i] for i in face]) return output
sim = Simulator(w) robot = w.robot(0) cam = sim.controller(0).sensor("rgbd_camera") link = robot.link(robot.numLinks() - 1) amplitudes = [random.uniform(0, 2) for i in range(robot.numLinks())] phases = [random.uniform(0, math.pi * 2) for i in range(robot.numLinks())] periods = [random.uniform(0.5, 2) for i in range(robot.numLinks())] qmin, qmax = robot.getJointLimits() vis.add("world", w) vis.add("cam", cam) vp = vis.getViewport() vp.camera.rot[1] -= 0.5 vis.setViewport(vp) default = vis.getViewport().getTransform() print('x:', so3.apply(default[0], [1, 0, 0])) print('y:', so3.apply(default[0], [0, 1, 0])) print('z:', so3.apply(default[0], [0, 0, 1])) print('offset:', default[1]) circle_points = [] npts = 50 times = [] for i in range(npts + 1): angle = math.radians(360 * i / npts) circle_points.append( se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default)) times.append(i * 20 / npts) circle_traj = SE3Trajectory(times, circle_points) circle_traj.milestones[-1] = circle_traj.milestones[0] circle_smooth_traj = SE3HermiteTrajectory() circle_smooth_traj.makeSpline(circle_traj, loop=True)
def make_grasp_map(world, id): """Estimates a grasp quality and regression map for a sensor image. Input: world (WorldModel): contains a robot and a sensor Output: a 4-channel numpy array of size (w,h,3) with w x h the sensor dimensions and all values in the range [0,1]. The first channel encodes the quality. The second encodes the grasp opening amount. The third and fourth encode the orientation of the grasp relative to the camera frame, as a heading and elevation. Make sure to note how you've transformed these quantities to [0,1]! These will be needed for decoding. """ robot = world.robot(0) sensor = robot.sensor(0) sensor_xform = sensing.get_sensor_xform(sensor, robot) w = int(sensor.getSetting("xres")) h = int(sensor.getSetting("yres")) #You'll be filling this image out result = np.zeros((h, w, 4)) result[:, :, 3] = 0.5 #this shows how to go from a point in space to a pixel # point = (0,0,0) # projected = sensing.camera_project(sensor,robot,point) # if projected is None: # pass # else: # x,y,z = projected # if x < 0 or x > w or y < 0 or y > h: # pass # else: # result[int(y),int(x),0]=1 # result[50,10,0]=1 #load the gripper info and grasp database source_gripper = robotiq_85 global gripper_robot, gripper_world, grasp_db if gripper_robot is None: gripper_world = WorldModel() if not gripper_world.readFile(source_gripper.klampt_model): raise ValueError("Unable to read gripper file " + source_gripper.klampt_model) gripper_robot = gripper_world.robot(0) gripper_geom_open = source_gripper.get_geometry(gripper_robot, source_gripper.open_config) if grasp_db is None: db = grasp_database.GraspDatabase(source_gripper) if not db.load("../data/grasps/robotiq_85_more_sampled_grasp_db.json"): raise RuntimeError("Can't load grasp database?") grasp_db = db for i in range(world.numRigidObjects()): obj = world.rigidObject(i) ycb_obj = obj.getName() if ycb_obj not in grasp_db.object_to_grasps: print("Can't find object", ycb_obj, "in database") print("Database only contains objects", grasp_db.objects) raise ValueError() grasps = grasp_db.object_to_grasps[ycb_obj] gripper_tip = vectorops.madd(source_gripper.center, source_gripper.primary_axis, source_gripper.finger_length) num_grasps_valid = 0 for gindex, g in enumerate(grasps): #TODO: problem 1B: do collision testing of gripper_geom found_approach = False if (id, ycb_obj, gindex) in grasps_feasible: found_approach = grasps_feasible[(id, ycb_obj, gindex)] else: #this is the Geometry3D corresponding to the robot at the opening configuration gripper_geom = source_gripper.get_geometry( gripper_robot, g.finger_config) local_pt, world_pt = g.ik_constraint.getPosition() local_axis, world_axis = g.ik_constraint.getRotationAxis() #TODO: put your code here R = so3.vector_rotation(source_gripper.primary_axis, world_axis) t = vectorops.sub(world_pt, source_gripper.center) gripper_geom_open.setCurrentTransform(R, t) non_collision = True for i in range(world.numRigidObjects()): if i == gripper_robot.getID(): continue if gripper_geom_open.collides( world.rigidObject(i).geometry()): non_collision = False break found_approach = True if non_collision else False if found_approach: gripper_geom.setCurrentTransform(R, t) for i in range(world.numRigidObjects()): if i == gripper_robot.getID() or i == obj.getID(): continue if gripper_geom.collides( world.rigidObject(i).geometry()): found_approach = False break grasps_feasible[(id, ycb_obj, gindex)] = found_approach if not found_approach: continue num_grasps_valid += 1 Tfixed = g.ik_constraint.closestMatch(*se3.identity()) Tworld = se3.mul(obj.getTransform(), Tfixed) gworld = se3.apply(Tworld, gripper_tip) projected = sensing.camera_project(sensor, robot, gworld) if projected is not None: x, y, z = projected x = int(x) y = int(y) if x < 0 or x >= w or y < 0 or y >= h: continue gripper_opening_axis = so3.apply(Tworld[0], source_gripper.secondary_axis) gripper_opening_axis_cam = so3.apply(so3.inv(sensor_xform[0]), gripper_opening_axis) if gripper_opening_axis_cam[1] < 0: gripper_opening_axis_cam = vectorops.mul( gripper_opening_axis_cam, -1) gripper_axis_heading = math.atan2(gripper_opening_axis_cam[1], gripper_opening_axis_cam[0]) xy = vectorops.norm(gripper_opening_axis_cam[:2]) if xy < 1e-7: gripper_axis_elevation = math.pi * 0.5 else: gripper_axis_elevation = math.atan( gripper_opening_axis_cam[2] / xy) score0 = math.exp(-g.score) window = 10 std = 5 for u in range(-window, window + 1): if y + u < 0 or y + u >= h: continue for v in range(-window, window + 1): if x + v < 0 or x + v >= w: continue score = score0 * math.exp(-(u**2 + v**2) / (2 * std**2)) if score > result[y + u, x + v, 0]: result[y + u, x + v, :] = [ score, source_gripper.config_to_opening( g.finger_config), gripper_axis_heading / math.pi, gripper_axis_elevation / math.pi + 0.5 ] print("{}/{} grasps valid for object {}".format( num_grasps_valid, len(grasps), ycb_obj)) return result
def viewToDeviceTransform(R, t): Ruser = so3.mul(worldToUser[0], so3.mul(R, userToWorld[0])) R = so3.mul(handleToUser[0], Ruser) return (R, so3.apply(so3.inv([0, 1, 0, 0, 0, 1, 1, 0, 0]), t))
def deviceToViewTransform(R, t): Ruser = so3.mul(so3.inv(handleToUser[0]), R) R = so3.mul(userToWorld[0], so3.mul(Ruser, worldToUser[0])) return (R, so3.apply([0, 1, 0, 0, 0, 1, 1, 0, 0], t))
def drive(self, qcur, angVel, vel, dt): """Drives the robot by an incremental time step to reach the desired Cartesian (angular/linear) velocities of the links previously specified in start(). Args: qcur (list of floats): The robot's current configuration. angVel (3-vector or list of 3-vectors): the angular velocity of each driven link. Set angVel to None to turn off orientation control of every constraint. angVel[i] to None to turn off orientation control of constraint i. vel (3-vector or list of 3-vectors): the linear velocity of each driven link. Set vel to None to turn off position control of every constraint. Set vel[i] to None to turn off position control of constraint i. dt (float): the time step. Returns: tuple: A pair ``(progress,qnext)``. ``progress`` gives a value in the range [0,1] indicating indicating how far along the nominal drive amount the solver was able to achieve. If the result is < 0, this indicates that the solver failed to make further progress. ``qnext`` is the resulting configuration that should be sent to the robot's controller. For longer moves, you should pass qnext back to this function as qcur. """ if angVel is None: #turn off orientation control if vel is None: #nothing specified return (1.0, qcur) angVel = [None] * len(self.links) else: assert len(angVel) > 0 if not hasattr(angVel[0], '__iter__'): angVel = [angVel] if vel is None: #turn off position control vel = [None] * len(self.links) else: assert len(vel) > 0 if not hasattr(vel[0], '__iter__'): vel = [vel] if len(qcur) != self.robot.numLinks(): raise ValueError( "Invalid size of current configuration, %d != %d" % (len(qcur), self.robot.numLinks())) if len(angVel) != len(self.links): raise ValueError( "Invalid # of angular velocities specified, %d != %d" % (len(angVel), len(self.links))) if len(vel) != len(self.links): raise ValueError( "Invalid # of translational velocities specified, %d != %d" % (len(vel), len(self.links))) anyNonzero = False zeroVec = [0, 0, 0] for (w, v) in zip(angVel, vel): if w is not None and list(w) != zeroVec: anyNonzero = True break if v is not None and list(v) != zeroVec: anyNonzero = True break if not anyNonzero: return (1.0, qcur) qout = [v for v in qcur] #update drive transforms self.robot.setConfig(qcur) robotLinks = [self.robot.link(l) for l in self.links] #limit the joint movement by joint limits and velocity tempqmin, tempqmax = self.robot.getJointLimits() if self.qmin is not None: tempqmin = self.qmin if self.qmax is not None: tempqmax = self.qmax vmax = self.robot.getVelocityLimits() vmin = vectorops.mul(vmax, -1) if self.vmin is not None: vmin = self.vmin if self.vmax is not None: vmax = self.vmax for i in range(self.robot.numLinks()): tempqmin[i] = max(tempqmin[i], qcur[i] + dt * vmin[i]) tempqmax[i] = min(tempqmax[i], qcur[i] + dt * vmax[i]) #Setup the IK solver parameters self.solver.setJointLimits(tempqmin, tempqmax) tempGoals = [ik.IKObjective(g) for g in self.ikGoals] for i in range(len(self.links)): if math.isfinite(self.positionTolerance) and math.isfinite( self.rotationTolerance): tempGoals[i].rotationScale = self.positionTolerance / ( self.positionTolerance + self.rotationTolerance) tempGoals[i].positionScale = self.rotationTolerance / ( self.positionTolerance + self.rotationTolerance) elif not math.isfinite( self.positionTolerance) and not math.isfinite( self.rotationTolerance): pass else: tempGoals[i].rotationScale = min( self.positionTolerance, self.rotationTolerance) / self.rotationTolerance tempGoals[i].positionScale = min( self.positionTolerance, self.rotationTolerance) / self.positionTolerance tolerance = min( 1e-6, min(self.positionTolerance, self.rotationTolerance) / (math.sqrt(3.0) * len(self.links))) self.solver.setTolerance(tolerance) #want to solve max_t s.t. there exists q satisfying T_i(q) = TPath_i(t) #where TPath_i(t) = Advance(Tdrive_i,Screw_i*t) if self.driveSpeedAdjustment == 0: self.driveSpeedAdjustment = 0.1 anyFailures = False while self.driveSpeedAdjustment > 0: #advance the desired cartesian goals #set up IK parameters: active dofs, IKGoal amount = dt * self.driveSpeedAdjustment desiredTransforms = [[None, None] for i in range(len(self.links))] for i in range(len(self.links)): if vel[i] is not None: desiredTransforms[i][1] = vectorops.madd( self.driveTransforms[i][1], vel[i], amount) tempGoals[i].setFixedPosConstraint( self.endEffectorOffsets[i], desiredTransforms[i][1]) else: tempGoals[i].setFreePosition() if angVel[i] is not None: increment = so3.from_moment( vectorops.mul(angVel[i], amount)) desiredTransforms[i][0] = so3.mul( increment, self.driveTransforms[i][0]) tempGoals[i].setFixedRotConstraint(desiredTransforms[i][0]) else: tempGoals[i].setFreeRotConstraint() self.solver.set(i, tempGoals[i]) err0 = self.solver.getResidual() quality0 = vectorops.norm(err0) res = self.solver.solve() q = self.robot.getConfig() activeDofs = self.solver.getActiveDofs() for k in activeDofs: if q[k] < tempqmin[k] or q[k] > tempqmax[k]: #the IK solver normalizer doesn't care about absolute #values for joints that wrap around 2pi if tempqmin[k] <= q[k] + 2 * math.pi and q[ k] + 2 * math.pi <= tempqmax[k]: q[k] += 2 * math.pi elif tempqmin[k] <= q[k] - 2 * math.pi and q[ k] - 2 * math.pi <= tempqmax[k]: q[k] -= 2 * math.pi else: print( "CartesianDriveSolver: Warning, result from IK solve is out of bounds: index", k, ",", tempqmin[k], "<=", q[k], "<=", tempqmax[k]) q[k] = max(min(q[k], tempqmax[k]), tempqmin[k]) self.robot.setConfig(q) #now evaluate quality of the solve errAfter = self.solver.getResidual() qualityAfter = vectorops.norm(errAfter) if qualityAfter > quality0: #print("CartesianDriveSolver: Solve failed: original configuration was better:",quality0,"vs",qualityAfter) #print(" solver result was",res,"increment",amount) res = False elif qualityAfter < quality0 - 1e-8: res = True if res: #success! for k in activeDofs: qout[k] = q[k] assert tempqmin[k] <= q[k] and q[k] <= tempqmax[k] #now advance the driven transforms #figure out how much to drive along screw numerator = 0 # this will get sum of distance * screws denominator = 0 # this will get sum of |screw|^2 for all screws #result will be numerator / denominator achievedTransforms = [ (l.getTransform()[0], l.getWorldPosition(ofs)) for l, ofs in zip(robotLinks, self.endEffectorOffsets) ] #TODO: get transforms relative to baseLink for i in range(len(self.links)): if vel[i] is not None: trel = vectorops.sub(achievedTransforms[i][1], self.driveTransforms[i][1]) vellen = vectorops.norm(vel[i]) axis = vectorops.div(vel[i], max(vellen, 1e-8)) ut = vellen tdistance = vectorops.dot(trel, axis) tdistance = min(max(tdistance, 0.0), dt * vellen) numerator += ut * tdistance denominator += ut**2 if angVel[i] is not None: Rrel = so3.mul(achievedTransforms[i][0], so3.inv(self.driveTransforms[i][0])) vellen = vectorops.norm(angVel[i]) angVelRel = so3.apply( so3.inv(self.driveTransforms[i][0]), angVel[i]) rotaxis = vectorops.div(angVelRel, max(vellen, 1e-8)) Rdistance = axis_rotation_magnitude(Rrel, rotaxis) Rdistance = min(max(Rdistance, 0.0), dt * vellen) uR = vellen numerator += uR * Rdistance denominator += uR**2 distance = numerator / max(denominator, 1e-8) #computed error-minimizing distance along screw motion for i in range(len(self.links)): if vel[i] is not None: self.driveTransforms[i][1] = vectorops.madd( self.driveTransforms[i][1], vel[i], distance) else: self.driveTransforms[i][1] = achievedTransforms[i][1] if angVel[i] is not None: Rincrement = so3.from_moment( vectorops.mul(angVel[i], distance)) self.driveTransforms[i][0] = normalize_rotation( so3.mul(Rincrement, self.driveTransforms[i][0])) else: self.driveTransforms[i][0] = achievedTransforms[i][0] if math.ceil(distance / dt * 10) < math.floor( self.driveSpeedAdjustment * 10): self.driveSpeedAdjustment -= 0.1 self.driveSpeedAdjustment = max(self.driveSpeedAdjustment, 0.0) elif not anyFailures: #increase drive velocity if self.driveSpeedAdjustment < 1.0: self.driveSpeedAdjustment += 0.1 self.driveSpeedAdjustment = min( 1.0, self.driveSpeedAdjustment) return (distance / dt, qout) else: #IK failed: try again with a slower speed adjustment anyFailures = True self.driveSpeedAdjustment -= 0.1 if self.driveSpeedAdjustment <= 1e-8: self.driveSpeedAdjustment = 0.0 break #no progress return (-1, qcur)