def test_trajectory_editing():
traj = SE3Trajectory([0, 1], [se3.identity(), se3.identity()])
saved, result = resource.edit("se3 traj",
traj,
'SE3Trajectory',
'Some spatial trajectory',
world=w,
referenceObject=r.link(7))
traj = Trajectory([0, 1], [[0, 0, 0], [0, 0, 1]])
saved, result = resource.edit("point traj",
traj,
'auto',
'Some point trajectory',
world=w,
referenceObject=r.link(7))
traj = RobotTrajectory(r, [0, 1, 2], [q0, qrand, qrand2])
saved, result = resource.edit("robot traj",
traj,
'auto',
'Random robot trajectory',
world=w,
referenceObject=r)
vis.kill()
def launch_xform_viewer(robotname):
"""Launches a very simple program that spawns an object from one of the
databases.
It launches a visualization of the mvbb decomposition of the object, and corresponding generated poses.
It then spawns a hand and tries all different poses to check for collision
"""
world = WorldModel()
robot = make_moving_base_robot(robotname, world)
set_moving_base_xform(robot, se3.identity()[0], se3.identity()[1])
xform = resource.get("default_initial_%s.xform" % robotname,
description="Initial hand transform",
default=se3.identity(),
world=world,
doedit=False)
print "Transform:", xform
program = XFormVisualizer(world, xform)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.01)
vis.kill()
return
def __init__(self):
self.serviceThread = None
self.gripperController = None
self.j = None
self.limb = 'left'
self.controlSet = 'arm'
self.lastState = {}
self.plugin = None
# set initial values
self.baseSensedVelocity = [0.0, 0.0, 0.0]
self.baseCommandVelocity = [0.0, 0.0, 0.0]
self.log = False
# === joint control ===
self.jointControlRatio = 0.4
# == arm position ===
self.ArmPosition = [[0.0] * 7, [0.0] * 7]
self.robotEndEffectorPosition = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
self.robotEndEffectorTransform = [se3.identity(), se3.identity()]
self.gripperPosition = [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]]
self.kin = {}
# self.kin['left'] = baxter_kinematics('left')
# self.kin['right'] = baxter_kinematics('right')
self.last_driveVel = [0.0, 0.0, 0.0]
self.near_singularity = False
self.last_sigulatiry = False
# flag to mark the data
self.flag = 0
# open log file
timestr = time.strftime("%Y%m%d-%H%M%S")
"""self.csvfile_gamepad = open('data/gamepad_log' + timestr + '.csv', 'wb')
fieldnames = ['rstick', 'lstick', 'LB', 'RB', 'LT', 'RT', 'B', 'A', 'X', 'Y', 'Dpad_x', 'Dpad_y',
'jointAngles', 'gripperStatus', 'eePosition', 'eeTransformation',
'baseSensedVelocity', 'baseCommandVelocity', 'time', 'Flag']
self.gamepad_csv = csv.DictWriter(self.csvfile_gamepad, fieldnames=fieldnames)
self.gamepad_csv.writeheader()
self.switch_pub = None"""
# == Autonomous ==
self.lastPick = "Out"
self.isAuto = False
rospy.Subscriber('Detect', String, callback_pos)
rospy.Subscriber('/MarkerArray', MarkerArray, callback_state)
rospy.Subscriber('/Offsets', Pose, callback_offsets)
rospy.Subscriber('/PickID', Int64, callback_pick)
rospy.Subscriber('/PlaceID', Int64, callback_place)
rospy.Subscriber('/Command', String, callback_command)
rospy.Subscriber('/GripperSpeed', Int64, callback_speed)
rospy.Subscriber('/GripperClosePercent', Int64, callback_percent)
rospy.Subscriber('/GripperState', String, callback_gripper)
rospy.Subscriber('/robot/limb/left/endpoint_state', EndpointState, self.callback_pose)
# rospy.Subscriber('/robot/limb/left/endpoint_state/pose', Pose, callback_pose)
self.pub_l = rospy.Publisher('/left/UbirosGentle', Int8, queue_size=1)
self.pub_r = rospy.Publisher('/right/UbirosGentle', Int8, queue_size=1)
self.pub_state = rospy.Publisher('/CurrentStatus', String, queue_size=1)
def __init__(self):
print("*****************************************************************************")
self.serviceThread = None
#self.gripperController = None
self.j = None
self.limb = 'left'
self.controlSet = 'arm'
self.lastState = {}
self.plugin = None
# set initial values
self.baseSensedVelocity = [0.0, 0.0, 0.0]
self.baseCommandVelocity = [0.0, 0.0, 0.0]
self.log = False
# === joint control ===
self.jointControlRatio = 0.4
# == arm position ===
self.ArmPosition = [[0.0]*7, [0.0]*7]
self.robotEndEffectorPosition = [[0.0, 0.0, 0.0],[0.0, 0.0, 0.0]]
self.robotEndEffectorTransform = [se3.identity(),se3.identity()]
#self.gripperPosition = [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]]
self.gripperPosition = [0,0]
self.kin = {}
# self.kin['left'] = baxter_kinematics('left')
# self.kin['right'] = baxter_kinematics('right')
self.last_driveVel = [0.0, 0.0, 0.0]
self.near_singularity = False
self.last_sigulatiry = False
# flag to mark the data
self.flag = 0
# open log file
timestr = time.strftime("%Y%m%d-%H%M%S")
self.csvfile_gamepad = open('data/gamepad_log' + timestr + '.csv', 'wb')
fieldnames = ['rstick', 'lstick', 'LB', 'RB', 'LT', 'RT', 'B', 'A', 'X', 'Y', 'Dpad_x', 'Dpad_y',
'jointAngles', 'gripperStatus', 'eePosition', 'eeTransformation',
'baseSensedVelocity', 'baseCommandVelocity', 'time', 'Flag']
self.gamepad_csv = csv.DictWriter(self.csvfile_gamepad, fieldnames=fieldnames)
self.gamepad_csv.writeheader()
self.switch_pub = None
#Publishers for Softhands
self.pub_r = rospy.Publisher('/left/UbirosGentle', Int8, queue_size = 1)
self.pub_l = rospy.Publisher('/right/UbirosGentle', Int8, queue_size = 1)
self.pubPro_l = rospy.Publisher('/left/UbirosGentlePro', Int8, queue_size = 1)
self.pubPro_r = rospy.Publisher('/right/UbirosGentlePro', Int8, queue_size = 1)
self.angles_sub = rospy.Subscriber('/relaxed_ik/joint_angle_solutions', Float32[], self.callback_angles)
self.grip_l = 0
self.grip_r = 0
self.camera_count = 1
def shift_grasp(amt):
global cur_object,cur_grasp,shown_grasps,db
for i,grasp in shown_grasps:
grasp.remove_from_vis("grasp"+str(i))
shown_grasps = []
all_grasps = db.object_to_grasps[db.objects[cur_object]]
if amt == None:
cur_grasp = -1
else:
cur_grasp += amt
if cur_grasp >= len(all_grasps):
cur_grasp = -1
elif cur_grasp < -1:
cur_grasp = len(all_grasps)-1
if cur_grasp==-1:
for i,grasp in enumerate(all_grasps):
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp"+str(i))
shown_grasps.append((i,grasp))
print("Showing",len(shown_grasps),"grasps")
else:
grasp = all_grasps[cur_grasp]
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp"+str(cur_grasp))
Tbase = grasp.ik_constraint.closestMatch(*se3.identity())
g.add_to_vis(robot,animate=False,base_xform=Tbase)
robot.setConfig(grasp.set_finger_config(robot.getConfig()))
shown_grasps.append((cur_grasp,grasp))
if grasp.score is not None:
vis.addText("score","Score %.3f"%(grasp.score,),position=(10,10))
else:
vis.addText("score","",position=(10,10))
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 launch_test_mvbb_grasps(robotname, box_db, links_to_check = None):
"""Launches a very simple program that spawns a box with dimensions specified in boxes_db.
"""
world = WorldModel()
world.loadElement("data/terrains/plane.env")
robot = make_moving_base_robot(robotname, world)
xform = resource.get("default_initial_%s.xform" % robotname, description="Initial hand transform",
default=se3.identity(), world=world, doedit=False)
for box_dims, poses in box_db.db.items():
if world.numRigidObjects() > 0:
world.remove(world.rigidObject(0))
obj = make_box(world,
box_dims[0],
box_dims[1],
box_dims[2])
poses_filtered = []
R,t = obj.getTransform()
obj.setTransform(R, [0, 0, box_dims[2]/2.])
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p_T_h = np.array(se3.homogeneous(xform))
p_T_h[2][3] += 0.02
for pose in poses:
w_T_p = w_T_o.dot(pose)
w_T_h_des_se3 = se3.from_homogeneous(w_T_p.dot(p_T_h))
if CollisionTestPose(world, robot, obj, w_T_h_des_se3):
pose_pp = se3.from_homogeneous(pose)
t_pp = pose_pp[1]
q_pp = so3.quaternion(pose_pp[0])
q_pp = [q_pp[1], q_pp[2], q_pp[3], q_pp[0]]
print "Pose", t_pp + q_pp, "has been filtered since it is in collision for box", box_dims
elif w_T_p[2][3] <= 0.:
print "Pose", t_pp + q_pp, "has been filtered since it penetrates with table"
else:
poses_filtered.append(pose)
print "Filtered", len(poses)-len(poses_filtered), "out of", len(poses), "poses"
# embed()
# create a hand emulator from the given robot name
module = importlib.import_module('plugins.' + robotname)
# emulator takes the robot index (0), start link index (6), and start driver index (6)
program = FilteredMVBBTesterVisualizer(box_dims,
poses_filtered,
world,
p_T_h,
module,
box_db,
links_to_check)
vis.setPlugin(None)
vis.setPlugin(program)
program.reshape(800, 600)
vis.show()
# this code manually updates the visualization
while vis.shown():
time.sleep(0.1)
return
def sample_object_pose_table(obj,stable_fs,bmin,bmax):
"""Samples a transform of the object so that it lies on in the given
bounding box bmin,bmax.
Args:
obj (RigidObjectModel)
stable_fs (list of lists): giving the stable faces of the object,
as in MP2.
bmin,bmax (3-vectors): the bounding box of the area in which the
objects should lie.
"""
#TODO: fill me out, problem 1a
table_height = bmin[2] + 0.01
face = random.choice(stable_fs)
normal = np.cross(face[1] - face[0],face[2]-face[0])
normal = normal / np.linalg.norm(normal)
centroid = np.sum(face,axis=0)/len(face)
#TODO:...
R = so3.vector_rotation(normal, [0, 0, -1])
bb = obj.geometry().getBBTight()
# print(bb)
t = [,,table_height - centroid[2]]
# print(R)
# print(centroid)
obj.setTransform(*se3.identity())
def edit_template(world):
"""Shows how to pop up a visualization window with a world in which the robot configuration and a transform can be edited"""
#add the world to the visualizer
vis.add("world",world)
xform = se3.identity()
vis.add("transform",xform)
robotPath = ("world",world.robot(0).getName()) #compound item reference: refers to robot 0 in the world
vis.edit(robotPath)
vis.edit("transform")
#This prints how to get references to items in the visualizer
print("Visualization items:")
vis.listItems(indent=2)
vis.setWindowTitle("Visualization editing test")
if not MULTITHREADED:
vis.loop(setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#TODO: you may modify the world here.
vis.unlock()
time.sleep(0.01)
print("Resulting configuration",vis.getItemConfig(robotPath))
print("Resulting transform (config)",vis.getItemConfig("transform")) # this is a vector describing the item parameters
xform = list(xform) #convert se3 element from tuple to list
config.setConfig(xform,vis.getItemConfig("transform"))
print("Resulting transform (se3)",xform)
#quit the visualization thread nicely
vis.kill()
def shift_grasp(amt, data=data):
for i, grasp in data['shown_grasps']:
grasp.remove_from_vis("grasp" + str(i))
data['shown_grasps'] = []
all_grasps = db.object_to_grasps[db.objects[data['cur_object']]]
if amt == None:
data['cur_grasp'] = -1
else:
data['cur_grasp'] += amt
if data['cur_grasp'] >= len(all_grasps):
data['cur_grasp'] = -1
elif data['cur_grasp'] < -1:
data['cur_grasp'] = len(all_grasps) - 1
if data['cur_grasp'] == -1:
for i, grasp in enumerate(all_grasps):
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp" + str(i))
data['shown_grasps'].append((i, grasp))
print("Showing", len(data['shown_grasps']), "grasps")
else:
grasp = all_grasps[data['cur_grasp']]
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp" + str(data['cur_grasp']))
Tbase = grasp.ik_constraint.closestMatch(*se3.identity())
g.add_to_vis(robot, animate=False, base_xform=Tbase)
robot.setConfig(grasp.set_finger_config(robot.getConfig()))
data['shown_grasps'].append((data['cur_grasp'], grasp))
if grasp.score is not None:
vis.addText("score",
"Score %.3f" % (grasp.score, ),
position=(10, 10))
else:
vis.addText("score", "", position=(10, 10))
def make_object(object_set,objectname,world):
"""Adds an object to the world using its geometry / mass properties
and places it in a default location (x,y)=(0,0) and resting on plane."""
for pattern in object_geom_file_patterns[object_set]:
objfile = pattern%(objectname,)
objmass = object_masses[object_set].get('mass',default_object_mass)
f = open(object_template_fn,'r')
pattern = ''.join(f.readlines())
f.close()
f2 = open("temp.obj",'w')
f2.write(pattern % (objfile,objmass))
f2.close()
nobjs = world.numRigidObjects()
if world.loadElement('temp.obj') < 0 :
continue
assert nobjs < world.numRigidObjects(),"Hmm... the object didn't load, but loadElement didn't return -1?"
obj = world.rigidObject(world.numRigidObjects()-1)
obj.setTransform(*se3.identity())
bmin,bmax = obj.geometry().getBB()
T = obj.getTransform()
spacing = 0.006
T = (T[0],vectorops.add(T[1],(-(bmin[0]+bmax[0])*0.5,-(bmin[1]+bmax[1])*0.5,-bmin[2]+spacing)))
obj.setTransform(*T)
obj.appearance().setColor(0.2,0.5,0.7,1.0)
obj.setName(objectname)
return obj
raise RuntimeError("Unable to load object name %s from set %s"%(objectname,object_set))
def to_PointCloud2(klampt_pc, frame='0', stamp='now'):
"""Converts a Klampt Geometry3D or PointCloud to a ROS PointCloud2 message.
If it's a Geometry3D, the points are first transformed by the current
transform.
frame is the ROS frame in the header. Default '0' indicates no frame.
stamp is the ROS timestamp, or 'now', or a numeric value.
"""
from klampt import Geometry3D, PointCloud
from sensor_msgs.msg import PointField
from sensor_msgs import point_cloud2
from std_msgs.msg import Header
if stamp == 'now':
stamp = rospy.Time.now()
elif isinstance(stamp, (int, float)):
stamp = rospy.Time(stamp)
if isinstance(klampt_pc, Geometry3D):
pc = klampt_pc.getPointCloud()
T = klampt_pc.getCurrentTransform()
if T != se3.identity():
pc.transform(*T)
klampt_pc = pc
if not isinstance(klampt_pc, PointCloud):
raise TypeError("Must provide a klampt.PointCloud object")
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
for i in range(len(klampt_pc.propertyNames)):
fields.append(
PointField(klampt_pc.propertyNames[i], 12 + i * 4,
PointField.FLOAT32, 1))
import numpy
points = numpy.array(klampt_pc.vertices).reshape(
(klampt_pc.numPoints(), 3))
alldata = points
if len(fields) > 3:
properties = numpy.array(klampt_pc.properties).reshape(
(klampt_pc.numPoints(), len(klampt_pc.propertyNames)))
alldata = numpy.hstack((points, properties))
seq_no = 0
try:
seq_no = klampt_pc.getSetting('id')
except Exception:
pass
ros_pc = point_cloud2.create_cloud(
Header(stamp=stamp, seq=seq_no, frame_id=frame), fields, alldata)
try:
w = klampt_pc.getSetting('width')
h = klampt_pc.getSetting('height')
ros_pc.height = int(w)
ros_pc.height = int(h)
except:
ros_pc.height = 1
ros_pc.width = klampt_pc.numPoints()
return ros_pc
def sample_fixed_grasp(self, Tref=None):
"""For non-fixed grasps, samples a Grasp with a fully specified base transform."""
if Tref is None:
Tref = se3.identity()
Tfixed = self.ik_constraint.closestMatch(*Tref)
ik2 = self.ik_constraint.copy()
ik2.setFixedTransform(self.ik_constraint.link(), *Tfixed)
return Grasp(ik2, self.finger_links, self.finger_config, self.contacts,
self.score)
def __init__(self,world,robot,object,Tobject_gripper,gripper,goal_bounds):
MultiStepPlanner.__init__(self,['placement','qplace','qpreplace','retract','transfer'])
self.world=world
self.robot=robot
self.object=object
self.Tobject_gripper=Tobject_gripper
self.gripper=gripper
self.goal_bounds=goal_bounds
object.setTransform(*se3.identity())
self.objbb = object.geometry().getBBTight()
self.qstart = robot.getConfig() #lift
def object_match(self, object_source, object_target):
"""Determine whether object_source is a match to object_target.
If they match, return a transform from the reference frame of
object_source to object_target. Otherwise, return None.
Default implementation: determine whether the name of
object_source matches object_target.name or object_target.getName()
exactly.
"""
if object_source != _object_name(object_target):
return se3.identity()
return None
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)
def init(self, scene, object, hints):
"""Needs object to contain a structured PointCloud."""
if not isinstance(object, (RigidObjectModel, Geometry3D, PointCloud)):
print(
"Need to pass an object as a RigidObjectModel, Geometry3D, or PointCloud"
)
return False
if isinstance(object, RigidObjectModel):
return self.init(scene, object.geometry(), hints)
pc = None
xform = None
if isinstance(object, Geometry3D):
pc = object.getPointCloud()
xform = object.getCurrentTransform()
else:
pc = object
xform = se3.identity()
self.pc = pc
self.pc_xform = xform
#now look through PC and find flat parts
#do a spatial hash
from collections import defaultdict
estimation_knn = 6
pts = numpy_convert.to_numpy(pc)
N = pts.shape[0]
positions = pts[:, :3]
normals = np.zeros((N, 3))
indices = (positions * (1.0 / self._gripper.opening_span)).astype(int)
pt_hash = defaultdict(list)
for i, (ind, p) in enumerate(zip(indices, positions)):
pt_hash[ind].append((i, p))
options = []
for (ind, iplist) in pt_hash.items():
if len(iplist) < estimation_knn:
pass
else:
pindices = [ip[0] for ip in iplist]
pts = [ip[1] for ip in iplist]
c, n = fit_plane_centroid(pts)
if n[2] < 0:
n = vectorops.mul(n, -1)
verticality = self.vertical_penalty(math.acos(n[2]))
var = sum(
vectorops.dot(vectorops.sub(p, c), n)**2 for p in pts)
roughness = self.roughness_penalty(var)
options.append((cn, n, verticality + roughness))
if len(options) == 0:
return False
self.options = options.sorted(key=lambda x: -x[2])
self.index = 0
return True
def get_geometry(self,robot,qfinger=None,type='Group'):
"""Returns a Geometry of the gripper frozen at its configuration.
If qfinger = None, the current configuration is used. Otherwise,
qfinger is a finger configuration.
type can be 'Group' (most general and fastest) or 'TriangleMesh'
(compatible with Jupyter notebook visualizer.)
"""
if qfinger is not None:
q0 = robot.getConfig()
robot.setConfig(self.set_finger_config(q0,qfinger))
res = Geometry3D()
gripper_links = self.gripper_links if self.gripper_links is not None else [self.base_link] + self.descendant_links(robot)
if type == 'Group':
res.setGroup()
Tbase = robot.link(self.base_link).getTransform()
for i,link in enumerate(gripper_links):
Trel = se3.mul(se3.inv(Tbase),robot.link(link).getTransform())
g = robot.link(link).geometry().clone()
if not g.empty():
g.setCurrentTransform(*se3.identity())
g.transform(*Trel)
else:
print("Uh... link",robot.link(link).getName(),"has empty geometry?")
res.setElement(i,g)
if qfinger is not None:
robot.setConfig(q0)
return res
else:
import numpy as np
from klampt.io import numpy_convert
#merge the gripper parts into a static geometry
verts = []
tris = []
nverts = 0
for i,link in enumerate(gripper_links):
xform,(iverts,itris) = numpy_convert.to_numpy(robot.link(link).geometry())
verts.append(np.dot(np.hstack((iverts,np.ones((len(iverts),1)))),xform.T)[:,:3])
tris.append(itris+nverts)
nverts += len(iverts)
verts = np.vstack(verts)
tris = np.vstack(tris)
for t in tris:
assert all(v >= 0 and v < len(verts) for v in t)
mesh = numpy_convert.from_numpy((verts,tris),'TriangleMesh')
res.setTriangleMesh(mesh)
if qfinger is not None:
robot.setConfig(q0)
return res
def getRobotStatus(self):
T1 = self.serviceThread.eeGetter_left.get()
if T1 is None:
T1 = se3.identity()
R1,t1=T1
T2 = self.serviceThread.eeGetter_right.get()
if T2 is None:
T2 = se3.identity()
R2,t2=T2
self.baseSensedVelocity = self.serviceThread.baseVelocitySensed.get()
self.baseCommandVelocity = self.serviceThread.baseVelocityCommand.get()
self.ArmPosition[0] = self.serviceThread.ArmPosition_left.get()
self.ArmPosition[1] = self.serviceThread.ArmPosition_right.get()
self.robotEndEffectorPosition[0] = t1
self.robotEndEffectorPosition[1] = t2
self.robotEndEffectorTransform[0] = T2
self.robotEndEffectorTransform[1] = T2
self.gripperPosition[0] = self.serviceThread.gripperGetter_left.get()
self.gripperPosition[1] = self.serviceThread.gripperGetter_right.get()
def loadedItem(self, fn, obj):
if fn in self.active:
print("klampt_browser: Re-loaded item", fn,
"so I'm first removing it")
self.remove(fn)
assert fn not in self.active
item = ResourceItem(obj)
self.active[fn] = item
item.plugin = GLVisualizationPlugin()
basename = os.path.basename(fn)
#determine whether it's being animated
if isinstance(obj, Trajectory) and len(obj.milestones) > 0:
d = len(obj.milestones[0])
if self.world.numRobots() > 0 and d == self.world.robot(
0).numLinks():
obj = RobotTrajectory(self.world.robot(0), obj.times,
obj.milestones)
robotpath = ('world', self.world.robot(0).getName())
item.animationBuddy = robotpath
elif d == 3:
item.plugin.add("anim_point", [0, 0, 0])
item.animationBuddy = "anim_point"
elif d == 12:
item.plugin.add("anim_xform", se3.identity())
item.animationBuddy = "anim_xform"
else:
print("klampt_browser: Can't interpret trajectory of length",
d)
elif isinstance(obj, MultiPath):
if self.world.numRobots() > 0:
robotpath = ('world', self.world.robot(0).getName())
item.animationBuddy = robotpath
item.plugin.add("world", self.world)
item.plugin.add(basename, obj)
item.plugin.addText("label", basename, position=(10, 10))
try:
type = vis.objectToVisType(obj, self.world)
except:
type = 'unknown'
if type in robot_override_types:
if self.world.numRobots() > 0:
path = ('world', self.world.robot(0).getName())
item.plugin.hide(path)
item.plugin.initialize()
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 shiftGrasp(amt):
global grasp, grasps, grasp_index
grasp_index += amt
if grasp_index >= len(grasps):
grasp_index = 0
elif grasp_index < 0:
grasp_index = len(grasps) - 1
print("Grasp", grasp_index)
grasp = grasps[grasp_index]
Tgripper = grasp.ik_constraint.closestMatch(*se3.identity())
source_gripper_model.setConfig(
orig_grasps[grasp_index].set_finger_config(
source_gripper_model.getConfig()))
source_gripper.add_to_vis(source_gripper_model,
animate=False,
base_xform=Tgripper)
grasp.add_to_vis("grasp")
def to_Mesh(klampt_mesh):
"""From a Klampt Geometry3D or TriangleMesh to a ROS Mesh"""
from klampt import Geometry3D,TriangleMesh
from shape_msgs.msg import MeshTriangle
if isinstance(klampt_mesh,Geometry3D):
mesh = klampt_mesh.getTriangleMesh()
T = klampt_mesh.getCurrentTransform()
if T != se3.identity():
mesh.transform(*T)
klampt_mesh = mesh
ros_mesh = Mesh()
for i in range(len(klampt_mesh.vertices)//3):
k = i*3
ros_mesh.vertices.append(Vector3(klampt_mesh.vertices[k],klampt_mesh.vertices[k+1],klampt_mesh.vertices[k+2]))
for i in range(len(klampt_mesh.indices)//3):
k = i*3
ros_mesh.triangles.append(MeshTriangle([klampt_mesh.indices[k],klampt_mesh.indices[k+1],klampt_mesh.indices[k+2]]))
return ros_mesh
def debug_stable_faces(obj, faces):
from klampt import vis, Geometry3D, GeometricPrimitive
from klampt.math import se3
import random
vis.createWindow()
obj.setTransform(*se3.identity())
vis.add("object", obj)
for i, f in enumerate(faces):
gf = GeometricPrimitive()
gf.setPolygon(np.stack(f).flatten())
color = (1, 0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(),
0.5)
vis.add("face{}".format(i),
Geometry3D(gf),
color=color,
hide_label=True)
vis.setWindowTitle("Stable faces")
vis.dialog()
def init(self, scene, object, hints):
"""Checks for either an exact match or if object_match(o,object)
exists"""
if _object_name(object) in self._object_to_grasps:
self._target_object = object
self._matching_object = _object_name(object)
self._matching_xform = se3.identity()
self._grasp_index = 0
return True
for o, g in self._object_to_grasps.items():
xform = self.object_match(o, object)
if xform is not None:
self._target_object = object
self._matching_object = o
self._matching_xform = xform
self._grasp_index = 0
return True
return False
def getDesiredCartesianPose(self,limb,device):
"""Returns a pair (R,t) of the desired EE pose if the limb should have
a cartesian pose message, or None if it should not.
- limb: either 'left' or 'right'
- device: an index of the haptic device
Implementation-wise, this reads from self.startTransforms and the deviceState
to determine the correct desired end effector transform. The delta
from devices[device]['deviceCurrentTransform'] to devices[device]['deviceInitialTransform']
is scaled, then offset by self.startTransforms[device]. (self.startTransforms is
the end effector transform when deviceInitialTransform is set)
"""
if limb=='left':
T = self.serviceThread.eeGetter_left.get()
else:
T = self.serviceThread.eeGetter_right.get()
if T is None:
T = se3.identity()
R,t=T
deviceState = self.haptic_client.deviceState
if deviceState == None: return T
dstate = deviceState[device]
if dstate.widgetInitialTransform[0] == None or self.startTransforms[device] == None: return T
Tcur = dstate.widgetCurrentTransform
T0 = dstate.widgetInitialTransform[0]
if T0 == None:
T0 = Tcur
#print "Button is down and mode is",dstate['mode']
#print dstate
assert T0 != None,"T0 is null"
assert Tcur != None,"Tcur is null"
relRot = so3.mul(Tcur[0],so3.inv(T0[0]))
relTrans = vectorops.sub(Tcur[1],T0[1])
#print "Rotation moment",so3.moment(relRot)
desRot = so3.mul(relRot,self.startTransforms[device][0])
desPos = vectorops.add(vectorops.mul(relTrans,hapticArmTranslationScaling),self.startTransforms[device][1])
#TEST: just use start rotation
#desRot = self.startTransforms[i][0]
return (desRot,desPos)
def shift_object(amt, data=data):
vis.remove(db.objects[data['cur_object']])
data['cur_object'] += amt
if data['cur_object'] >= len(db.objects):
data['cur_object'] = 0
elif data['cur_object'] < 0:
data['cur_object'] = len(db.objects) - 1
if data['cur_object'] >= w.numRigidObjects():
for i in range(w.numRigidObjects(), data['cur_object'] + 1):
o = db.objects[i]
fn = _find_object(o)
obj = w.makeRigidObject(o)
if not obj.loadFile(fn):
if not obj.geometry().loadFile(fn):
print("Couldn't load object", o, "from", fn)
exit(1)
obj.setTransform(*se3.identity())
obj = w.rigidObject(data['cur_object'])
vis.add(db.objects[data['cur_object']], obj)
shift_grasp(None)
def arrange_objects(world, obj_fns, bmin, bmax, interior=False):
global ycb_stable_faces
for o, obj in enumerate(obj_fns):
if obj not in ycb_stable_faces:
world.rigidObject(o).setTransform(*se3.identity())
ycb_stable_faces[obj] = stable_faces(world.rigidObject(o),
stability_tol=0.01,
merge_tol=0.05)
if len(ycb_stable_faces[obj]) == 0:
print("Object", obj,
"has no stable faces with robustness 0.01, trying 0.0")
ycb_stable_faces[obj] = stable_faces(world.rigidObject(o),
stability_tol=0.0,
merge_tol=0.05)
#debug_stable_faces(world.rigidObject(o),ycb_stable_faces[obj])
i = 0
while i < world.numRigidObjects():
faces = ycb_stable_faces[obj_fns[i]]
samples = 0
feasible = False
while not feasible and samples < 100:
sample_object_pose_table(world.rigidObject(i), faces, bmin, bmax)
samples += 1
feasible = True
for j in range(i):
if world.rigidObject(i).geometry().collides(
world.rigidObject(j).geometry()):
feasible = False
break
if feasible:
for j in range(world.numTerrains()):
if world.rigidObject(i).geometry().collides(
world.terrain(j).geometry()):
feasible = False
break
if not feasible:
world.remove(world.rigidObject(i))
print("Couldn't find feasible placement for", i, "th object")
else:
i += 1
def make_ycb_object(objectname, world, textured=True):
"""Adds an object to the world using its geometry / mass properties
and places it in a default location (x,y)=(0,0) and resting on plane."""
if objectname == 'random':
objectname = random.choice(ycb_objects)
if isinstance(objectname, int):
objectname = ycb_objects[objectname]
objmass = ycb_masses.get(objectname, DEFAULT_OBJECT_MASS)
if textured:
fn = os.path.join(YCB_DIR, objectname, 'textured.obj')
else:
fn = os.path.join(YCB_DIR, objectname, 'nontextured.ply')
obj = world.makeRigidObject(objectname)
if not obj.geometry().loadFile(fn):
raise IOError("Unable to read geometry from file", fn)
obj.setTransform(*se3.identity())
#set mass automatically
mass = obj.getMass()
surfaceFraction = 0.5
mass.estimate(obj.geometry(), objmass, surfaceFraction)
obj.setMass(mass)
bmin, bmax = obj.geometry().getBB()
T = obj.getTransform()
spacing = 0.006
T = (T[0],
vectorops.add(T[1], (-(bmin[0] + bmax[0]) * 0.5,
-(bmin[1] + bmax[1]) * 0.5, -bmin[2] + spacing)))
obj.setTransform(*T)
if textured:
obj.appearance().setColor(1, 1, 1, 1.0)
else:
obj.appearance().setColor(random.random(), random.random(),
random.random(), 1.0)
obj.setName(objectname)
return obj
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 EditTransform(value=None,xmin=None,xmax=None,labels=None,
klampt_widget=None,xform_name='edited_xform',axis_length=DEFAULT_AXIS_LENGTH,axis_width=DEFAULT_AXIS_WIDTH,
callback=None):
"""Creates a Jupyter widget for interactive editing of a rigid transform point
Args:
value (klampt.se3 element), optional: the initial value of the transform (klampt.se3 element).
If given as (R,t), the R and t members must be lists and will hold the edited values.
xmin/xmax (list of 3 floats, optional): the minimum and maximum of the translation
labels (list of strs, optional): if given, the labels of roll,pitch,yaw and x,y,z
klampt_widget (KlamptWidget, optional): the KlamptWidget visualization to update, or None if
you don't want to visualize the point.
xform_name (str, optional): the name of the xform in the visualization world to edit.
axis_length,axis_width (float, optional): the length and width of the visualized widget
callback (function, optional): a function callback((R,t)) called when a DOF's value has changed.
Returns:
VBox: a widget that can be displayed as you like
"""
if value is None:
value = se3.identity()
else:
if not isinstance(value,(tuple,list)):
raise ValueError("value must be a 2-element sequence")
if len(value) != 2:
raise ValueError("value must be a 2-element sequence")
if len(value[0]) != 9:
raise ValueError("value[0] must be a 9-element list")
if len(value[1]) != 3:
raise ValueError("value[1] must be a 3-element list")
if labels is None:
labels = ['roll','pitch','yaw','x','y','z']
if xmin is None:
xmin = [-5,-5,-5]
elif isinstance(xmin,(int,float)):
xmin = [xmin,xmin,xmin]
if xmax is None:
xmax = [5,5,5]
elif isinstance(xmax,(int,float)):
xmax = [xmax,xmax,xmax]
if len(xmin) != 3:
raise ValueError("xmin must be a 3-element list")
if len(xmax) != 3:
raise ValueError("xmax must be a 3-element list")
if klampt_widget:
klampt_widget.addXform(name=xform_name,length=axis_length,width=axis_width)
klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1])
rpy = list(so3.rpy(value[0]))
def _do_rotation_change(index,element):
rpy[index] = element
value[0][:] = so3.from_rpy(rpy)
if klampt_widget:
klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1])
if callback:
callback(value)
def _do_translation_change(index,element):
value[1][index] = element
if klampt_widget:
klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1])
if callback:
callback(value)
elems = []
for i in range(3):
elems.append(widgets.FloatSlider(description=labels[i],value=rpy[i],min=0,max=math.pi*2,step=0.001))
elems[-1].observe(lambda v,i=i:_do_rotation_change(i,v['new']),'value')
for i in range(3):
elems.append(widgets.FloatSlider(description=labels[3+i],value=value[1][i],min=xmin[i],max=xmax[i],step=0.001))
elems[-1].observe(lambda v,i=i:_do_translation_change(i,v['new']),'value')
return widgets.VBox(elems)
