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 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)
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?") Tee = robot.link(ee_link).getTransform() gpen.setCurrentTransform(*Tee) box = GeometricPrimitive()