def load_hand_configs():
global hand_types, hand_subrobots, hand_configs
for link, hand in hand_types.iteritems():
if hand_configs[link] is None:
continue
resource.setDirectory(hands[link].resource_directory)
qhand = resource.get(hand_configs[link] + ".config",
type="Config",
doedit=False)
#take out the 6 floating DOFs
hand_subrobots[link].setConfig(qhand[6:])
def edit_config():
global world, robot
resource.setDirectory("resources/")
qhands = [r.getConfig() for r in hand_subrobots.values()]
q = resource.get("robosimian_body_stability.config",
default=robot.getConfig(),
doedit=False)
robot.setConfig(q)
for r, qhand in zip(hand_subrobots.values(), qhands):
r.setConfig(qhand)
vis.add("world", world)
vis.edit(("world", robot.getName()), True)
vis.dialog()
vis.edit(("world", robot.getName()), False)
resource.set("robosimian_body_stability.config", robot.getConfig())
print("Numpy size", mnp[0].shape, mnp[1].shape)
m2 = from_numpy(mnp, 'TriangleMesh')
print(len(m2.vertices), len(m2.indices))
print("POINT CLOUD CONVERT")
pcnp = to_numpy(pc)
print("Numpy size", pcnp.shape)
pc2 = from_numpy(pcnp, 'PointCloud')
print(pc2.numPoints(), pc2.numProperties())
print("GEOMETRY CONVERT")
gnp = to_numpy(geom)
print("Numpy geometry has transform", gnp[0])
print("TRAJECTORY CONVERT")
resource.setDirectory('.')
traj = resource.get("../../data/motions/athlete_flex_opt.path")
trajnp = to_numpy(traj)
rtraj = RobotTrajectory(w.robot(0), traj.times, traj.milestones)
rtrajnp = to_numpy(rtraj)
rtraj2 = from_numpy(rtrajnp, template=rtraj)
print("Return type", rtraj2.__class__.__name__, "should be RobotTrajectory")
geom2 = w.robot(0).link(11).geometry()
vg = geom2.convert('VolumeGrid', 0.01)
print("VOLUME GRID CONVERT")
vgnp = to_numpy(vg.getVolumeGrid())
print(vgnp[0], vgnp[1])
import matplotlib.pyplot as plt
import numpy as np
CLOSED_LOOP_TEST = 1
PLAN_TO_GOAL_TEST = 1
MANUAL_PLAN_CREATION = 1
#load the robot / world file
fn = "../../data/robots/jaco.rob"
if len(sys.argv) > 1:
fn = sys.argv[1]
world = WorldModel()
res = world.readFile(fn)
if not res:
print("Unable to read file", fn)
exit(0)
robot = world.robot(0)
resource.setDirectory("resources/" + robot.getName())
def simplify(robot):
"""Utility function: replaces a robot's geometry with simplified bounding
boxes."""
for i in range(robot.numLinks()):
geom = robot.link(i).geometry()
if geom.empty(): continue
geom.setCurrentTransform(*se3.identity())
BB = geom.getBB()
print(BB[0], BB[1])
BBgeom = GeometricPrimitive()
BBgeom.setAABB(BB[0], BB[1])
geom.setGeometricPrimitive(BBgeom)
resource_demo.py: This program demonstrates how to use the
klampt.resource module.
============================================================
""")
worldfile = "../../data/athlete_plane.xml"
if len(sys.argv) > 1:
worldfile = sys.argv[1]
world = klampt.WorldModel()
if not world.readFile(worldfile):
print("Usage: python resource_demo.py [world file]")
exit(1)
robotname = world.robot(0).getName()
#look in resources/athlete/
resource.setDirectory('resources/' + robotname)
print("Configured the resource module to read from",
resource.getDirectory())
templates = {
'1': config_edit_template,
'2': xform_edit_template,
'3': load_save_template,
'4': thumbnail_template,
'5': vis_interaction_test
}
print("Available templates")
import inspect
for k in sorted(templates.keys()):
fname = 'untitled'
for x in inspect.getmembers(templates[k]):
trace = [trajinit]
times = [0]
params = [[]]
else:
trajsolved, trace, times, params = geometryopt.optimizeCollFreeTrajectory(
trajcache,
trajinit,
env=obstaclegeoms,
constraints=[ctest2],
greedyStart=False,
verbose=1,
want_trace=True,
want_times=True,
want_constraints=True)
#play_with_trajectory(trajsolved,[3])
resource.setDirectory("output")
print("Saving to output/robottrajopt_solved.path")
resource.set("robottrajopt_solved.path", trajsolved)
def animate_trajectories(trajs, times, endWaitTime=5.0, speed=0.2):
global world
active = 0
for i in range(len(trajs)):
vis.add(
"traj" + str(i), trajs[i].getLinkTrajectory(
END_EFFECTOR_LINK,
0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION))
vis.hide("traj" + str(i))
vis.hideLabel("traj" + str(i))
vis.show()
from klampt import WorldModel
from klampt import vis
from klampt.math import vectorops,so3,se3
from klampt.model.create import pile
from klampt.io import resource
from known_grippers import *
#load the world model
w = WorldModel()
if not w.readFile(robotiq_140_trina_left.klampt_model):
print("Couldn't load",robotiq_140_trina_left.klampt_model)
exit(1)
import os
data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__),'../data'))
resource.setDirectory(os.path.join(data_dir,'resources/TRINA'))
qhome = resource.get('home.config')
w.robot(0).setConfig(qhome)
#create a box with objects in it
box = world_generator.make_box(w,0.5,0.5,0.1)
box.geometry().translate((1,0,0.8))
objects_in_box = []
for i in range(1):
g = world_generator.make_ycb_object('random',w)
objects_in_box.append(g)
grasp_edit.grasp_edit_ui(robotiq_140_trina_right,objects_in_box[0])
#visualize the right gripper
robotiq_140_trina_right.add_to_vis(w.robot(0))
