def create_world():
world = WorldModel()
# world.loadElement('robots_urdf/base_link.off')
world.readFile('./assets/cardata/car.rob')
robot = world.robot(0)
robot.setConfig([0, -2, 0])
link = robot.link(2)
link_geom = link.geometry()
link_geom.scale(4) # make the geometry larger so it's more difficult
# world.readFile('./robots_urdf/05-visual.urdf')
# load the environment too
load_path = './assets/jagged_narrow.xml'
xmldoc = minidom.parse(load_path)
itemlist = xmldoc.getElementsByTagName('Geom')
for i, item in enumerate(itemlist):
extends = [
float(x) * 2 for x in item.getElementsByTagName('extents')
[0].childNodes[0].data.split()
]
translates = [
float(x) for x in item.getElementsByTagName('translation')
[0].childNodes[0].data.split()
]
create.primitives.box(extends[0],
extends[1],
extends[2],
translates,
world=world,
name='obstacle%d' % i)
return world, robot, [0, 1, 2], [2], [
0, 1, 2, 3, 4
] # the last three are link_index, geom_index, obs_index
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 create_world():
"""Just create the world so geometries are clearly defined"""
load_path = './assets/table.xml'
xmldoc = minidom.parse(load_path)
itemlist = xmldoc.getElementsByTagName('Geom')
print('num body = ', len(itemlist))
world = WorldModel()
for i, item in enumerate(itemlist):
extends = [
float(x) * 2 for x in item.getElementsByTagName('extents')
[0].childNodes[0].data.split()
]
translates = [
float(x) for x in item.getElementsByTagName('translation')
[0].childNodes[0].data.split()
]
create.primitives.box(extends[0],
extends[1],
extends[2],
translates,
world=world,
name='obstacle%d' % i)
# load the robot
pr2_path = './assets/pr2/pr2.urdf'
world.readFile(pr2_path)
robot = world.robot(0)
print(f'It has {robot.numLinks()} links')
# link_names = ['r_shoulder_pan_link', 'r_shoulder_lift_link', 'r_upper_arm_roll_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 'r_wrist_flex_link', 'r_wrist_roll_link']
link_names = [
'r_shoulder_pan_link',
'r_shoulder_lift_link',
'r_upper_arm_roll_link',
'r_elbow_flex_link',
'r_forearm_roll_link',
'r_wrist_flex_link',
'r_wrist_roll_link',
]
geom_names = [
'r_shoulder_pan_link',
'r_shoulder_lift_link',
'r_upper_arm_roll_link',
'r_upper_arm_link',
'r_elbow_flex_link',
'r_forearm_roll_link',
'r_forearm_link',
'r_wrist_flex_link',
'r_wrist_roll_link',
]
link_indexes = []
for link_name in link_names:
link = robot.link(link_name)
link_indexes.append(link.getIndex())
geom_indexes = []
for link_name in geom_names:
link = robot.link(link_name)
geom_indexes.append(link.getIndex())
obs_indexes = [0]
return world, robot, link_indexes, geom_indexes, obs_indexes
def __init__(self):
GLRealtimeProgram.__init__(self, 'World Viewer')
self.world = WorldModel()
self.fps = 10.0
self.dt = 1 / self.fps
self.pre_drawables = []
self.post_drawables = []
def grasp_plan_main():
world = WorldModel()
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
world.readFile("table.xml")
xform = resource.get("table_camera_00.xform",type='RigidTransform')
sensing.set_sensor_xform(sensor,xform)
box = GeometricPrimitive()
box.setAABB([0,0,0],[1,1,1])
box = resource.get('table.geom','GeometricPrimitive',default=box,world=world,doedit='auto')
bmin,bmax = [v for v in box.properties[0:3]],[v for v in box.properties[3:6]]
nobj = 5
obj_fns = []
for o in range(nobj):
fn = random.choice(ycb_objects)
obj = make_ycb_object(fn,world)
#TODO: you might want to mess with colors here too
obj.appearance().setSilhouette(0)
obj_fns.append(fn)
for i in range(world.numTerrains()):
#TODO: you might want to mess with colors here too
world.terrain(i).appearance().setSilhouette(0)
problem1.arrange_objects(world,obj_fns,bmin,bmax)
intrinsics = dict()
w = int(sensor.getSetting('xres'))
h = int(sensor.getSetting('yres'))
xfov = float(sensor.getSetting('xfov'))
yfov = float(sensor.getSetting('yfov'))
intrinsics['cx'] = w/2
intrinsics['cy'] = h/2
intrinsics['fx'] = math.tan(xfov*0.5)*h*2
intrinsics['fy'] = math.tan(xfov*0.5)*h*2
print("Intrinsics",intrinsics)
planner = ImageBasedGraspPredictor()
def do_grasp_plan(event=None,world=world,sensor=sensor,planner=planner,camera_xform=xform,camera_intrinsics=intrinsics):
sensor.kinematicReset()
sensor.kinematicSimulate(world,0.01)
rgb,depth = sensing.camera_to_images(sensor)
grasps,scores = planner.generate(camera_xform,camera_intrinsics,rgb,depth)
for i,(g,s) in enumerate(zip(grasps,scores)):
color = (1-s,s,0,1)
g.add_to_vis("grasp_"+str(i),color=color)
def resample_objects(event=None,world=world,obj_fns=obj_fns,bmin=bmin,bmax=bmax):
problem1.arrange_objects(world,obj_fns,bmin,bmax)
vis.add("world",world)
vis.add("sensor",sensor)
vis.addAction(do_grasp_plan,'Run grasp planner','p')
vis.addAction(resample_objects,'Sample new arrangement','s')
vis.run()
def __init__(self):
self.world = WorldModel()
self.robot = self.world.loadRobot(
'franka_panda/panda_model_w_table.urdf')
self.robot.setJointLimits([
-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326,
-2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0
], [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0,
2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0
])
self.grasptarget_link = self.robot.link(16)
time.sleep(1)
def gen_grasp_worlds(N=10):
if len(base_boxes) == 0:
#edit base boxes
for basefn in base_files:
world = WorldModel()
world.readFile(basefn)
base_name = os.path.splitext(os.path.basename(basefn))[0]
box = GeometricPrimitive()
box.setAABB([0, 0, 0], [1, 1, 1])
base_boxes.append(
resource.get(base_name + '.geom',
'GeometricPrimitive',
default=box,
world=world,
doedit='auto'))
output_file_pattern = "generated_worlds/world_%04d.xml"
#TODO: play around with me, problem 1a
num_objects = [3, 3, 4, 5, 6, 6]
for i in range(N):
nobj = random.choice(num_objects)
world = WorldModel()
base_world = random.choice(range(len(base_files)))
world.readFile(base_files[base_world])
obj_fns = []
for o in range(nobj):
fn = random.choice(ycb_objects)
obj = make_ycb_object(fn, world)
obj_fns.append(fn)
bbox = base_boxes[base_world]
bmin, bmax = [v for v in bbox.properties[0:3]
], [v for v in bbox.properties[3:6]]
#TODO: Problem 1: arrange objects within box bmin,bmax
arrange_objects(world, obj_fns, bmin, bmax)
save_world(world, output_file_pattern % (i, ))
def main():
#creates a world and loads all the items on the command line
world = WorldModel()
res = world.readFile("./HRP2_Robot.xml")
if not res:
raise RuntimeError("Unable to load model")
robot = world.robot(0)
# coordinates.setWorldModel(world)
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin)
#add the world to the visualizer
vis.add("world", world)
vis.add("robot", world.robot(0))
vis.show()
# ipdb.set_trace()
Robotstate_Traj, Contact_Force_Traj = Traj_Loader()
h = 0.0068 # 0.0043: vert
# 0.0033: flat
playspeed = 2.5
norm = 500
while vis.shown():
# This is the main plot program
for i in range(0, Robotstate_Traj.shape[1]):
vis.lock()
Robotstate_Traj_i = Robotstate_Traj[:, i]
Robotstate_Traj_Full_i = Dimension_Recovery(Robotstate_Traj_i)
# Now it is the plot of the contact force at the contact extremities
robot.setConfig(Robotstate_Traj_Full_i)
Contact_Force_Traj_i = Contact_Force_Traj[:, i]
left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec(
robot, Contact_Force_Traj_i, norm)
vis.add("left foot force",
Trajectory([0, 1], [left_ft, left_ft_end]))
vis.add("right foot force",
Trajectory([0, 1], [rght_ft, rght_ft_end]))
vis.add("left hand force",
Trajectory([0, 1], [left_hd, left_hd_end]))
vis.add("right hand force",
Trajectory([0, 1], [rght_hd, rght_hd_end]))
COMPos_start = robot.getCom()
COMPos_end = COMPos_start
COMPos_end[2] = COMPos_end[2] + 100
vis.add("Center of Mass",
Trajectory([0, 1], [COMPos_start, COMPos_end]))
# ipdb.set_trace()
vis.unlock()
time.sleep(h * playspeed)
def mounted(gripper,klampt_model,base_link,name=None,
register=True):
"""From a standalone gripper, return a GripperInfo such that the link
indices are shifted onto a new robot model.
"""
if name is None:
name = gripper.name + "_mounted"
w = WorldModel()
w.enableGeometryLoading(False)
res = w.readFile(klampt_model)
if not res:
raise IOError("Unable to load file "+str(klampt_model))
robot = w.robot(0)
w.enableGeometryLoading(True)
if isinstance(base_link,str):
base_link = robot.link(base_link).index
shifted_finger_links = [l+base_link for l in gripper.finger_links]
mount_driver = -1
for i in range(robot.numDrivers()):
ls = robot.driver(i).getAffectedLinks()
if any(l in ls for l in shifted_finger_links):
mount_driver = i
break
if mount_driver < 0:
raise RuntimeError("Can't find the base driver for the mounted gripper?")
shifted_finger_drivers = [l+mount_driver for l in gripper.finger_drivers]
res = copy.copy(gripper)
res.name = name
res.base_link = base_link
res.klampt_model = klampt_model
res.finger_links = shifted_finger_links
res.finger_drivers = shifted_finger_drivers
if register:
GripperInfo.register(res)
return res
def _loadWorld(self, world_fn):
world = WorldModel()
res = world.readFile(world_fn)
if not res:
print("Unable to read file", world_fn)
exit(0)
for i in range(world.numRigidObjects()):
obj = world.rigidObject(i)
#this will perform a reasonable center of mass / inertia estimate
m = obj.getMass()
m.estimate(obj.geometry(), mass=0.454, surfaceFraction=0.2)
obj.setMass(m)
self.world = world
class IK():
def __init__(self):
self.world = WorldModel()
self.robot = self.world.loadRobot(
'franka_panda/panda_model_w_table.urdf')
self.robot.setJointLimits([
-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326,
-2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0
], [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0,
2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0
])
self.grasptarget_link = self.robot.link(16)
time.sleep(1)
def __call__(self, from_cfg, to_ee_loc, return_flag=False):
if isinstance(to_ee_loc, np.ndarray):
to_ee_loc = list(to_ee_loc.flat)
from_cfg_full = copy(self.robot.getConfig())
from_cfg_full[7:14] = from_cfg
self.robot.setConfig(from_cfg_full)
objective = ik.objective(self.grasptarget_link,
local=[0, 0, 0],
world=to_ee_loc)
flag = ik.solve(objective, iters=1000, tol=1e-4)
cfg = self.robot.getConfig()[7:14]
if not return_flag:
return cfg
else:
return cfg, flag
def __init__(self):
self.world = WorldModel()
self.robot = self.world.loadRobot(
'franka_panda/panda_model_w_table.urdf')
self.robot.setJointLimits([
-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326,
-2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0
], [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0,
2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0
])
self.grasptarget_link = self.robot.link(16)
self.model_filename = "../dynamical_system_modulation_svm/models/gammaSVM_frankaROCUS_bounded_pyb.pkl"
reference_points = np.array([[0, 0.15, 0.975], [0.0, 0.5, 0.80],
[0.0, 0, 0.61]])
self.modulator = Modulator(self.model_filename, reference_points)
def klamptModel(self):
"""Returns the Klamp't RobotModel associated with this robot, either by
the robotModel object or loading from the given filename self.fn.
The results are cached so this can be called many times.
"""
if self.robotModel is not None:
return self.robotModel
if self.fn is None:
raise RuntimeError("Can't load robot model for "+self.name+", no file given")
from klampt import WorldModel,RobotModel
self._worldTemp = WorldModel()
self.robotModel = self._worldTemp.loadRobot(self.fn)
if self.robotModel.index < 0:
raise IOError("Unable to load robot from file "+self.fn)
return self.robotModel
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 edit_camera_xform(world_fn, xform=None, title=None):
"""Visual editor of the camera position
"""
world = WorldModel()
world.readFile(world_fn)
world.readFile("camera.rob")
robot = world.robot(0)
sensor = robot.sensor(0)
if xform is not None:
sensing.set_sensor_xform(sensor, xform)
vis.createWindow()
if title is not None:
vis.setWindowTitle(title)
vis.resizeWindow(1024, 768)
vis.add("world", world)
vis.add("sensor", sensor)
vis.add("sensor_xform", sensing.get_sensor_xform(sensor, robot))
vis.edit("sensor_xform")
def update_sensor_xform():
sensor_xform = vis.getItemConfig("sensor_xform")
sensor_xform = sensor_xform[:9], sensor_xform[9:]
sensing.set_sensor_xform(sensor, sensor_xform)
vis.loop(callback=update_sensor_xform)
sensor_xform = vis.getItemConfig("sensor_xform")
return sensor_xform[:9], sensor_xform[9:]
def __init__(self, floor_length=20, floor_width=20):
self.w = WorldModel()
self.floor = Geometry3D()
self.floor.loadFile(model_path + "cube.off")
self.floor.transform(
[floor_length, 0, 0, 0, floor_width, 0, 0, 0, 0.01],
[-floor_length / 2.0, -floor_width / 2.0, -0.01])
floor_terrain = self.w.makeTerrain("floor")
floor_terrain.geometry().set(self.floor)
floor_terrain.appearance().setColor(0.4, 0.3, 0.2, 1.0)
###colors
self.light_blue = [3.0 / 255.0, 140.0 / 255.0, 252.0 / 255.0, 1.0]
self.wall_green = [50.0 / 255.0, 168.0 / 255.0, 143.0 / 255.0, 1.0]
###sizes
self.table_length = 1.0
self.table_width = 0.5
self.table_top_thickness = 0.03
self.table_height = 0.6
self.leg_width = 0.05
self.cube_width = 0.05
def __init__(self):
self.world = WorldModel()
self.robot = self.world.loadRobot(
'franka_panda/panda_model_w_table.urdf')
self.robot.setJointLimits([
-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326,
-2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0
], [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0,
2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0
])
self.initial_cfg_left = [0] * 7 + [
-np.pi / 2, -np.pi / 2, np.pi / 2 + 0.3, -np.pi / 2, 0, np.pi / 2,
3 * np.pi / 4
] + [0] * 3
self.initial_cfg_right = [0] * 7 + [
-np.pi / 2, np.pi / 2, np.pi / 2 - 0.3, -np.pi / 2, 0, np.pi / 2,
3 * np.pi / 4
] + [0] * 3
self.grasptarget_link = self.robot.link(16)
time.sleep(1)
def load_world():
world = WorldModel()
create.primitives.box(0.5,
0.1,
0.5, [0.6, -0.2, 1.6],
world=world,
name='obstacle') # z is 1.2 for classical setting
create.primitives.box(
0.5, 0.1, 0.5, [0.6, 0.25, 1.6], world=world,
name='obstacle2') # z used to be 1.6 for classical setting
create.primitives.sphere(0.1, [-1, -1, 0], world=world, name='origin')
create.primitives.sphere(0.1, [0, -1, 0], world=world, name='X')
create.primitives.sphere(0.1, [-1, 1, 0], world=world, name='Y')
fn = "./assets/ur5data/ur5Blocks.xml"
res = world.readFile(fn)
robot = world.robot(0)
# change joint limits to avoid self collision
qmin, qmax = robot.getJointLimits()
qmin[2] = -math.pi / 2
qmax[2] = 0
qmin[3] = 0
qmax[3] = math.pi
robot.setJointLimits(qmin, qmax)
return world, robot
class IK():
def __init__(self):
self.world = WorldModel()
self.robot = self.world.loadRobot(
'franka_panda/panda_model_w_table.urdf')
# self.table = self.world.loadRobot('franka_panda/table/table_shelf.urdf')
# print('done loading')
# print(self.robot.getJointLimits())
self.robot.setJointLimits([
-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326,
-2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0
], [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0,
2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0
])
self.initial_cfg_left = [0] * 7 + [
-m.pi / 2, -m.pi / 2, m.pi / 2 + 0.3, -m.pi / 2, 0, m.pi / 2,
3 * m.pi / 4
] + [0] * 3
self.initial_cfg_right = [0] * 7 + [
-m.pi / 2, m.pi / 2, m.pi / 2 - 0.3, -m.pi / 2, 0, m.pi / 2,
3 * m.pi / 4
] + [0] * 3
self.grasptarget_link = self.robot.link(16)
time.sleep(1)
# print('initializing collider')
# self.collider = WorldCollider(self.world)
# print('done')
def solve(self, target_loc):
if isinstance(target_loc, np.ndarray):
target_loc = list(target_loc.flat)
if target_loc[0] < 0:
initial_cfg = self.initial_cfg_left
else:
initial_cfg = self.initial_cfg_right
self.robot.setConfig(initial_cfg)
# return True, self.robot.getConfig()[7:14]
objective = ik.objective(self.grasptarget_link,
local=[0, 0, 0],
world=target_loc)
flag = ik.solve(objective, iters=1000, tol=1e-4)
cfg = self.robot.getConfig()[7:14]
# collisions = list(self.collider.robotSelfCollisions(self.robot))
# print(f'get {len(collisions)} collisions')
# for c1, c2 in collisions:
# print('collisions', c1.getName(), c2.getName())
return flag, cfg, initial_cfg[7:14]
def setup_robot_and_light(
self,
robotfile='./data/idealbot.rob',
mesh_file='./data/environment_meshes/full_detail_hospital_cad_meters.obj',
float_height=0.08):
world = WorldModel()
res1 = world.loadElement(robotfile)
robot = world.robot(0)
qmin, qmax = robot.getJointLimits()
qmin = np.nan_to_num(qmin, posinf=1000, neginf=-100)
qmax = np.nan_to_num(qmax, posinf=1000, neginf=-100)
robot.setJointLimits(qmin, qmax)
#world.loadElement(robotfile)
#a = Geometry3D()
# res = a.loadFile(mesh_file)
res = world.loadElement(mesh_file)
print(res)
collider = WorldCollider(world)
#we then dilate the base nd ignore collisions between it and the 2 subsequent links:
# collider.ignoreCollision((robot.link(self.base_height_link),robot.link(3)))
# collider.ignoreCollision((robot.link(self.base_height_link),robot.link(3)))
# collider.ignoreCollision((robot.link(8),robot.link(6)))
# we now
cfig = robot.getConfig()
terrain = world.terrain(0)
lamp = robot.link(self.lamp_linknum)
print('\n\n\nbase height link = {}, lamp linknum = {}\n\n\n'.format(
self.base_height_link, self.lamp_linknum))
cfig[self.base_height_link] = float_height
robot.setConfig(cfig)
robot.link(self.lamp_linknum).appearance().setColor(
210 / 255, 128 / 255, 240 / 255, 1)
world.saveFile('disinfection.xml')
return world, robot, lamp, collider
class IK():
def __init__(self):
self.world = WorldModel()
self.robot = self.world.loadRobot(
'franka_panda/panda_model_w_table.urdf')
self.robot.setJointLimits([
-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -2.9671, -1.8326,
-2.9671, -3.1416, -2.9671, -0.0873, -2.9671, 0.0, 0.0, 0.0
], [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.9671, 1.8326, 2.9671, 0.0,
2.9671, 3.8223, 2.9671, 0.0, 0.0, 0.0
])
self.initial_cfg_left = [0] * 7 + [
-np.pi / 2, -np.pi / 2, np.pi / 2 + 0.3, -np.pi / 2, 0, np.pi / 2,
3 * np.pi / 4
] + [0] * 3
self.initial_cfg_right = [0] * 7 + [
-np.pi / 2, np.pi / 2, np.pi / 2 - 0.3, -np.pi / 2, 0, np.pi / 2,
3 * np.pi / 4
] + [0] * 3
self.grasptarget_link = self.robot.link(16)
time.sleep(1)
def solve(self, target_loc):
if isinstance(target_loc, np.ndarray):
target_loc = list(target_loc.flat)
if target_loc[0] < 0:
initial_cfg = self.initial_cfg_left
else:
initial_cfg = self.initial_cfg_right
self.robot.setConfig(initial_cfg)
objective = ik.objective(self.grasptarget_link,
local=[0, 0, 0],
world=target_loc)
flag = ik.solve(objective, iters=1000, tol=1e-4)
cfg = self.robot.getConfig()[7:14]
return flag, cfg, initial_cfg[7:14]
def visualize_pc_in_klampt_vis(self, pcloud_fname):
title = pcloud_fname + " klampt world"
vis_window_id = vis.createWindow(title)
vis.setWindowTitle(title)
world = WorldModel()
vis.add("world", world)
pcd = o3d.io.read_point_cloud(pcloud_fname)
print(pcd)
pc_xyzs = np.asarray(pcd.points)
pc_xyzs_as_list = pc_xyzs.flatten().astype("float")
# pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0])
pcloud = PointCloud()
pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list)
print(pcloud.numPoints())
vis.add("pcloud", pcloud)
# vis.setColor("pcloud", 0, 0, 1, a=1)
# vis.setAttribute("p1", "size", 5.0)
box = klampt.GeometricPrimitive()
box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2])
g = klampt.Geometry3D(box)
vis.add("box", g)
vis.setColor("box", 0, 0, 1, 0.5)
coordinates.setWorldModel(world)
vis.add("coordinates", coordinates.manager())
vis.show()
while vis.shown():
vis.lock()
vis.unlock()
time.sleep(0.01)
vis.kill()
class WorldViewer(GLRealtimeProgram):
def __init__(self):
GLRealtimeProgram.__init__(self, 'World Viewer')
self.world = WorldModel()
self.fps = 10.0
self.dt = 1 / self.fps
self.pre_drawables = []
self.post_drawables = []
def display(self):
for drawable in self.pre_drawables:
drawable.draw()
self.world.drawGL()
for drawable in self.post_drawables:
drawable.draw()
# draw poses for all robots and boxes
poses = [se3.identity()]
for i in range(self.world.numRobots()):
robot = self.world.robot(i)
poses.append(robot.link(0).getTransform())
poses.append(robot.link(robot.numLinks() - 1).getTransform())
for i in range(self.world.numRigidObjects()):
poses.append(self.world.rigidObject(i).getTransform())
for pose in poses:
gldraw.xform_widget(pose, 0.1, 0.01, fancy=True)
def mousefunc(self, button, state, x, y):
return GLRealtimeProgram.mousefunc(self, button, state, x, y)
def motionfunc(self, x, y, dx, dy):
return GLRealtimeProgram.motionfunc(self, x, y, dx, dy)
def add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
class RobotInfo:
"""Stores common semantic properties for a robot.
Attributes:
- name (str): a string identifying this robot
- fn (fn, optional): a file pointing to the Klamp't model.
- endEffectors (dict str->EndEffectorInfo): a set of named end
effectors.
- parts (dict str->list): a set of named parts
"""
def __init__(self,
name,
fn=None,
freeBase=False,
parts=None,
endEffectors=None,
properties=None):
self.name = name
self.fn = fn
self.robotModel = None
self._worldTemp = None
self.freeBase = freeBase
if parts is None:
parts = dict()
else:
if not isinstance(parts, dict):
raise ValueError(
"`parts` must be a dict from names to link lists")
for (k, v) in parts:
if not hasattr(v, '__iter__'):
raise ValueError(
"`parts` must be a dict from names to link lists")
if endEffectors is None:
endEffectors = dict()
else:
if not isinstance(endEffectors, dict):
raise ValueError(
"`endEffectors` must be a dict from names to links")
if properties is None:
properties = dict()
else:
if not isinstance(properties, dict):
raise ValueError("`properties` must be a dict")
self.parts = parts
self.endEffectors = endEffectors
self.properties = properties
def klamptModel(self):
"""Returns the Klamp't RobotModel associated with this robot, either by
the robotModel object or loading from the given filename self.fn.
The results are cached so this can be called many times.
"""
if self.robotModel is not None:
return self.robotModel
if self.fn is None:
raise RuntimeError("Can't load robot model for " + self.name +
", no file given")
from klampt import WorldModel, RobotModel
self._worldTemp = WorldModel()
self.robotModel = self._worldTemp.loadRobot(self.fn)
if self.robotModel.index < 0:
raise IOError("Unable to load robot from file " + self.fn)
return self.robotModel
def partLinks(self, part):
return self.parts[part]
def partLinkIndices(self, part):
res = self.parts[part]
return self.toIndices(res)
def partLinkNames(self, part):
res = self.parts[part]
return self.toNames(res)
def partAsSubrobot(self, part):
from klampt.model.subrobot import SubRobotModel
partLinks = self.partLinkIndices(part)
model = self.klamptModel()
return SubRobotModel(model, partLinks)
def eeSolver(self, endEffector, target):
"""Given a named end effector and a target point, transform, or set of
parameters from config.setConfig(ikgoal) / config.getConfig(ikgoal),
returns the :class:`~klampt.robotsim.IKSolver` for the end effector and
that target.
"""
ee = self.endEffectors[endEffector]
from klampt import IKSolver, IKObjective
from ..model import config, ik
robot = self.klamptModel()
link = robot.link(ee.link)
s = IKSolver(robot)
q = config.getConfig(target)
obj = ee.ikObjective
if ee.ikObjective is None:
if len(q) == 3:
obj = ik.objective(link, local=[0, 0, 0], world=q)
elif len(q) == 12:
obj = ik.objective(link, R=q[:9], t=q[9:])
else:
raise ValueError(
"Invalid # of elements given in target, must be either a 3-vector or SE3 element"
)
s.add(obj)
s.setActiveDofs(self.toIndices(ee.activeLinks))
return s
def toIndices(self, items):
"""Returns link identifiers as link indices"""
if all(isinstance(v, int) for v in items):
return items
else:
robot = self.klamptModel()
for i, v in enumerate(items):
if not isinstance(v, int):
assert isinstance(
v, str), "Link identifiers must be int or str"
items[i] = robot.link(v).getIndex()
assert items[
i] >= 0, "Link %s doesn't exist in robot %s" % (
v, self.name)
return items
def toNames(self, items):
"""Returns link identifiers as link names"""
if all(isinstance(v, str) for v in items):
return items
else:
robot = self.klamptModel()
for i, v in enumerate(items):
if not isinstance(v, str):
assert isinstance(
v, int), "Link identifiers must be int or str"
assert v >= 0 and v < robot.numLinks(
), "Link %d is invalid for robot %s" % (v, self.name)
items[i] = robot.link(v).getName()
return items
def load(self, f):
"""Loads the info from a JSON file. f is a file object."""
import json
from ..io import loader
jsonobj = json.load(f)
for attr in [
'name', 'fn', 'parts', 'freeBase', 'endEffectors', 'properties'
]:
if attr not in jsonobj:
raise IOError("Loaded JSON object doesn't contain '" + attr +
"' key")
setattr(self, attr, jsonobj[attr])
ees = dict()
for (k, v) in self.endEffectors.items():
obj = None if v['ikObjective'] is None else loader.fromJson(
v['ikObjective'], 'IKObjective')
ees[k] = EndEffectorInfo(v['link'], v['activeLinks'], obj)
self.endEffectors = ees
def save(self, f):
"""Saves the info to a JSON file. f is a file object."""
import json
from ..io import loader
jsonobj = dict()
for attr in ['name', 'fn', 'parts', 'freeBase', 'properties']:
jsonobj[attr] = getattr(self, attr)
ees = dict()
for k, v in self.endEffectors.items():
obj = None if v.ikObjective is None else loader.toJson(
v.ikObjective)
ees[k] = {
'link': v.link,
'activeLinks': v.activeLinks,
'ikObjective': obj
}
jsonobj['endEffectors'] = ees
json.dump(jsonobj, f)
from klampt import WorldModel, vis
world = WorldModel()
world.readFile("world.xml")
vis.init("IPython")
vis.add("world", world)
vis.show()
#string = vis.nativeWindow().page()
#with open("vis.html", "w") as f:
# f.write(string)
from klampt import WorldModel
from klampt import vis
import klampt
import time
world = WorldModel()
world_file = "../data/simulation_test_worlds/drc_rough_terrain_world.xml"
if not world.readFile(world_file):
raise RuntimeError("Unable to load terrain model")
robot_file = "../data/robot_model/robosimian_caesar_new.rob"
if not world.readFile(robot_file):
raise RuntimeError("Unable to load robot model")
vis_window_id = vis.createWindow(world_file)
vis.setWindow(vis_window_id)
vis.add("world", world)
vp = vis.getViewport()
vis.setViewport(vp)
vis.autoFitCamera()
world.robot(0).setConfig([
12.621508747630084,
1.3060978650033888,
0.7271994997360561,
-0.18389666460947365,
-0.2336561986984183,
0.23915345995072382,
0.0,
import numpy as np
import time
import copy
import json
sys.path.append("../common")
import grasp
import grasp_database
from known_grippers import *
# physics simulation doesn't work for now, set it to False!
PHYSICS_SIMULATION = False
if __name__ == '__main__':
#load the robot / world file
fn = "world.xml"
world = WorldModel()
res = world.readFile(fn)
if not res:
print("Unable to read file", fn)
exit(0)
for i in range(world.numRigidObjects()):
obj = world.rigidObject(i)
# this will perform a reasonable center of mass / inertia estimate
m = obj.getMass()
m.estimate(obj.geometry(), mass=0.454, surfaceFraction=0.2)
obj.setMass(m)
plate_mass = world.rigidObject("plate").getMass()
plate_mass.setInertia([1, 0, 0, 0, 1, 0, 0, 0, 1])
world.rigidObject("plate").setMass(plate_mass)
from klampt import WorldModel
from klampt.io import resource
from klampt import vis
from klampt.model import trajectory
import planning
import math
# PROBLEM = '1a'
PROBLEM = '1b'
if __name__ == '__main__':
#load the robot / world file
fn = "problem1.xml"
world = WorldModel()
res = world.readFile(fn)
if not res:
print("Unable to read file", fn)
exit(0)
robot = world.robot(0)
#need to fix the spin joints somewhat
qmin, qmax = robot.getJointLimits()
for i in range(len(qmin)):
if qmax[i] - qmin[i] > math.pi * 2:
qmin[i] = -float('inf')
qmax[i] = float('inf')
robot.setJointLimits(qmin, qmax)
qstart = resource.get("start.config", world=world)
#add the world elements individually to the visualization
import sys
import time
import numpy as np
from Robot import Robot
from klampt import WorldModel
from Simmulation import Simulation
from functions import visualize
if len(sys.argv)<=1:
print "USAGE: kinematicSim.py [world_file]"
exit()
world = WorldModel()
world_file = sys.argv[1]
res = world.readFile(world_file)
if not res:
raise RuntimeError("Unable to load model " + world_file)
# np.random.seed(11)
np.random.seed(1711)
simulation = Simulation(world)
start_pc = [-0, 0, 0.05, 0, 0, 0]
goal_pc = [0.7, -0.5, 0.05, 0, 0, 0]
simulation.create(start_pc, goal_pc)
gamma = 0.99
agent = Robot(simulation, gamma)
if len(sys.argv) > 2 and sys.argv[2] == 'train':
epochs = 600
decay = 0.99
max_step = 3000
