def __init__(self): self.reset() self.color = [-1, -1, -1] #TODO: fill this in with your own camera model, if you wish self.Tsensor = None cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0] self.w, self.h = 320, 240 self.fov = 90 self.dmax = 7 if event == 'A': #at goal post, pointing a bit up and to the left self.Tsensor = (so3.mul( so3.rotation([0, 0, 1], 0.20), so3.mul(so3.rotation([0, -1, 0], 0.25), cameraRot)), [-2.55, -1.1, 0.25]) elif event == 'B': #on ground near robot, pointing up and slightly to the left self.Tsensor = (so3.mul( so3.rotation([1, 0, 0], -0.10), so3.mul(so3.rotation([0, -1, 0], math.radians(90)), cameraRot)), [-1.5, -0.5, 0.25]) self.w = 640 self.h = 480 self.dmax = 10 else: #on ground near robot, pointing to the right self.Tsensor = (cameraRot, [-1.5, -0.5, 0.25]) self.dt = 0.02 return
def __init__(self): cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0] #at goal post, pointing a bit up and to the left self.Tsensor = (so3.mul( so3.rotation([0, 0, 1], 0.20), so3.mul(so3.rotation([0, -1, 0], 0.25), cameraRot)), [-2.55, -1.1, 0.25]) self.T_r = np.matrix(self.Tsensor[0]).reshape((3, 3)).T self.T_t = np.matrix(self.Tsensor[1]).reshape((3, 1)) self.fov = 90 self.w, self.h = 320, 240 self.dmax = 5 self.dt = 0.02 self.f = (0.5 * self.w) / np.tan(self.fov / 2.0 / 180 * np.pi) # Kalman Filter parameters self.A = np.vstack((np.hstack((np.zeros( (3, 3)), np.eye(3))), np.zeros((3, 6)))) self.dyn_noise = np.vstack((np.hstack( (np.eye(3) * (0.5 * self.dt**2 * 0.008)**2, np.zeros( (3, 3)))), np.hstack((np.zeros( (3, 3)), np.eye(3) * 0.008)))) # covariance self.F = np.eye(self.A.shape[0]) + self.A * self.dt self.g = np.vstack((np.zeros((5, 1)), np.matrix([-GRAVITY]))) * self.dt self.H_jaccobian = None # use calJaccobian to calculate this when there is some estimated state to use self.obs_noise = np.diag([0.25, 0.25, 0.5, 0.5]) # prior self.previous_estimates = dict() self.previous_cov = dict() 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 euler_angle_to_rotation(ea,convention='zyx'): """Converts an euler angle representation to a rotation matrix. Can use arbitrary axes specified by the convention arguments (default is 'zyx', or roll-pitch-yaw euler angles). Any 3-letter combination of 'x', 'y', and 'z' are accepted. """ axis_names_to_vectors = dict([('x',(1,0,0)),('y',(0,1,0)),('z',(0,0,1))]) axis0,axis1,axis2=convention R0 = so3.rotation(axis_names_to_vectors[axis0],ea[0]) R1 = so3.rotation(axis_names_to_vectors[axis1],ea[1]) R2 = so3.rotation(axis_names_to_vectors[axis2],ea[2]) return so3.mul(R0,so3.mul(R1,R2))
def main(): w = WorldModel() w.readFile("../data/robots/kinova_gen3_7dof.urdf") robot = w.robot(0) #put a "pen" geometry on the end effector gpen = Geometry3D() res = gpen.loadFile("../data/objects/cylinder.off") assert res gpen.scale(0.01,0.01,0.15) gpen.rotate(so3.rotation((0,1,0),math.pi/2)) robot.link(7).geometry().setCurrentTransform(*se3.identity()) gpen.transform(*robot.link(8).getParentTransform()) robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry())) robot.setConfig(robot.getConfig()) trajs = get_paths() #place on a reasonable height and offset tableh = 0.1 for traj in trajs: for m in traj.milestones: m[0] = m[0]*0.4 + 0.35 m[1] = m[1]*0.4 m[2] = tableh if len(m) == 6: m[3] *= 0.4 m[4] *= 0.4 return run_cartesian(w,trajs)
def __init__(self): self.reset() #TODO: fill this in with your own camera model, if you wish self.Tsensor = None cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0] self.w, self.h = 320, 240 self.fov = 90 self.dmax = 5 self.dt = 0.02 if event == 'A': #at goal post, pointing a bit up and to the left self.Tsensor = (so3.mul( so3.rotation([0, 0, 1], 0.20), so3.mul(so3.rotation([0, -1, 0], 0.25), cameraRot)), [-2.55, -1.1, 0.25]) # Kalman Filter parameters self.A = np.vstack((np.hstack((np.zeros( (3, 3)), np.eye(3))), np.zeros((3, 6)))) self.dyn_noise = np.vstack((np.hstack( (np.eye(3) * (0.5 * self.dt**2 * 0.008)**2, np.zeros( (3, 3)))), np.hstack((np.zeros( (3, 3)), np.eye(3) * 0.008)))) # covariance self.F = np.eye(self.A.shape[0]) + self.A * self.dt self.g = np.vstack((np.zeros( (5, 1)), np.matrix([-GRAVITY]))) * self.dt self.H_jaccobian = None # use calJaccobian to calculate this when there is some estimated state to use self.obs_noise = np.diag([0.25, 0.25, 0.5, 0.5]) elif event == 'B': #on ground near robot, pointing up and slightly to the left self.Tsensor = (so3.mul( so3.rotation([1, 0, 0], -0.10), so3.mul(so3.rotation([0, -1, 0], math.radians(90)), cameraRot)), [-1.5, -0.5, 0.25]) self.w = 640 self.h = 480 self.dmax = 10 else: #on ground near robot, pointing to the right self.Tsensor = (cameraRot, [-1.5, -0.5, 0.25]) self.T_r = np.matrix(self.Tsensor[0]).reshape((3, 3)).T self.T_t = np.matrix(self.Tsensor[1]).reshape((3, 1)) self.f = (0.5 * self.w) / np.tan(self.fov / 2.0 / 180 * np.pi) # prior self.previous_estimates = dict() self.previous_cov = dict() return
def interpolate_rotation(R1, R2, u): """Interpolate linearly between the two rotations R1 and R2. TODO: the current implementation doesn't work properly. Why? """ m1 = so3.moment(R1) m2 = so3.moment(R2) mu = interpolate_linear(m1, m2, u) angle = vectorops.norm(mu) axis = vectorops.div(mu, angle) return so3.rotation(axis, angle)
def getWall(dimX, dimY, dimZ, pos = [0, 0, 0], rotZ = 0): ## Get the wall geometry wall = Geometry3D() wall.loadFile("cube.off") # The wall is based upon cube primitive of unit dimension ## Not sure why the scaling is done through the rotation matrix, works though! #wall.transform([dimX, 0, 0, 0, dimY, 0, 0, 0, dimZ], pos) wall.scale(dimX, dimY, dimZ) rotMat = so3.rotation((0, 0, 1), math.radians(rotZ)) wall.transform(rotMat, pos) return wall
def xy_randomize(obj,bmin,bmax): R,t = obj.getTransform() obmin,obmax = obj.geometry().getBB() w = 0.5*(obmax[0]-obmin[0]) h = 0.5*(obmax[1]-obmin[1]) correction = max(w,h) R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R) t[0] = random.uniform(bmin[0]+correction,bmax[0]-correction) t[1] = random.uniform(bmin[1]+correction,bmax[1]-correction) obj.setTransform(R,t)
def xy_randomize(obj,bmin,bmax): """Randomizes the xy position and z orientation of an object inside of a bounding box bmin->bmax. Assumes the bounding box is large enough to hold the object in any orientation. """ R,t = obj.getTransform() R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R) obj.setTransform(R,t) obmin,obmax = obj.geometry().getBB() t[0] = random.uniform(bmin[0]-obmin[0],bmax[0]-obmax[0]) t[1] = random.uniform(bmin[1]-obmin[1],bmax[1]-obmax[1]) obj.setTransform(R,t)
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 xyz_rpy(xyz=None, rpy=None): xyz = xyz or [0, 0, 0] rpy = rpy or [0, 0, 0] Rall = None for (i, angle) in enumerate(rpy): axis = [0, 0, 0] axis[i] = 1 R = so3.rotation(axis, angle) if Rall: Rall = so3.mul(Rall, R) else: Rall = R return klampt2numpy((Rall, xyz))
def main(): w = WorldModel() w.readFile("../data/robots/kinova_gen3_7dof.urdf") robot = w.robot(0) #put a "pen" geometry on the end effector gpen = Geometry3D() res = gpen.loadFile("../data/objects/cylinder.off") assert res gpen.scale(0.01,0.01,0.15) gpen.rotate(so3.rotation((0,1,0),math.pi/2)) robot.link(7).geometry().setCurrentTransform(*se3.identity()) gpen.transform(*robot.link(8).getParentTransform()) robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry())) robot.setConfig(robot.getConfig()) return run_simulation(w)
def set_pos_on_palm(self, link_idx, pos, tilted_angle): ''' change the position of link mounted on the palm to x, y :param pos: tuple value (r, theta) on palm coordinate :param link_idx: indicate link :return: ''' if np.abs(tilted_angle) > 30: return print('pass') x = pos[0] * np.cos(pos[1] / 180 * np.pi) y = pos[0] * np.sin(pos[1] / 180 * np.pi) p_T = [x, y, 0] angle = np.arctan2(x, y) R_1 = so3.rotation([1, 0, 0], angle + math.radians(tilted_angle)) R_2 = (0, 0, -1, 0, -1, 0, -1, 0, 0) p_R = so3.mul(R_2, R_1) self.robot.link(link_idx).setParentTransform(p_R, p_T)
def find_target(self): self.current_target=self.waiting_list[0] best_p=self.sim.world.rigidObject(self.current_target).getTransform()[1] self.current_target_pos=best_p for i in self.waiting_list: p=self.sim.world.rigidObject(i).getTransform()[1] # print i,p[2],vectorops.distance([0,0],[p[0],p[1]]) if i not in self.failedpickup: if p[2]>best_p[2]+0.05: self.current_target=i self.current_target_pos=p # print 'higher is easier!' best_p=p elif p[2]>best_p[2]-0.04 and vectorops.distance([0,0],[p[0],p[1]])<vectorops.distance([0,0],[best_p[0],best_p[1]]): self.current_target=i self.current_target_pos=p best_p=p elif self.current_target in self.failedpickup: self.current_target=i self.current_target_pos=p best_p=p d_y=-0.02 face_down=face_down_forward self.tooclose = False; if best_p[1]>0.15: d_y=-0.02 face_down=face_down_backward self.tooclose = True; print 'too close to the wall!!' elif best_p[1]<-0.15: d_y=0.02 print best_p self.tooclose = True; print 'too close to the wall!!' #here is hardcoding the best relative position for grasp the ball target=(face_down,vectorops.add(best_p,[0,d_y,0.14])) if self.tooclose: if best_p[0]<0: self.boxside=boxleft aready_pos=start_pos_left else: self.boxside=boxright aready_pos=start_pos_right aready_pos[1][0]=best_p[0] balllocation = best_p handballdiff = vectorops.sub(aready_pos[1],best_p) axis = vectorops.unit(vectorops.cross(handballdiff,aready_pos[1])) axis = (1,0,0) if best_p[1]>0: axis=(-1,0,0) angleforaxis = -1*math.acos(vectorops.dot(aready_pos[1],handballdiff)/vectorops.norm(aready_pos[1])/vectorops.norm(handballdiff)) angleforaxis = so2.normalize(angleforaxis) #if angleforaxis>math.pi: # angleforaxis=angleforaxis-2*math.pi adjR = so3.rotation(axis, angleforaxis) print balllocation print vectorops.norm(aready_pos[1]),vectorops.norm(handballdiff),angleforaxis target=(adjR,vectorops.add(best_p,vectorops.div(vectorops.unit(handballdiff),5))) # print self.current_target # print 'find target at:',self.current_target_pos return target
return rgb,depth world = klampt.WorldModel() world.readFile("../../data/simulation_test_worlds/sensortest.xml") #world.readFile("../../data/tx90scenario0.xml") robot = world.robot(0) vis.add("world",world) sim = klampt.Simulator(world) sensor = sim.controller(0).sensor("rgbd_camera") print("sensor.getSetting('link'):",sensor.getSetting("link")) print("sensor.getSetting('Tsensor'):",sensor.getSetting("Tsensor")) input("Press enter to continue...") #T = (so3.sample(),[0,0,1.0]) T = (so3.mul(so3.rotation([1,0,0],math.radians(-10)),[1,0,0, 0,0,-1, 0,1,0]),[0,-2.0,0.5]) sensing.set_sensor_xform(sensor,T,link=-1) vis.add("sensor",sensor) read_local = True #Note: GLEW sensor simulation only runs if it occurs in the visualization thread (e.g., the idle loop) class SensorTestWorld (GLPluginInterface): def __init__(self): GLPluginInterface.__init__(self) robot.randomizeConfig() #sensor.kinematicSimulate(world,0.01) sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7) self.rgb=None
def deviceToWidgetTransform(R, t): """Given a device transform, map to the widget transform world coordinates.""" Tview = so3.rotation((0, 0, 1), math.pi), [0, 0, 0] return se3.mul(Tview, deviceToViewTransform(R, t))
world = klampt.WorldModel() world.readFile("../../data/simulation_test_worlds/sensortest.xml") #world.readFile("../../data/tx90scenario0.xml") robot = world.robot(0) vis.add("world", world) sim = klampt.Simulator(world) sensor = sim.controller(0).sensor("rgbd_camera") print("sensor.getSetting('link'):", sensor.getSetting("link")) print("sensor.getSetting('Tsensor'):", sensor.getSetting("Tsensor")) input("Press enter to continue...") #T = (so3.sample(),[0,0,1.0]) T = (so3.mul(so3.rotation([1, 0, 0], math.radians(-10)), [1, 0, 0, 0, 0, -1, 0, 1, 0]), [0, -2.0, 0.5]) sensing.set_sensor_xform(sensor, T, link=-1) vis.add("sensor", sensor) read_local = True #Note: GLEW sensor simulation only runs if it occurs in the visualization thread (e.g., the idle loop) class SensorTestWorld(GLPluginInterface): def __init__(self): GLPluginInterface.__init__(self) robot.randomizeConfig() #sensor.kinematicSimulate(world,0.01) sim.controller(0).setPIDCommand(robot.getConfig(), [0.0] * 7)
def widgetToDeviceTransform(R, t): """Given a widget transform in the world coordinates, map to a device transform """ Tview = so3.rotation((0, 0, 1), math.pi), [0, 0, 0] Rview, tview = se3.mul(se3.inv(Tview), (R, t)) return viewToDeviceTransform(Rview, tview)
w = WorldModel() w.readFile("../data/robots/kinova_gen3_7dof.urdf") w.readFile("../data/terrains/plane.env") robot = w.robot(0) obstacles = [w.terrain(0)] ee_link = 'EndEffector_Link' ee_local_pos = (0.15,0,0) ee_local_axis = (1,0,0) #put a "pen" geometry on the end effector gpen = Geometry3D() res = gpen.loadFile("../data/objects/cylinder.off") assert res gpen.scale(0.01,0.01,0.15) gpen.rotate(so3.rotation((0,1,0),math.pi/2)) robot.link(7).geometry().setCurrentTransform(*se3.identity()) gpen.transform(*robot.link(8).getParentTransform()) robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry())) robot.setConfig(robot.getConfig()) def show_workspace(grid): vis.add("world",w) res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid') g_workspace = Geometry3D(res) g_surface = g_workspace.convert('TriangleMesh',0.5) if g_surface.numElements() != 0: vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5)) else: print("Nothing reachable?")
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) R0 = so3.identity() R1 = so3.rotation([0, 0, 1], math.pi / 2) dR0 = [0.0] * 9 #TODO: for some reason the interpolation doesn't work very well... #vis.add("Camera smooth traj",circle_smooth_traj.discretize_se3(0.1)) #for m in circle_smooth_traj.milestones: # T = m[:12] # vT = m[12:] # print("Orientation velocity",vT[:9])
TODO: the current implementation doesn't work properly. Why? """ m1 = so3.moment(R1) m2 = so3.moment(R2) mu = interpolate_linear(m1, m2, u) angle = vectorops.norm(mu) axis = vectorops.div(mu, angle) return so3.rotation(axis, angle) def interpolate_transform(T1, T2, u): """Interpolate linearly between the two transformations T1 and T2.""" return (interpolate_rotation(T1[0], T2[0], u), interpolate_linear(T1[1], T2[1], u)) Ra = so3.rotation((0, 1, 0), math.pi * -0.9) Rb = so3.rotation((0, 1, 0), math.pi * 0.9) print("Angle between") print(so3.matrix(Ra)) print("and") print(so3.matrix(Rb)) print("is", so3.distance(Ra, Rb)) Ta = [Ra, [-1, 0, 1]] Tb = [Rb, [1, 0, 1]] if __name__ == "__main__": vis.add("world", se3.identity()) vis.add("start", Ta) vis.add("end", Tb) vis.add("interpolated", Ta) vis.edit("start")