def update(bunnycm, task): if base.inputmgr.keymap['space'] is True: for i in range(1): bunnycm1 = bunnycm.copy() bunnycm1.set_mass(.1) rndcolor = np.random.rand(4) rndcolor[-1] = 1 bunnycm1.set_rgba(rndcolor) rotmat = rm.rotmat_from_euler(0, 0, math.pi / 12) z = math.floor(i / 100) y = math.floor((i - z * 100) / 10) x = i - z * 100 - y * 10 print(x, y, z, "\n") bunnycm1.set_homomat( rm.homomat_from_posrot( np.array( [x * 0.015 - 0.07, y * 0.015 - 0.07, 0.35 + z * 0.015]), rotmat)) base.attach_internal_update_obj(bunnycm1) bunnycm1.start_physics() base.inputmgr.keymap['space'] = False return task.cont
def gettwoend(self): self.endpairs=[] self.temporigin = [] self.origin=[] self.homomat=[] for i, coordinate in enumerate(self.coordinate): endpair = [] axis = coordinate.T[2] large = 0 large_vec = np.array([0,0,0]) small = 0 small_vec = np.array([0, 0, 0]) facet_1_ID=self.doubleholdpair[i][0] facet_2_ID = self.doubleholdpair[i][1] vertices = np.append(self.largeface_vertices[facet_1_ID], self.largeface_vertices[facet_2_ID], axis = 0) for vertice in vertices: value = np.dot(axis, vertice) if value>=large: large = value large_vec = vertice elif value<small: small = value small_vec = vertice endpair.append(large_vec) endpair.append(small_vec) self.temporigin.append((large_vec+small_vec)*0.5) self.endpairs.append(endpair) origin = self.getorigin(normal_list=[self.largeface_normals[facet_1_ID],self.largeface_normals[facet_2_ID], axis], point_list=[self.largefacescenter[facet_1_ID],self.largefacescenter[facet_2_ID],(large_vec+small_vec)*0.5]) self.origin.append(origin) homomat = np.linalg.inv(rm.homomat_from_posrot(origin, coordinate)) self.pRotMat.append(homomat) self.pCoM.append(rm.homomat_transform_points(homomat, self.com)) tempvertice = [] for vertice in vertices: tempvertice.append(rm.homomat_transform_points(homomat, vertice)) self.pVertices.append(tempvertice) self.pNormals.append([np.dot(coordinate.T, self.largeface_normals[facet_1_ID]),np.dot(coordinate.T, self.largeface_normals[facet_2_ID])]) self.pFacetpairscenter.append([rm.homomat_transform_points(homomat,self.largefacescenter[facet_1_ID]), rm.homomat_transform_points(homomat,self.largefacescenter[facet_2_ID])])
def gen_roundstick(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, count=[8, 8]): """ :param spos: :param epos: :param thickness: :return: a Trimesh object (Primitive) author: weiwei date: 20191228osaka """ pos = spos height = np.linalg.norm(epos - spos) if np.allclose(height, 0): rotmat = np.eye(3) else: rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos) homomat = rm.homomat_from_posrot(pos, rotmat) return tp.Capsule(height=height, radius=thickness / 2.0, count=count, homomat=homomat)
def gen_meshmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=True, toggle_jntscs=False, name='robot_mesh', rgba=None): mm_collection = mc.ModelCollection(name=name) for id in range(self.jlobject.ndof + 1): if self.jlobject.lnks[id]['collisionmodel'] is not None: this_collisionmodel = self.jlobject.lnks[id][ 'collisionmodel'].copy() pos = self.jlobject.lnks[id]['gl_pos'] rotmat = self.jlobject.lnks[id]['gl_rotmat'] this_collisionmodel.set_homomat( rm.homomat_from_posrot(pos, rotmat)) this_rgba = self.jlobject.lnks[id][ 'rgba'] if rgba is None else rgba this_collisionmodel.set_rgba(this_rgba) this_collisionmodel.attach_to(mm_collection) # tool center coord if toggle_tcpcs: self._toggle_tcpcs(mm_collection, tcp_jntid, tcp_loc_pos, tcp_loc_rotmat, tcpic_rgba=np.array([.5, 0, 1, 1]), tcpic_thickness=.0062) # toggle all coord if toggle_jntscs: alpha = 1 if rgba == None else rgba[3] self._toggle_jntcs(mm_collection, jntcs_thickness=.0062, alpha=alpha) return mm_collection
def gen_rectstick(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=.005, sections=8): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param sections: # of discretized sectors used to approximate a cylinder :return: a Trimesh object (Primitive) author: weiwei date: 20191228osaka """ pos = spos height = np.linalg.norm(epos - spos) if np.allclose(height, 0): rotmat = np.eye(3) else: rotmat = rm.rotmat_between_vectors(np.array([0, 0, 1]), epos - spos) homomat = rm.homomat_from_posrot(pos, rotmat) return tp.Cylinder(height=height, radius=thickness / 2.0, sections=sections, homomat=homomat)
# planecm.setMat(base.pg.np4ToMat4(planenode.gethomomat())) # print(planenode.gethomomat()) # Boxes # model = loader.loadModel('models/box.egg') model = cm.CollisionModel("./objects/bunnysim.stl") node = bbd.BDBody(model, cdtype='box', dynamic=True) bulletnodelist = [] for i in range(10): # node = bch.genBulletCDMesh(model) # newnode = copy.deepcopy(node) newnode = node.copy() newnode.setMass(1) rot = rm.rotmat_from_axangle([0, 1, 0], -math.pi / 4) pos = np.array([0, 0, .1 + i * .3]) newnode.set_homomat(rm.homomat_from_posrot(pos, rot)) base.physicsworld.attachRigidBody(newnode) bulletnodelist.append(newnode) # modelcopy = copy.deepcopy(model) # modelcopy.setColor(.8, .6, .3, .5) # modelcopy.reparentTo(np) # debugNode = BulletDebugNode('Debug') # debugNode.showWireframe(True) # debugNode.showConstraints(True) # debugNode.showBoundingBoxes(False) # debugNode.showNormals(False) # debugNP = base.render.attachNewNode(debugNode) # # debugNP.show() # world.setDebugNode(debugNP.node())
restitution = 0.5 heightlist = [0.001 * i for i in range(820, 860, 30)] errorrange_xyz = 0.003 errorrange_rpy = np.radians(2) errorconfiguration = geterrorconfiguration(errorrange_xyz, errorrange_rpy) xyz_error_print = printonscreen(pos=(-1, 0.7, 0), words="Error range on xyz: " + str(errorrange_xyz) + " (mm)") rpy_error_print = printonscreen(pos=(-1, 0.6, 0), words="Error range on rpy: " + str(errorrange_rpy) + " (degree)") # obj_box = cm.gen_box(extent=[.2, 1, .3], rgba=[.3, 0, 0, 1]) obj_box = cm.gen_sphere(radius=.5, rgba=[.3, 0, 0, 1]) obj_bd_box = bdm.BDModel(obj_box, mass=.3, type="triangles") obj_bd_box.set_pos(np.array([.7, 0, 2.7])) obj_bd_box.start_physics() base.attach_internal_update_obj(obj_bd_box) plane1 = cm.gen_box([0.30000, 0.3000, .3000100], rm.homomat_from_posrot([.500, 0, -.050])) plane1bm = bdm.BDModel(objinit=plane1, mass=1) plane1bm.set_rgba((.5, .5, .5, 1)) plane1bm.set_pos(np.array([0, 0, 1.000])) # plane1bm.start_physics() # base.attach_internal_update_obj(plane1bm) plane = cm.gen_box([5.0000, 5.000, .000100], rm.homomat_from_posrot([.500, 0, -.050])) planebm = bdm.BDModel(objinit=plane, mass=0) planebm.set_rgba((.5, .5, .5, 1)) planebm.set_pos(np.array([0, 0, 0.600])) planebm.start_physics() base.attach_internal_update_obj(planebm) address = this_dir + "/PlacementData" # objname = "test_long"
def getplacementRotMat(self): self.p = [] for i in range(len(self.rotmat)): for j in range(len(self.rotmat[i])): com = copy.deepcopy(self.com) holdingpairsnormals = self.hpairsnormals[i] holdingpairscenters = self.hpairscenters[i] temholdingpairsvertices = self.hpairsvertices[i] # sloperot = None a = self.rotmat[i][j] b = self.holdingpairsSptPnt[i] addvector = np.array([[1], [1], [1]]) holdingpairscenters = np.concatenate( (holdingpairscenters, addvector), axis=1) addvector_vertices = np.array([1]) addvector_com = np.array([1]) print(com) com = np.concatenate((np.array(com), addvector_com), axis=0) holdingpairsvertices = [] for temvertices in temholdingpairsvertices: tempverticeonSFC = [] for vertices in temvertices: vertices = np.concatenate( (vertices, addvector_vertices), axis=0) tempverticeonSFC.append(vertices) holdingpairsvertices.append(tempverticeonSFC) sloperot = rm.homomat_from_posrot(rot=np.dot( rm.rotmat_from_axangle([0, 1, 0], np.radians(-54.74)), rm.rotmat_from_axangle([0, 0, 1], np.radians(-45)))) # sloperot = np.eye(4) # sloperot2 = da.npmat4_to_pdmat4(sloperot1) # sloperot3 = da.pdmat4_to_npmat4(sloperot2) p = da.npmat4_to_pdmat4(rm.homomat_from_posrot(b, a)) self.p.append(p) p = da.pdmat4_to_npmat4(p) p = np.linalg.inv(p) p = np.dot(sloperot, p) placementNormals0 = np.dot(p[0:3, 0:3], holdingpairsnormals[0].T).T placementNormals1 = np.dot(p[0:3, 0:3], holdingpairsnormals[1].T).T placementNormals2 = np.dot(p[0:3, 0:3], holdingpairsnormals[2].T).T placementfacetpairscenter0 = np.dot(p, holdingpairscenters[0].T).T placementfacetpairscenter1 = np.dot(p, holdingpairscenters[1].T).T placementfacetpairscenter2 = np.dot(p, holdingpairscenters[2].T).T #change CoM # x_e=5 # y_e = 5 # z_e = 5 # CoMRotMat = np.array([[1,0,0,x_e],[0,1,0,y_e],[0,0,1,z_e],[0,0,0,1]]) # com = np.dot(CoMRotMat,com.T) #----------------------- print(com.T) placementCom0 = np.dot(p, com.T).T dtemplacementVertices = [] for vertices in holdingpairsvertices: templacementVertices = [] for vertice in vertices: placementfacetpairvertices = np.dot(p, vertice.T).T templacementVertices.append(placementfacetpairvertices) dtemplacementVertices.append(templacementVertices) ddtemplacementface = [] for placement in self.hpairsfaces_triple[i][j]: dtemplacementface = [] for face in placement: templacementface = [] for faceverticeID in face: faceverticePos = np.concatenate( (self.vertices[faceverticeID], np.array([1])), axis=0) rotatedfaceveritce = np.dot(p, faceverticePos.T).T templacementface.append( rotatedfaceveritce) #every small face dtemplacementface.append( templacementface) #every facet ddtemplacementface.append( dtemplacementface) #every placement p = da.npmat4_to_pdmat4(p) self.pRotMat.append(p) self.pNormals.append([ placementNormals0[0:3], placementNormals1[0:3], placementNormals2[0:3] ]) self.pFacetpairscenter.append([ placementfacetpairscenter0[0:3], placementfacetpairscenter1[0:3], placementfacetpairscenter2[0:3] ]) self.pVertices.append(dtemplacementVertices) self.pCoM.append(placementCom0) self.pFaces.append(ddtemplacementface)
# elif x ==13: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008]) # elif x ==14: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010]) # elif x ==15: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0068-0.011]) # elif x ==16: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010]) # elif x == 17: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008]) # elif x == 18: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.005-0.005]) # elif x == 19: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.003-0.001]) c1 = cm.gen_box(extent=[.006*18.5, 0.006*9, .001], homomat=rm.homomat_from_posrot([0.006*18,0.006*6.5,0], rm.rotmat_from_axangle([0,0,1], 0*np.pi/2)),rgba=[0,0,0,0.2]) c2 = cm.gen_box(extent=[.006*18.5, 0.006*12, .001], homomat=rm.homomat_from_posrot([0.006*18,0.006*29,0], rm.rotmat_from_axangle([0,0,1], 0*np.pi/2)),rgba=[0, 0, 0, 0.2]) c3 = cm.gen_box(extent=[.010, .050, .001], homomat=rm.homomat_from_posrot([0, 0.02, 0], rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)), rgba=[0, 0, 0, 0.2]) c4 = cm.gen_box(extent=[.010, .050, .001], homomat=rm.homomat_from_posrot([0.02,0,0], rm.rotmat_from_axangle([0,0,1], 0)), rgba=[0, 0, 0, 0.2]) # c1 = cm.gen_box(extent=[.006 * 20, 0.006 * 30, .001], homomat=rm.homomat_from_posrot([0.006 * 19, 0.006 * 4.5, 0], # rm.rotmat_from_axangle( # [0, 0, 1], # 0 * np.pi / 2)), # rgba=[0, 0, 0, 0.2]) # c2 = cm.gen_box(extent=[.006 * 3, 0.006 * 3, .001], homomat=rm.homomat_from_posrot([0.006 * 4, 0.006 * 12, 0], # rm.rotmat_from_axangle( # [0, 0, 1], # 0 * np.pi / 2)), # rgba=[0, 0, 0, 0.2])
import numpy as np import basis.robot_math as rm base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2]) gm.gen_frame().attach_to(base) objcm = cm.CollisionModel('tubebig.stl') robot_s = ym.Yumi(enable_cc=True) manipulator_name = 'rgt_arm' hand_name = 'rgt_hnd' start_conf = robot_s.get_jnt_values(manipulator_name) goal_homomat_list = [] for i in range(6): goal_pos = np.array([.55, -.1, .3]) - np.array([i * .1, i * .1, 0]) # goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) goal_rotmat = np.eye(3) goal_homomat_list.append(rm.homomat_from_posrot(goal_pos, goal_rotmat)) tmp_objcm = objcm.copy() tmp_objcm.set_rgba([1, 0, 0, .3]) tmp_objcm.set_homomat(rm.homomat_from_posrot(goal_pos, goal_rotmat)) tmp_objcm.attach_to(base) grasp_info_list = gutil.load_pickle_file(objcm_name='tubebig', file_name='yumi_tube_big.pickle') grasp_info = grasp_info_list[0] pp_planner = PickPlacePlanner(robot_s=robot_s) conf_list, jawwidth_list, objpose_list = \ pp_planner.gen_pick_and_place_motion(hnd_name=hand_name, objcm=objcm, grasp_info_list=grasp_info_list, goal_homomat_list=goal_homomat_list, start_conf=robot_s.get_jnt_values(hand_name), end_conf=robot_s.get_jnt_values(hand_name), depart_direction_list=[np.array([0, 0, 1])] * len(goal_homomat_list),
import motion.probabilistic.rrt_connect as rrtc import manipulation.pick_place_planner as ppp if __name__ == '__main__': # world base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1]) gm.gen_frame().attach_to(base) object = cm.CollisionModel("./objects/tubebig.stl") object.set_rgba([.5, .7, .3, 1]) gm.gen_frame().attach_to(object) object_start = object.copy() object_start_pos = np.array([0.800, -.350, 0.900]) object_start_rotmat = np.eye(3) object_start_homomat = rm.homomat_from_posrot(object_start_pos, object_start_rotmat) object_start.set_pos(object_start_pos) object_start.set_rotmat(object_start_rotmat) object_start.attach_to(base) object_goal = object.copy() object_goal_pos = np.array([0.800, -.300, 0.900]) object_goal_rotmat = np.eye(3) object_goal_homomat = rm.homomat_from_posrot(object_goal_pos, object_goal_rotmat) object_goal.set_pos(object_goal_pos) object_goal.set_rotmat(object_goal_rotmat) object_goal.attach_to(base) # robot_instance = ur3d.UR3Dual() robot = ur3ed.UR3EDual()
] for i in obstacle_list: i.show_cdprimit() # base.run() ## show tool tool = cm.gen_box(extent=[.05, .2, .05]) tool_initial = tool.copy() tool_final = tool.copy() tool_initial.set_rgba([0, 0, 1, 1]) tool_initial_pos = tool_table_pos + np.array([0, 0, .15 + .03]) tool_initial_rotmat = rm.rotmat_from_axangle([0, 0, 1], math.radians(90)) tool_initial.set_pos(tool_initial_pos) tool_initial.set_rotmat(tool_initial_rotmat) # tool_initial.attach_to(base) tool_initial.show_localframe() tool_initial_homomat = rm.homomat_from_posrot(tool_initial_pos, rotmat) tool_final.set_rgba([0, 0, 1, 1]) tool_final_pos = np.array([ table2_center[0], table2_center[1], object_box4.get_pos()[2] + .009 + .11 ]) rotmat = np.dot(rm.rotmat_from_axangle([0, 1, 0], math.radians(90)), rm.rotmat_from_axangle([0, 0, 1], math.radians(90))) tool_final.set_pos(tool_final_pos) tool_final.set_rotmat(rotmat) tool_final_homomat = rm.homomat_from_posrot(tool_final_pos, rotmat) ## rbt_s and grasp list robot_s = xsm.XArm7YunjiMobile() rrtc_s = rrtc.RRTConnect(robot_s) rrtc_planner = rrtdwc.RRTDWConnect(robot_s) grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle')
# elif x ==14: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010]) # elif x ==15: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0068-0.011]) # elif x ==16: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010]) # elif x == 17: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008]) # elif x == 18: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.005-0.005]) # elif x == 19: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.003-0.001]) #left right c1 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 8, .001], homomat=rm.homomat_from_posrot( [0.006 * 22, 0.006 * 9, 0], rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)), rgba=[0, 0, 0, 0.2]) c2 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 8, .001], homomat=rm.homomat_from_posrot( [0.006 * 22, 0.006 * 29, 0], rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)), rgba=[0, 0, 0, 0.2]) #top down c3 = cm.gen_box(extent=[.006 * 4, 0.006 * 28.5, .001], homomat=rm.homomat_from_posrot( [0.006 * 3.5, 0.006 * 19, 0], rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)), rgba=[0, 0, 0, 0.2]) c4 = cm.gen_box(extent=[.006 * 4, 0.006 * 28.5, .001], homomat=rm.homomat_from_posrot(
# elif x ==14: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010]) # elif x ==15: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0068-0.011]) # elif x ==16: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.0065-0.010]) # elif x == 17: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.006-0.008]) # elif x == 18: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.005-0.005]) # elif x == 19: # matrix[y][x] = matrix[y][x] + np.array([0, 0, -0.003-0.001]) c1 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 9, .001], homomat=rm.homomat_from_posrot( [0.006 * 18, 0.006 * 6.5, 0], rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)), rgba=[0, 0, 0, 0.2]) c2 = cm.gen_box(extent=[.006 * 18.5, 0.006 * 12, .001], homomat=rm.homomat_from_posrot( [0.006 * 18, 0.006 * 29, 0], rm.rotmat_from_axangle([0, 0, 1], 0 * np.pi / 2)), rgba=[0, 0, 0, 0.2]) c3 = cm.gen_box(extent=[.010, .050, .001], homomat=rm.homomat_from_posrot([0, 0.02, 0], rm.rotmat_from_axangle( [0, 0, 1], np.pi / 2)), rgba=[0, 0, 0, 0.2]) c4 = cm.gen_box(extent=[.010, .050, .001], homomat=rm.homomat_from_posrot([0.02, 0, 0], rm.rotmat_from_axangle(
testobj = bdm.BDModel(objpath, mass=1, restitution=restitution, dynamic=True, friction=friction, type="triangles") # testobj.set_scale((0.001,0.001,0.001)) testobj.set_rgba((.3, .5, .7, 0.5)) testobj.start_physics() testobj.set_linearDamping(0.1) testobj.set_angularDamping(0.1) objbm.append(testobj) # table plane = cm.gen_box([5.0000, 5.000, .000100], rm.homomat_from_posrot([.500, 0, -.050])) planebm = bdm.BDModel(objinit=plane, mass=0, friction=friction) planebm.set_rgba((.5, .5, .5, 1)) planebm.set_pos(np.array([0, 0, 0.600])) planebm.start_physics() # slope slope = slope.Slope(z=0, placement="ping", size=0.5) slopemodels = slope.getSlopeDym(mass=0, restitution=restitution, dynamic=True, friction=friction) slopePos = [0.600, 0, 0.78] slopemodels[0].set_pos(slopePos) slopemodels[1].set_pos(slopePos) slopemodels[2].set_pos(slopePos)
# object_goal = object.copy() # object_goal_pos = np.array([0.800, -.300, 0.900]) # object_goal_rotmat = np.eye(3) # object_goal_homomat = rm.homomat_from_posrot(object_goal_pos, object_goal_rotmat) # object_goal.set_pos(object_goal_pos) # object_goal.set_rotmat(object_goal_rotmat) # object_goal.attach_to(base) fixture = cm.CollisionModel("./objects/tc100.stl") fixture.set_scale((.001, .001, .001)) fixture.set_rgba([.8, .6, .3, 0.5]) fixture_start = fixture.copy() fixture_start_pos = np.array([0.800, -.150, 0.780]) fixture_start_rotmat = np.eye(3) fixture_start_homomat = rm.homomat_from_posrot(fixture_start_pos, fixture_start_rotmat) fixture_start_rotmat = fixture_start_homomat[:3, :3] fixture_start.set_pos(fixture_start_pos) fixture_start.set_rotmat(fixture_start_rotmat) fixture_start.attach_to(base) slopename = "tc71.stl" slope_high = slope.Slope(z=-0.005, placement="ping", size=sinfo.Sloperate[slopename], show=False) slopeforcd_high = slope_high.getSlope() for i, item in enumerate(slopeforcd_high): item.set_homomat(fixture_start_homomat.dot(item.get_homomat())) slopeforcd_high[i] = item
bpgm.set_vert_size(.01) bpgm1.attach_to(base) bpgm2.attach_to(base) lsgm = gen_linesegs([[np.array([.1, 0, .01]), np.array([.01, 0, .01])], [np.array([.01, 0, .01]), np.array([.1, 0, .1])], [np.array([.1, 0, .1]), np.array([.1, 0, .01])]]) lsgm.attach_to(base) gen_circarrow(radius=.1, portion=.8).attach_to(base) gen_dasharrow(spos=np.array([0, 0, 0]), epos=np.array([0, 0, 2])).attach_to(base) gen_dashframe(pos=np.array([0, 0, 0]), rotmat=np.eye(3)).attach_to(base) axmat = rm.rotmat_from_axangle([1, 1, 1], math.pi / 4) gen_frame(rotmat=axmat).attach_to(base) axmat[:, 0] = .1 * axmat[:, 0] axmat[:, 1] = .07 * axmat[:, 1] axmat[:, 2] = .3 * axmat[:, 2] gen_ellipsoid(pos=np.array([0, 0, 0]), axmat=axmat).attach_to(base) print(rm.unit_vector(np.array([0, 0, 0]))) pos = np.array([.3, 0, 0]) rotmat = rm.rotmat_from_euler(math.pi / 6, 0, 0) homomat = rm.homomat_from_posrot(pos, rotmat) gen_frame_box([.1, .2, .3], homomat).attach_to(base) base.run()
import robot_sim.robots.cobotta.cobotta as cbt import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15]) gm.gen_frame().attach_to(base) # ground ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7]) ground.set_pos(np.array([0, 0, -.51])) ground.attach_to(base) # object holder object_holder = cm.CollisionModel("objects/holder.stl") object_holder.set_rgba([.5, .5, .5, 1]) object_holder_gl_pos = np.array([-.15, -.3, .0]) object_holder_gl_rotmat = np.eye(3) obgl_start_homomat = rm.homomat_from_posrot(object_holder_gl_pos, object_holder_gl_rotmat) object_holder.set_pos(object_holder_gl_pos) object_holder.set_rotmat(object_holder_gl_rotmat) gm.gen_frame().attach_to(object_holder) object_holder_copy = object_holder.copy() object_holder_copy.attach_to(base) # object holder goal object_holder_gl_goal_pos = np.array([.25, -.05, .05]) object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2) obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos, object_holder_gl_goal_rotmat) object_holder_goal_copy = object_holder.copy() object_holder_goal_copy.set_homomat(obgl_goal_homomat) object_holder_goal_copy.attach_to(base) robot_s = cbt.Cobotta()
def get_homomat(self): npv3 = da.pdv3_to_npv3(self._objpdnp.getPos()) npmat3 = da.pdquat_to_npmat3(self._objpdnp.getQuat()) return rm.homomat_from_posrot(npv3, npmat3)