def drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes if self.baseLinkNo >= 0: Tb = self.baseLink.getTransform() if self.taskType == "position": x = se3.apply(Tb,x) xdes = se3.apply(Tb,xdes) elif self.taskType == "po": x = se3.mul(Tb,x) xdes = se3.mul(Tb,xdes) glPointSize(6) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) if self.taskType == "position": glColor3f(1,0,0) #red glVertex3fv(xdes) glColor3f(1,0.5,0) #orange glVertex3fv(x) elif self.taskType == "po": glColor3f(1,0,0) #red glVertex3fv(xdes[1]) glColor3f(1,0.5,0) #orange glVertex3fv(x[1]) glEnd()
def __call__(self,controller): sim = self.sim xform = self.base_xform #turn this to false to turn off print statements ReflexController.verbose = True ReflexController.__call__(self,controller) #controller state machine #print "State:",state t_lift = 2 lift_traj_duration = 0.5 if self.state == 'idle': if sim.getTime() > 0.5: desired = se3.mul((so3.identity(),[0,0,-0.10]),xform) send_moving_base_xform_linear(controller,desired[0],desired[1],lift_traj_duration) self.state = 'lowering' elif self.state == 'lowering': if sim.getTime() > 1: #this is needed to stop at the current position in case there's some residual velocity controller.setPIDCommand(controller.getCommandedConfig(),[0.0]*len(controller.getCommandedConfig())) #the controller sends a command to the hand: f1,f2,f3,preshape self.hand.setCommand([0.2,0.2,0.2,0]) self.state = 'closing' elif self.state == 'closing': if sim.getTime() > t_lift: self.base_xform = get_moving_base_xform(self.sim.controller(0).model()) self.state = 'raising' elif self.state == 'raising': #the controller sends a command to the base after 1 s to lift the object t_traj = min(1, max(0, (sim.getTime() - t_lift) / lift_traj_duration)) desired = se3.mul((so3.identity(), [0, 0, 0.10 * t_traj]), xform) send_moving_base_xform_PID(controller, desired[0], desired[1]) if sim.getTime() > (t_lift + lift_traj_duration): self.state = 'raised'
def setGhostConfig(self,q,name="ghost",robot=0):
"""Sets the configuration of the ghost to q. If the ghost is named, place its name
in prefixname."""
if robot < 0 or robot >= self.world.numRobots():
raise ValueError("Invalid robot specified")
robot = self.world.robot(robot)
q_original = robot.getConfig()
if len(q) != robot.numLinks():
raise ValueError("Config must be correct size: %d != %d"%(len(q),robot.numLinks()))
robot.setConfig(q)
self.beginRpc(strict=False)
rpcs = []
for i in range(robot.numLinks()):
T = robot.link(i).getTransform()
p = robot.link(i).getParent()
if p>=0:
Tp = robot.link(p).getTransform()
T = se3.mul(se3.inv(Tp),T)
mat = se3.homogeneous(T)
#mat is now a 4x4 homogeneous matrix
linkname = name+robot.link(i).getName()
#send to the ghost link with name "name"...
self._do_rpc({'type':'set_transform','object':linkname,'matrix':[mat[0][0],mat[0][1],mat[0][2],mat[0][3],mat[1][0],mat[1][1],mat[1][2],mat[1][3],mat[2][0],mat[2][1],mat[2][2],mat[2][3],mat[3][0],mat[3][1],mat[3][2],mat[3][3]]})
self.endRpc(strict=False)
robot.setConfig(q_original) #restore original config
def controlfunc(controller):
"""Place your code here... for a more sophisticated controller you could also create a class where the control loop goes in the __call__ method."""
if not is_soft_hand:
#print the contact sensors... you can safely take this out if you don't want to use it
try:
f1_contact = [s.getMeasurements()[0] for s in f1_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f1_distal_takktile_sensors]
f2_contact = [s.getMeasurements()[0] for s in f2_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f2_distal_takktile_sensors]
f3_contact = [s.getMeasurements()[0] for s in f3_proximal_takktile_sensors] + [s.getMeasurements()[0] for s in f3_distal_takktile_sensors]
print "Contact sensors"
print " finger 1:",[int(v) for v in f1_contact]
print " finger 2:",[int(v) for v in f2_contact]
print " finger 3:",[int(v) for v in f3_contact]
except:
pass
if sim.getTime() < 0.05:
if is_soft_hand:
hand.setCommand([0.8])
else:
#the controller sends a command to the hand: f1,f2,f3,preshape
hand.setCommand([0.2,0.2,0.2,0])
t_lift = 1
lift_traj_duration = 0.5
if sim.getTime() > t_lift:
#the controller sends a command to the base after 1 s to lift the object
t_traj = min(1, max(0, (sim.getTime() - t_lift) / lift_traj_duration))
desired = se3.mul((so3.identity(), [0, 0, 0.10 * t_traj]), xform)
send_moving_base_xform_PID(controller, desired[0], desired[1])
#need to manually call the hand emulator
hand.process({},dt)
def objectTransform(self, x):
"""Given an arm configuration x, returns the object transformation"""
q = self.q0[:]
for i, xi in zip(self.hand.armIndices, x):
q[i] = xi
self.robot.setConfig(q)
Thand = self.robot.link(self.hand.link).getTransform()
return se3.mul(Thand, self.Tgrasp)
def loop_callback():
global base_xform
xform = vis.getItemConfig("base_xform")
base_xform = (xform[:9], xform[9:])
vis.lock()
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform))
vis.unlock()
def graspedObjectTransform(robot, hand, qrobot0, Tobj0, qrobot):
"""Given initial robot configuration qrobot0 and object transform Tobj0,
returns the object transformation corresponding to new configuration
qrobot assuming the object is rigidly attached to the hand"""
Tgrasp = graspedObjectTransform(robot, hand, qrobot0, Tobj0)
robot.setConfig(qrobot)
Thand = robot.link(hand.link).getTransform()
return se3.mul(Thand, Tgrasp)
def graspTransform(robot, hand, qrobot0, Tobj0):
"""Given initial robot configuration qrobot0 and object transform Tobj0,
returns the grasp transformation Tgrasp, which produces the object transform
via the composition Tobj = Thand * Tgrasp"""
robot.setConfig(qrobot0)
Thand0 = robot.link(hand.link).getTransform()
Tgrasp = se3.mul(se3.inv(Thand0), Tobj0)
return Tgrasp
def reset():
vis.lock()
robot.setConfig(q0)
base_link.setParentTransform(
*se3.mul(se3.inv(parent_xform), base_xform0))
vis.unlock()
vis.add("base_xform", base_xform0)
vis.edit("base_xform")
vis.setItemConfig("gripper", grob.getConfig())
def next(self):
"""Returns the next Grasp from the database."""
if self._matching_object is None:
return None
grasps = self._object_to_grasps[self._matching_object]
if self._grasp_index >= len(grasps):
self._matching_object = None
return None
grasp = grasps[self._grasp_index]
self._grasp_index += 1
return grasp.get_transformed(se3.mul(self._target_object.getTransform(),self._matching_xform))
def feasible(self):
finger_pad_links = ['gripper:Link_4', 'gripper:Link_6']
finger_pad_links_idx = []
for i in finger_pad_links:
idx = self.robot1.link(i).getIndex()
finger_pad_links_idx.append(idx)
# set object to correct (R, t) in world frame
Tworld_gripper = self.gripper_link1.getTransform()
Tworld_object = se3.mul(Tworld_gripper, self.Tobject_gripper)
self.object.setTransform(Tworld_object[0], Tworld_object[1])
# self collide
if self.robot1.selfCollides():
print('self collides')
return False
# collision between object and robot
for i in range(self.robot1.numLinks()):
# ignore the collisions between finger links and the object to grasp
if i in finger_pad_links_idx:
continue
if self.robot1.link(i).geometry().collides(self.object.geometry()):
print('hammer and robot', i)
return False
# collision between robot and world environment
for i in range(self.world1.numTerrains()):
for j in range(self.robot1.numLinks()):
if self.robot1.link(j).geometry().collides(
self.world1.terrain(i).geometry()):
print('robot and world')
return False
if self.object.geometry().collides(
self.world1.terrain(i).geometry()):
print('hammer and world')
return False
# collision between robot and objects in world
for i in range(self.world1.numRigidObjects()):
for j in range(self.robot1.numLinks()):
if self.world1.rigidObject(
i) == self.object and j in finger_pad_links_idx:
continue
if self.robot1.link(j).geometry().collides(
self.world1.rigidObject(i).geometry()):
print('robot and other objects')
return False
if self.object.geometry().collides(
self.world1.rigidObject(i).geometry()):
print('hammer and other objects')
return False
return True
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time, qstart, next_item_to_pick
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
if PROBLEM == '3c':
qstart = solved_trajectory.eval(t)
if PHYSICS_SIMULATION:
#sim.controller(0).setPIDCommand(qstart,solved_trajectory.deriv(t))
#sim.controller(0).setMilestone(qstart)
sim.controller(0).setLinear(qstart, sim_dt * 5)
sim.simulate(sim_dt)
sim.updateWorld()
else:
robot.setConfig(qstart)
during_transfer = trajectory_is_transfer.eval(t)[0]
if not was_grasping:
#pick up object
Tobject_grasp = se3.mul(
se3.inv(gripper_link.getTransform()),
obj.getTransform())
was_grasping = True
if during_transfer:
obj.setTransform(
*se3.mul(robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration():
executing_plan = False
solved_trajectory = None
next_item_to_pick += 1
else:
robot.setConfig(solved_trajectory.eval(t, 'loop'))
obj.setTransform(
*se3.mul(robot.link(9).getTransform(), Tobject_grasp))
def __call__(self, controller):
sim = self.sim
xform = self.base_xform
#turn this to false to turn off print statements
ReflexController.verbose = True
ReflexController.__call__(self, controller)
#controller state machine
#print "State:",state
t_lift = 2
lift_traj_duration = 0.5
if self.state == 'idle':
if sim.getTime() > 0.5:
desired = se3.mul((so3.identity(), [0, 0, -0.10]), xform)
send_moving_base_xform_linear(controller, desired[0],
desired[1], lift_traj_duration)
self.state = 'lowering'
elif self.state == 'lowering':
if sim.getTime() > 1:
#this is needed to stop at the current position in case there's some residual velocity
controller.setPIDCommand(controller.getCommandedConfig(),
[0.0] *
len(controller.getCommandedConfig()))
#the controller sends a command to the hand: f1,f2,f3,preshape
self.hand.setCommand([0.2, 0.2, 0.2, 0])
self.state = 'closing'
elif self.state == 'closing':
if sim.getTime() > t_lift:
self.base_xform = get_moving_base_xform(
self.sim.controller(0).model())
self.state = 'raising'
elif self.state == 'raising':
#the controller sends a command to the base after 1 s to lift the object
t_traj = min(1,
max(0, (sim.getTime() - t_lift) / lift_traj_duration))
desired = se3.mul((so3.identity(), [0, 0, 0.10 * t_traj]), xform)
send_moving_base_xform_PID(controller, desired[0], desired[1])
if sim.getTime() > (t_lift + lift_traj_duration):
self.state = 'raised'
def bumpTrajectory():
relative_xforms = []
robot.setConfig(self.refConfig)
for e in self.endeffectors:
xform = vis.getItemConfig("ee_"+robot.link(e).getName())
T = (xform[:9],xform[9:])
T0 = robot.link(e).getTransform()
Trel = se3.mul(se3.inv(T0),T)
print "Relative transform of",e,"is",Trel
relative_xforms.append(Trel)
bumpTraj = cartesian_trajectory.cartesian_bump(self.robot,self.traj,self.constraints,relative_xforms,ee_relative=True,closest=True)
assert bumpTraj != None
vis.animate(("world",world.robot(0).getName()),bumpTraj)
self.refresh()
def object_free(self,q):
"""Helper: returns true if the object is collision free at configuration q, if it is
attached to the gripper."""
self.robot.setConfig(q)
gripper_link = self.robot.link(self.gripper.base_link)
self.object.setTransform(*se3.mul(gripper_link.getTransform(),self.Tobject_gripper))
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
return False
for i in range(self.world.numRigidObjects()):
if i == self.object.index: continue
if self.object.geometry().collides(self.world.rigidObject(i).geometry()):
return False
return True
def trans_all2needle(Tlink_set, Toct, Tneedle, pcd_list):
"""
:param Tlink_set: {list} -- the robot configuration list
:param Toct: {tuple} -- the transformation matrix from world to OCT
:param Tneedle: {tuple} -- the transformation matrix from robot end effector to needle
:param pcd_list: {list} -- the point cloud list
:return: None
"""
pcd_needle_list = []
pcd_t = copy.deepcopy(pcd_list[0][1])
pcd_t.paint_uniform_color([0, 0.651, 0.929])
pcd_needle_list.append(pcd_t)
mesh_frame = create_mesh_coordinate_frame(0.0006, [0, 0, 0])
pcd_needle_list.append(mesh_frame)
for i in range(len(Tlink_set)):
Tneedle2oct = se3.mul(se3.inv(Tneedle),
se3.mul(se3.inv(Tlink_set[i]), Toct))
pcd_s_copy = copy.deepcopy(pcd_list[i][0])
pcd_s_copy.transform(se3.homogeneous(Tneedle2oct))
pcd_s_copy.paint_uniform_color([0.4 + i * 0.1, 0.706, 0])
pcd_needle_list.append(pcd_s_copy)
draw_geometries(pcd_needle_list)
def check_collision(q):
# set robot to current config
robot1.setConfig(q)
finger_pad_links_idx = []
for i in finger_pad_links:
idx = robot1.link(i).getIndex()
finger_pad_links_idx.append(idx)
Tworld_gripper = gripper_link1.getTransform()
Tworld_object = se3.mul(Tworld_gripper, Tobject_gripper)
object.setTransform(Tworld_object[0], Tworld_object[1])
# self collide
if robot1.selfCollides():
print('self collides')
return False
# collision between object and robot
for i in range(robot1.numLinks()):
if i in finger_pad_links_idx:
continue
if robot1.link(i).geometry().collides(object.geometry()):
print('hammer and robot', i)
return False
# collision between robot and world environment
for i in range(world1.numTerrains()):
for j in range(robot1.numLinks()):
if robot1.link(j).geometry().collides(
world1.terrain(i).geometry()):
print('robot and world')
return False
if object.geometry().collides(world1.terrain(i).geometry()):
print('hammer and world', i, world1.terrain(i).getName())
return False
# collision between robot and objects in world
for i in range(world1.numRigidObjects()):
for j in range(robot1.numLinks()):
if world1.rigidObject(
i) == object and j in finger_pad_links_idx:
continue
if robot1.link(j).geometry().collides(
world1.rigidObject(i).geometry()):
print('robot and other objects')
return False
if object.geometry().collides(world1.rigidObject(i).geometry()):
print('hammer and other objects')
return False
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 controlfunc(controller):
"""Place your code here... for a more sophisticated controller you could also create a class where the control loop goes in the __call__ method."""
if not is_soft_hand:
#print the contact sensors... you can safely take this out if you don't want to use it
try:
f1_contact = [
s.getMeasurements()[0]
for s in f1_proximal_takktile_sensors
] + [
s.getMeasurements()[0] for s in f1_distal_takktile_sensors
]
f2_contact = [
s.getMeasurements()[0]
for s in f2_proximal_takktile_sensors
] + [
s.getMeasurements()[0] for s in f2_distal_takktile_sensors
]
f3_contact = [
s.getMeasurements()[0]
for s in f3_proximal_takktile_sensors
] + [
s.getMeasurements()[0] for s in f3_distal_takktile_sensors
]
print "Contact sensors"
print " finger 1:", [int(v) for v in f1_contact]
print " finger 2:", [int(v) for v in f2_contact]
print " finger 3:", [int(v) for v in f3_contact]
except:
pass
if sim.getTime() < 0.05:
if is_soft_hand:
hand.setCommand([0.8])
else:
#the controller sends a command to the hand: f1,f2,f3,preshape
hand.setCommand([0.2, 0.2, 0.2, 0])
t_lift = 1
lift_traj_duration = 0.5
if sim.getTime() > t_lift:
#the controller sends a command to the base after 1 s to lift the object
t_traj = min(1,
max(0, (sim.getTime() - t_lift) / lift_traj_duration))
desired = se3.mul((so3.identity(), [0, 0, 0.10 * t_traj]), xform)
send_moving_base_xform_PID(controller, desired[0], desired[1])
#need to manually call the hand emulator
hand.process({}, dt)
def SimulationPoses(o_T_h_des, w_T_h, w_T_o):
#w_T_h end-effector in word frame
#h_T_o object in end-effector frame
#w_T_o pbject in word frame
if not isinstance(o_T_h_des, tuple):
o_T_h_des = se3.from_homogeneous(o_T_h_des)
if not isinstance(w_T_h, tuple):
w_T_h = se3.from_homogeneous(w_T_h)
if not isinstance(w_T_o, tuple):
w_T_o = se3.from_homogeneous(w_T_o)
posedict = {}
posedict['desired'] = [se3.inv(o_T_h_des)]
posedict['actual'] = [se3.mul(se3.inv(w_T_h), w_T_o)]
return posedict
def all_linkgeom_transforms(self, thetas):
"""Compute transform matrix for all active links for all configurations.
Return a list (for each configuration) of list (for each active link) of (R, t) tuples
"""
trans = []
mtrans = []
for q in thetas:
self.robot.setConfig(q)
tmp = []
for _, _, geom_idx, _ in self.geom_cache['link']:
tmp.append(self.robot.link(geom_idx).getTransform())
trans.append(tmp)
# for mounted
tmp = []
for link_idx, relT, _, _ in self.geom_cache['mount']:
tmp.append(se3.mul(self.robot.link(link_idx).getTransform(), relT))
mtrans.append(tmp)
return trans, mtrans
def setGhostConfig(self, q, name="ghost", robot=0):
"""Sets the configuration of the ghost to q. If the ghost is named, place its name
in prefixname."""
if robot < 0 or robot >= self.world.numRobots():
raise ValueError("Invalid robot specified")
robot = self.world.robot(robot)
q_original = robot.getConfig()
if len(q) != robot.numLinks():
raise ValueError("Config must be correct size: %d != %d" %
(len(q), robot.numLinks()))
robot.setConfig(q)
self.beginRpc(strict=False)
rpcs = []
for i in range(robot.numLinks()):
T = robot.link(i).getTransform()
p = robot.link(i).getParent()
if p >= 0:
Tp = robot.link(p).getTransform()
T = se3.mul(se3.inv(Tp), T)
mat = se3.homogeneous(T)
#mat is now a 4x4 homogeneous matrix
linkname = name + robot.link(i).getName()
#send to the ghost link with name "name"...
self._do_rpc({
'type':
'set_transform',
'object':
linkname,
'matrix': [
mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0],
mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1],
mat[2][2], mat[2][3], mat[3][0], mat[3][1], mat[3][2],
mat[3][3]
]
})
self.endRpc(strict=False)
robot.setConfig(q_original) #restore original config
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv, T)
if self.taskType == 'po':
x = (T[0], se3.apply(T, self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T, self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType " + self.taskType)
return x
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv,T)
if self.taskType == 'po':
x = (T[0],se3.apply(T,self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T,self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType "+self.taskType)
return x
def make_grasp_map(world, id):
"""Estimates a grasp quality and regression map for a sensor image.
Input:
world (WorldModel): contains a robot and a sensor
Output: a 4-channel numpy array of size (w,h,3) with
w x h the sensor dimensions and all values in the range [0,1].
The first channel encodes the quality.
The second encodes the grasp opening amount.
The third and fourth encode the orientation of the grasp
relative to the camera frame, as a heading and elevation.
Make
sure to note how you've transformed these quantities to
[0,1]! These will be needed for decoding.
"""
robot = world.robot(0)
sensor = robot.sensor(0)
sensor_xform = sensing.get_sensor_xform(sensor, robot)
w = int(sensor.getSetting("xres"))
h = int(sensor.getSetting("yres"))
#You'll be filling this image out
result = np.zeros((h, w, 4))
result[:, :, 3] = 0.5
#this shows how to go from a point in space to a pixel
# point = (0,0,0)
# projected = sensing.camera_project(sensor,robot,point)
# if projected is None:
# pass
# else:
# x,y,z = projected
# if x < 0 or x > w or y < 0 or y > h:
# pass
# else:
# result[int(y),int(x),0]=1
# result[50,10,0]=1
#load the gripper info and grasp database
source_gripper = robotiq_85
global gripper_robot, gripper_world, grasp_db
if gripper_robot is None:
gripper_world = WorldModel()
if not gripper_world.readFile(source_gripper.klampt_model):
raise ValueError("Unable to read gripper file " +
source_gripper.klampt_model)
gripper_robot = gripper_world.robot(0)
gripper_geom_open = source_gripper.get_geometry(gripper_robot,
source_gripper.open_config)
if grasp_db is None:
db = grasp_database.GraspDatabase(source_gripper)
if not db.load("../data/grasps/robotiq_85_more_sampled_grasp_db.json"):
raise RuntimeError("Can't load grasp database?")
grasp_db = db
for i in range(world.numRigidObjects()):
obj = world.rigidObject(i)
ycb_obj = obj.getName()
if ycb_obj not in grasp_db.object_to_grasps:
print("Can't find object", ycb_obj, "in database")
print("Database only contains objects", grasp_db.objects)
raise ValueError()
grasps = grasp_db.object_to_grasps[ycb_obj]
gripper_tip = vectorops.madd(source_gripper.center,
source_gripper.primary_axis,
source_gripper.finger_length)
num_grasps_valid = 0
for gindex, g in enumerate(grasps):
#TODO: problem 1B: do collision testing of gripper_geom
found_approach = False
if (id, ycb_obj, gindex) in grasps_feasible:
found_approach = grasps_feasible[(id, ycb_obj, gindex)]
else:
#this is the Geometry3D corresponding to the robot at the opening configuration
gripper_geom = source_gripper.get_geometry(
gripper_robot, g.finger_config)
local_pt, world_pt = g.ik_constraint.getPosition()
local_axis, world_axis = g.ik_constraint.getRotationAxis()
#TODO: put your code here
R = so3.vector_rotation(source_gripper.primary_axis,
world_axis)
t = vectorops.sub(world_pt, source_gripper.center)
gripper_geom_open.setCurrentTransform(R, t)
non_collision = True
for i in range(world.numRigidObjects()):
if i == gripper_robot.getID():
continue
if gripper_geom_open.collides(
world.rigidObject(i).geometry()):
non_collision = False
break
found_approach = True if non_collision else False
if found_approach:
gripper_geom.setCurrentTransform(R, t)
for i in range(world.numRigidObjects()):
if i == gripper_robot.getID() or i == obj.getID():
continue
if gripper_geom.collides(
world.rigidObject(i).geometry()):
found_approach = False
break
grasps_feasible[(id, ycb_obj, gindex)] = found_approach
if not found_approach:
continue
num_grasps_valid += 1
Tfixed = g.ik_constraint.closestMatch(*se3.identity())
Tworld = se3.mul(obj.getTransform(), Tfixed)
gworld = se3.apply(Tworld, gripper_tip)
projected = sensing.camera_project(sensor, robot, gworld)
if projected is not None:
x, y, z = projected
x = int(x)
y = int(y)
if x < 0 or x >= w or y < 0 or y >= h:
continue
gripper_opening_axis = so3.apply(Tworld[0],
source_gripper.secondary_axis)
gripper_opening_axis_cam = so3.apply(so3.inv(sensor_xform[0]),
gripper_opening_axis)
if gripper_opening_axis_cam[1] < 0:
gripper_opening_axis_cam = vectorops.mul(
gripper_opening_axis_cam, -1)
gripper_axis_heading = math.atan2(gripper_opening_axis_cam[1],
gripper_opening_axis_cam[0])
xy = vectorops.norm(gripper_opening_axis_cam[:2])
if xy < 1e-7:
gripper_axis_elevation = math.pi * 0.5
else:
gripper_axis_elevation = math.atan(
gripper_opening_axis_cam[2] / xy)
score0 = math.exp(-g.score)
window = 10
std = 5
for u in range(-window, window + 1):
if y + u < 0 or y + u >= h: continue
for v in range(-window, window + 1):
if x + v < 0 or x + v >= w: continue
score = score0 * math.exp(-(u**2 + v**2) /
(2 * std**2))
if score > result[y + u, x + v, 0]:
result[y + u, x + v, :] = [
score,
source_gripper.config_to_opening(
g.finger_config),
gripper_axis_heading / math.pi,
gripper_axis_elevation / math.pi + 0.5
]
print("{}/{} grasps valid for object {}".format(
num_grasps_valid, len(grasps), ycb_obj))
return result
def geometry_to_povray(appearance, geometry, object, transform, properties):
if get_property_yes(properties, [geometry, object], "hide"):
return []
#analyze appearance
tex = get_property(properties, [geometry, object], 'texture')
if tex is None:
tex_params = []
#pigment
pigment = get_property(properties, [geometry, object], 'pigment')
if pigment is None:
transmit = 1. - appearance.getColor()[3]
pigment = vp.Pigment(*[
'color',
list(appearance.getColor())[0:3], 'transmit',
get_property(properties, [geometry, object], 'ambient',
transmit)
])
tex_params.append(pigment)
#finish
finish = get_property(properties, [geometry, object], 'finish')
if finish is None:
finish=vp.Finish(*['ambient',get_property(properties,[geometry,object],'ambient',0.2), \
'diffuse',get_property(properties,[geometry,object],'diffuse',0.7), \
'phong',get_property(properties,[geometry,object],'phong',1.), \
'phong_size',get_property(properties,[geometry,object],'phong_size',50)])
tex_params.append(finish)
#normal
normal = get_property(properties, [geometry, object], 'normal')
if normal is not None:
tex_params.append(normal)
#texture
tex = vp.Texture(*tex_params)
#create geometry
ret = []
if transform is None:
transform = geometry.getCurrentTransform()
else:
transform = se3.mul(transform, geometry.getCurrentTransform())
if geometry.type() == "GeometricPrimitive":
prim = geometry.getGeometricPrimitive()
if get_property_yes(properties, [prim, geometry, object], "hide"):
return ret
if prim.type == "Point":
rad = get_property(properties, [prim, geometry, object], "radius")
if rad is not None:
mesh_param = [se3.apply(transform, prim.properties[0:3]), rad]
mesh_param.append(tex)
mesh = vp.Sphere(*mesh_param)
ret.append(mesh)
elif prim.type == "Sphere":
mesh_param = [
se3.apply(transform, prim.properties[0:3]), prim.properties[3]
]
mesh_param.append(tex)
mesh = vp.Sphere(*mesh_param)
ret.append(mesh)
elif prim.type == "Segment":
rad = get_property(properties, [prim, geometry, object], "radius")
if rad is not None:
mesh_param = [
se3.apply(transform, prim.properties[0:3]),
se3.apply(transform, prim.properties[3:6]), rad
]
mesh_param.append(tex)
mesh = vp.Cylinder(*mesh_param)
ret.append(mesh)
elif prim.type == "AABB":
mesh_param = [
se3.apply(transform, prim.properties[0:3]),
se3.apply(transform, prim.properties[3:6])
]
mesh_param.append(tex)
mesh = vp.Box(*mesh_param)
ret.append(mesh)
elif geometry.type() == "Group":
for idElem in range(geometry.numElements()):
elem = geometry.getElement(idElem)
elem.getCurrentTransform()
ret += geometry_to_povray(appearance=appearance,
geometry=elem,
object=object,
transform=transform,
properties=properties)
elif geometry.type() == "TriangleMesh":
tm = geometry.getTriangleMesh()
if get_property_yes(properties, [geometry, object], "smooth"):
vss = [
se3.apply(transform, tuple(tm.vertices[i * 3:i * 3 + 3]))
for i in range(len(tm.vertices) // 3)
]
iss = [
tuple(tm.indices[i * 3:i * 3 + 3])
for i in range(len(tm.indices) // 3)
]
mesh_param = [
vp.VertexVectors(*([len(vss)] + vss)),
vp.FaceIndices(*([len(iss)] + iss))
]
mesh_param.append(tex)
mesh = vp.Mesh2(*mesh_param)
else:
vss = [
se3.apply(transform, tuple(tm.vertices[i * 3:i * 3 + 3]))
for i in range(len(tm.vertices) // 3)
]
iss = [
tuple(tm.indices[i * 3:i * 3 + 3])
for i in range(len(tm.indices) // 3)
]
mesh_param = [
vp.Triangle(vss[it[0]], vss[it[1]], vss[it[2]]) for it in iss
]
mesh_param.append(tex)
mesh = vp.Mesh(*mesh_param)
ret.append(mesh)
elif geometry.type() == "VolumeGrid":
from skimage import measure
import numpy as np
grid = geometry.getVolumeGrid()
volume = np.reshape(np.array(list(grid.values)), tuple(grid.dims))
spacing = [
(b - a) / d
for a, b, d in zip(grid.bbox[0:3], grid.bbox[3:6], grid.dims[0:3])
]
vss, iss, nss, _ = measure.marching_cubes_lewiner(volume,
level=0.,
spacing=spacing)
vss += np.expand_dims(np.array(grid.bbox[0:3]).T, 0)
vss = [vss[it, :].tolist() for it in range(vss.shape[0])]
iss = [iss[it, :].tolist() for it in range(iss.shape[0])]
nss = [nss[it, :].tolist() for it in range(nss.shape[0])]
mesh_param = [
vp.VertexVectors(*([len(vss)] + vss)),
vp.NormalVectors(*([len(nss)] + nss)),
vp.FaceIndices(*([len(iss)] + iss))
]
mesh_param.append(tex)
mesh = vp.Mesh2(*mesh_param)
ret.append(mesh)
elif geometry.type() == "PointCloud":
cloud_param = []
cloud = geometry.getPointCloud()
rad = get_property(properties, [cloud, geometry, object], "radius")
for id in range(len(cloud.vertices) // 3):
cloud_param.append(
vp.Sphere(cloud.vertices[id * 3:id * 3 + 3], rad))
cloud_param.append(tex)
mesh = vp.Union(*cloud_param)
ret.append(mesh)
else:
print("Geometry (name=%s) type: %s not supported!" %
(object.getName(), geometry.type()))
return ret
def planTriggered():
global world, robot, obj, target_gripper, solved_trajectory, trajectory_is_transfer, Tobject_grasp, obj, next_item_to_pick, qstart
if PROBLEM == '3a':
robot.setConfig(qstart)
res = place.transfer_plan(world, robot, qgoal, obj, Tobject_grasp)
if res is None:
print("Unable to plan transfer")
else:
traj = RobotTrajectory(robot, milestones=res)
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
elif PROBLEM == '3b':
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
traj = transfer
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
traj = traj.concat(retract, relative=True, jumpPolicy='jump')
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
else:
robot.setConfig(qstart)
obj = world.rigidObject(next_item_to_pick)
Tobj0 = obj.getTransform()
print("STARTING TO PICK OBJECT", obj.getName())
orig_grasps = db.object_to_grasps[obj.getName()]
grasps = [
grasp.get_transformed(obj.getTransform()).transfer(
source_gripper, target_gripper) for grasp in orig_grasps
]
res = pick.plan_pick_multistep(world, robot, obj, target_gripper,
grasps)
if res is None:
print("Unable to plan pick")
else:
transit, approach, lift = res
qgrasp = approach.milestones[-1]
#get the grasp transform
robot.setConfig(qgrasp)
Tobj = obj.getTransform()
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
robot.setConfig(lift.milestones[-1])
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
obj.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
robot.setConfig(qstart)
for i in range(num_in_firstbin):
dataset=random.choice(objects.keys())
index = random.randint(0,len(objects[dataset])-1)
objname = objects[dataset][index]
firstbin.append((dataset,objname))
for i in range(int(sys.argv[2])-num_in_firstbin):
dataset=random.choice(objects.keys())
index = random.randint(0,len(objects[dataset])-1)
objname = objects[dataset][index]
shelved.append((dataset,objname))
for objectset,objectname in shelved:
object=make_object(objectset,objectname,world)
object.setTransform(*se3.mul((so3.identity(),[shelf_x_shift,shelf_offset,shelf_height+increment/2*0.15]),object.getTransform()))
rigid_objects.append(object)
increment+=1
for objectset,objectname in firstbin:
object=make_object(objectset,objectname,world)
object.setTransform(*se3.mul((so3.identity(),[firstbin_x_shift,firstbin_offset,firstbin_height+increment_firstbin/2*0.22]),object.getTransform()))
firstbin_objects.append(object)
increment_firstbin+=1
if len(rigid_objects)>0:
xy_jiggle(world,rigid_objects,[shelf],[shelf_x_shift-0.5*shelf_dims[0],-0.5*shelf_dims[1]+shelf_offset],[shelf_x_shift+0.5*shelf_dims[0],0.5*shelf_dims[1]+shelf_offset],100)
if len(firstbin_objects)>0:
xy_jiggle(world,firstbin_objects,[shelf],[firstbin_x_shift-0.5*firstbin_dims[0],-0.5*firstbin_dims[1]+firstbin_offset],[firstbin_x_shift+0.5*firstbin_dims[0],0.5*firstbin_dims[1]+firstbin_offset],100)
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, \
execute_start_time, qstart, next_robot_to_move, swab_time, swab_init_height, swab_height
global sensor
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
if next_robot_to_move == 0:
cur_robot = robot
cur_obj = swab
elif next_robot_to_move == 1:
cur_robot = robot2
cur_obj = plate
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
qstart = solved_trajectory.eval(t)
if PHYSICS_SIMULATION:
# sim.controller(0).setPIDCommand(qstart,solved_trajectory.deriv(t))
# sim.controller(0).setMilestone(qstart)
sim.controller(0).setLinear(qstart, sim_dt * 5)
sim.simulate(sim_dt)
sim.updateWorld()
else:
cur_robot.setConfig(qstart)
during_transfer = trajectory_is_transfer.eval(t)[0]
if not was_grasping:
# pick up object
Tobject_grasp = se3.mul(
se3.inv(cur_robot.link(9).getTransform()),
cur_obj.getTransform())
was_grasping = True
if during_transfer:
cur_obj.setTransform(
*se3.mul(cur_robot.link(9).getTransform(), Tobject_grasp))
else:
was_grasping = False
if t > solved_trajectory.duration() - 0.8:
swab_init_height = swab.getTransform()[1][2]
swab_height = copy.deepcopy(swab_init_height)
if t > solved_trajectory.duration() + 1:
executing_plan = False
solved_trajectory = None
vis.add('gripper end', cur_robot.link(9).getTransform())
next_robot_to_move += 1
# let swab drop into trash can
if solved_trajectory is not None and next_robot_to_move == 0:
if solved_trajectory.duration(
) - 0.8 < t < solved_trajectory.duration() + 1:
if swab_height > 0:
swab_height = swab_init_height - 0.5 * 1 * swab_time**2
swab_time = swab_time + sim_dt
Rs, ts = swab.getTransform()
ts[2] = swab_height
swab.setTransform(Rs, ts)
def planTriggered():
global world,robot, robot2, swab,target_gripper,grasp_swab, grasp_plate, solved_trajectory, \
trajectory_is_transfer, next_robot_to_move
########################## Plan for robot0 & swab ############################
if next_robot_to_move == 0:
# plan pick
Tobj0 = swab.getTransform()
# center of tube: (0.5 0.025 0.72)
offset = 0.005
goal_bounds = [(0.615 - offset, 0.025 - offset / 4, 0.85),
(0.615 + offset, 0.025 + offset / 4, 1.0)]
qstart = robot.getConfig()
res = pick.plan_pick_one(world,
robot,
swab,
target_gripper,
grasp_swab,
robot0=True)
if res is None:
print("Unable to plan pick")
else:
print("plan pick success")
transit, approach, lift = res
# plan place
qgrasp = lift.milestones[-1]
robot.setConfig(qgrasp)
Tobj = swab.getTransform() # swab in world frame
# swab in gripper frame
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
place.robot0 = True
res = place.plan_place(world, robot, swab, Tobject_grasp,
target_gripper, goal_bounds, True)
robot.setConfig(qstart)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
swab.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
############################# Plan for Plate & robot 2 starts here! #################################
elif next_robot_to_move == 1:
Tobj0 = plate.getTransform()
goal_bounds = [(0.7, 0.62, 0.71), (0.7, 0.62, 0.71)]
res = pick.plan_pick_one(world,
robot2,
plate,
target_gripper,
grasp_plate,
robot0=False)
if res is None:
print("Unable to plan pick")
else:
print("plan pick success")
transit, approach, lift = res
qgrasp = lift.milestones[-1]
robot2.setConfig(qgrasp)
Tobj = plate.getTransform() # plate in world frame
# plate in gripper frame
Tobject_grasp = se3.mul(se3.inv(robot2.link(9).getTransform()),
Tobj)
place.robot0 = False
res = place.plan_place(world, robot2, plate, Tobject_grasp,
target_gripper, goal_bounds, False)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
plate.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[robot2.link(9).index])
def calculation():
"""
this is the main function of calibration calculation color.
"""
joint_positions = []
data_file = open(calidata_path + 'calibration_joint_positions.txt', 'r')
for line in data_file:
line = line.rstrip()
l = [num for num in line.split(' ')]
l2 = [float(num) for num in l]
joint_positions.append(l2)
data_file.close()
#the marker transforms here are actually just points not transformations
marker_transforms = []
data_file = open(calidata_path + 'calibration_marker_transforms.txt', 'r')
for line in data_file:
line = line.rstrip()
l = [num for num in line.split(' ')]
l2 = [float(num) for num in l]
marker_transforms.append(l2)
data_file.close()
T_marker_assumed = loader.load(
'RigidTransform', calidata_path + 'assumed_marker_world_transform.txt')
T_marker_assumed = (so3.inv(T_marker_assumed[0]), T_marker_assumed[1])
world = WorldModel()
res = world.readFile("robot_model_data/ur5Blocks.xml")
if not res:
raise RuntimeError("unable to load model")
#vis.add("world",world)
robot = world.robot(0)
link = robot.link(7)
q0 = robot.getConfig()
Tcameras = []
Tlinks = []
for i in range(len(joint_positions)):
q = robot.getConfig()
q[1:7] = joint_positions[i][0:6]
robot.setConfig(q)
Tlinks.append(link.getTransform())
#vis.add("ghost"+str(i),q)
#vis.setColor("ghost"+str(i),0,1,0,0.5)
robot.setConfig(q0)
#vis.add("MARKER",se3.mul(link.getTransform(),T_marker_assumed))
#vis.setAttribute("MARKER","width",2)
for loop in range(NLOOPS if ESTIMATE_MARKER_TRANSFORM else 1):
###Calculate Tcamera in EE (only needed for transform fit..)
###Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
#actually using only point fit here....
if METHOD == 'point fit':
Tcamera2 = point_fit_transform(
[Tm_c for Tm_c in marker_transforms],
[se3.mul(se3.inv(Tl), T_marker_assumed)[1] for Tl in Tlinks])
Tcamera2 = [so3.from_matrix(Tcamera2[0]), Tcamera2[1]]
Tcamera = Tcamera2
#else:
# Tcamera = transform_average(Tcameras)
#with the Tcamera from the current iteration, calculate marker points in world
Tmarkers_world = [
se3.apply(se3.mul(Tl, Tcamera), Tm_c)
for (Tl, Tm_c) in zip(Tlinks, marker_transforms)
]
#Tmarkers_link = [se3.mul(se3.inv(Tl),Tm) for Tl,Tm in zip(Tlinks,Tmarkers)]
if ESTIMATE_MARKER_TRANSFORM:
T_marker_assumed_inv = point_fit_transform(
[[0, 0, 0] for Tm_c in marker_transforms], [
se3.apply(se3.mul(Tl, Tcamera2), Tm_c)
for (Tl, Tm_c) in zip(Tlinks, marker_transforms)
])
T_marker_assumed_inv = [
so3.from_matrix(T_marker_assumed_inv[0]),
T_marker_assumed_inv[1]
]
#print T_marker_assumed_inv
T_marker_assumed = T_marker_assumed_inv
#print "New assumed marker position",T_marker_assumed
else:
pass
#print "Would want new marker transform",transform_average(Tmarkers_world)
#print "New estimated camera position",Tcamera
SSE_t = 0
for (Tm_c, Tl) in zip(marker_transforms, Tlinks):
Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)),
T_marker_assumed)
SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1])
#print "RMSE rotation (radians)",np.sqrt(SSE_R/len(Tlinks))
print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
#Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
#Tmarkers = [se3.mul(Tl,se3.mul(Tcamera,Tm_c)) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)]
print "Saving to calibrated_camera_xform.txt"
cali_txt_path = calidata_path + "calibrated_camera_xform.txt"
loader.save(Tcamera, "RigidTransform", cali_txt_path)
if ESTIMATE_MARKER_TRANSFORM:
print "Saving to calibrated_marker_link_xform.txt"
loader.save(T_marker_assumed, "RigidTransform",
calidata_path + "calibrated_marker_world_xform.txt")
#SSE_R = 0
SSE_t = 0
for (Tm_c, Tl) in zip(marker_transforms, Tlinks):
Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)),
T_marker_assumed)
SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1])
print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
def loop_through_sensors(
world=world,
sensor=sensor,
world_camera_viewpoints=world_camera_viewpoints,
index=index,
counters=counters):
viewno = counters[0]
variation = counters[1]
total_count = counters[2]
print("Generating data for sensor view", viewno, "variation",
variation)
sensor_xform = world_camera_viewpoints[viewno]
#TODO: problem 1.B: perturb camera and change colors
# Generate random axis, random angle, rotate about angle for orientation perturbation
# Generate random axis, random dist, translate distance over axis for position perturbation
rand_axis = np.random.rand(3)
rand_axis = vectorops.unit(rand_axis)
multiplier = np.random.choice([True, False], 1)
rand_angle = (np.random.rand(1)[0] * 10 +
10) * (-1 if multiplier else 1)
# print(rand_angle)
R = so3.from_axis_angle((rand_axis, np.radians(rand_angle)))
rand_axis = vectorops.unit(np.random.random(3))
rand_dist = np.random.random(1)[0] * .10 + .10
# print(rand_dist)
t = vectorops.mul(rand_axis, rand_dist)
sensor_xform = se3.mul((R, t), sensor_xform)
sensing.set_sensor_xform(sensor, sensor_xform)
rgb_filename = "image_dataset/color_%04d_var%04d.png" % (
total_count, variation)
depth_filename = "image_dataset/depth_%04d_var%04d.png" % (
total_count, variation)
grasp_filename = "image_dataset/grasp_%04d_var%04d.png" % (
total_count, variation)
#Generate sensor images
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
print("Done with kinematic simulate")
rgb, depth = sensing.camera_to_images(sensor)
print("Saving to", rgb_filename, depth_filename)
Image.fromarray(rgb).save(rgb_filename)
depth_scale = 8000
depth_quantized = (depth * depth_scale).astype(np.uint32)
Image.fromarray(depth_quantized).save(depth_filename)
with open("image_dataset/metadata.csv", 'a') as f:
line = "{},{},{},{},{},{},{}\n".format(
index, viewno, loader.write(sensor_xform,
'RigidTransform'),
os.path.basename(rgb_filename),
os.path.basename(depth_filename),
os.path.basename(grasp_filename), variation)
f.write(line)
#calculate grasp map and save it
grasp_map = make_grasp_map(world, total_count)
grasp_map_quantized = (grasp_map * 255).astype(np.uint8)
channels = ['score', 'opening', 'axis_heading', 'axis_elevation']
for i, c in enumerate(channels):
base, ext = os.path.splitext(grasp_filename)
fn = base + '_' + c + ext
Image.fromarray(grasp_map_quantized[:, :, i]).save(fn)
#loop through variations and views
counters[1] += 1
if counters[1] >= max_variations:
counters[1] = 0
counters[0] += 1
if counters[0] >= len(world_camera_viewpoints):
vis.show(False)
counters[2] += 1
def listen_tf(listener,
klampt_obj,
frameprefix="klampt",
root="world",
onerror=None):
"""Reads Klampt frames from the ROS tf module.
Args:
listener (tf.TransformListener): the tf listener
klampt_obj: the object to update. Can be WorldModel, RobotModel,
anything with a setTransform or setCurrentTransform method,
or None (in the latter case, a se3 object will be returned).
Note:
RobotModel configurations will not be changed, just
the link transforms.
frameprefix (str): the name of the base frame for this object
root (str): the name of the TF world frame.
onerror (str or None): either 'raise' in which case a tf exception
is raised, 'print', in which case the error is printed, or None,
in which case any exception is silently ignored.
"""
from klampt import WorldModel, RobotModel
import tf
def do_lookup(frame, parent):
try:
(trans, rot) = listener.lookupTransform(frame, parent,
rospy.Time(0))
return (so3.from_quaternion(
(rot[3], rot[0], rot[1], rot[2])), trans)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
if onerror == 'print':
print("listen_tf: Error looking up frame", frame)
elif onerror == 'raise':
raise
return None
if isinstance(klampt_obj, WorldModel):
world = klampt_obj
prefix = frameprefix
for i in range(world.numRigidObjects()):
T = do_lookup(prefix + "/" + world.rigidObject(i).getName(), root)
if T:
world.rigidObject(i).setTransform(*T)
for i in range(world.numRobots()):
robot = world.robot(i)
rprefix = prefix + "/" + robot.getName()
listen_tf(listener, robot, rprefix, root, onerror)
return
elif isinstance(klampt_obj, RobotModel):
robot = klampt_obj
rprefix = frameprefix
for j in range(robot.numLinks()):
p = robot.link(j).getParent()
if p < 0:
T = do_lookup(rprefix + "/" + robot.link(j).getName(), root)
if T:
robot.link(j).setTransform(*T)
else:
T = do_lookup(rprefix + "/" + robot.link(j).getName(),
rprefix + "/" + robot.link(p).getName())
if T:
robot.link(j).setTransform(
*se3.mul(robot.link(p).getTransform(), T))
return
elif hasattr(klampt_obj, 'setTransform'):
T = dolookup(frameprefix, root)
if T:
klampt_obj.setTransform(*T)
elif hasattr(klampt_obj, 'setCurrentTransform'):
T = dolookup(frameprefix, root)
if T:
klampt_obj.setCurrentTransform(*T)
elif klampt_obj is None:
return dolookup(frameprefix, root)
else:
raise ValueError("Invalid type given to listen_tf: ",
klampt_obj.__class__.__name__)
def broadcast_tf(broadcaster,
klampt_obj,
frameprefix="klampt",
root="world",
stamp=0.):
"""Broadcasts Klampt frames to the ROS tf module.
Args:
broadcaster (tf.TransformBroadcaster): the tf broadcaster
klampt_obj: the object to publish. Can be WorldModel, Simulator,
RobotModel, SimRobotController, anything with a getTransform
or getCurrentTransform method, or se3 element
frameprefix (str): the name of the base frame for this object
root (str): the name of the TF world frame.
Note:
If klampt_obj is a Simulator or SimRobotController, then its
corresponding model will be updated.
"""
from klampt import WorldModel, Simulator, RobotModel, SimRobotController
if stamp == 'now':
stamp = rospy.Time.now()
elif isinstance(stamp, (int, float)):
stamp = rospy.Time(stamp)
world = None
if isinstance(klampt_obj, WorldModel):
world = klampt_obj
elif isinstance(klampt_obj, Simulator):
world = klampt_obj.model()
klampt_obj.updateModel()
if world is not None:
prefix = frameprefix
for i in range(world.numRigidObjects()):
T = world.rigidObject(i).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp, root,
prefix + "/" + world.rigidObject(i).getName())
for i in range(world.numRobots()):
robot = world.robot(i)
rprefix = prefix + "/" + robot.getName()
broadcast_tf(broadcaster, robot, rprefix, root)
return
robot = None
if isinstance(klampt_obj, RobotModel):
robot = klampt_obj
elif isinstance(klampt_obj, SimRobotController):
robot = klampt_obj.model()
robot.setConfig(klampt_obj.getSensedConfig())
if robot is not None:
rprefix = frameprefix
for j in range(robot.numLinks()):
p = robot.link(j).getParent()
if p < 0:
T = robot.link(j).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp, root,
rprefix + "/" + robot.link(j).getName())
else:
Tparent = se3.mul(se3.inv(robot.link(p).getTransform()),
robot.link(j).getTransform())
q = so3.quaternion(Tparent[0])
broadcaster.sendTransform(
Tparent[1], (q[1], q[2], q[3], q[0]), stamp,
rprefix + "/" + robot.link(p).getName(),
rprefix + "/" + robot.link(j).getName())
return
transform = None
if isinstance(klampt_obj, (list, tuple)):
#assume to be an SE3 element.
transform = klampt_obj
elif hasattr(klampt_obj, 'getTransform'):
transform = klampt_obj.getTransform()
elif hasattr(klampt_obj, 'getCurrentTransform'):
transform = klampt_obj.getCurrentTransform()
else:
raise ValueError("Invalid type given to broadcast_tf: ",
klampt_obj.__class__.__name__)
q = so3.quaternion(transform[0])
broadcaster.sendTransform(transform[1], (q[1], q[2], q[3], q[0]), stamp,
root, frameprefix)
return
vis.add("cam", cam)
vp = vis.getViewport()
vp.camera.rot[1] -= 0.5
vis.setViewport(vp)
default = vis.getViewport().getTransform()
print('x:', so3.apply(default[0], [1, 0, 0]))
print('y:', so3.apply(default[0], [0, 1, 0]))
print('z:', so3.apply(default[0], [0, 0, 1]))
print('offset:', default[1])
circle_points = []
npts = 50
times = []
for i in range(npts + 1):
angle = math.radians(360 * i / npts)
circle_points.append(
se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default))
times.append(i * 20 / npts)
circle_traj = SE3Trajectory(times, circle_points)
circle_traj.milestones[-1] = circle_traj.milestones[0]
circle_smooth_traj = SE3HermiteTrajectory()
circle_smooth_traj.makeSpline(circle_traj, loop=True)
R0 = so3.identity()
R1 = so3.rotation([0, 0, 1], math.pi / 2)
dR0 = [0.0] * 9
#TODO: for some reason the interpolation doesn't work very well...
#vis.add("Camera smooth traj",circle_smooth_traj.discretize_se3(0.1))
#for m in circle_smooth_traj.milestones:
# T = m[:12]
# vT = m[12:]
# print("Orientation velocity",vT[:9])
