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()