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 test_getObjectPhalanxMeanContactPoint(self):
world = WorldModel()
world.loadElement(abspath + '/../data/terrains/plane.env')
obj = make_box(world, 0.03, 0.05, 0.1)
R, t = obj.getTransform()
obj.setTransform(R, [0, 0, 0.1 / 2.])
p = [0.015, -0.025, 0.1]
q = [3.7494e-33, 6.12323e-17, 1.0, -6.12323e-17]
o_T_p = np.array(se3.homogeneous((so3.from_quaternion(q), p)))
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p = (-0.03, -0.12, -0.013 + 0.02
) # -0.013 + 0.02, from working exp at commit 98305499
R = (0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0)
p_T_h = np.array(se3.homogeneous((R, p)))
w_T_h_des = w_T_o.dot(o_T_p).dot(p_T_h)
w_T_h_des_se3 = se3.from_homogeneous(w_T_h_des)
robot = make_moving_base_robot('soft_hand', world)
set_moving_base_xform(robot, w_T_h_des_se3[0], w_T_h_des_se3[1])
sim = SimpleSimulator(world)
# setup the preshrink
visPreshrink = False # turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
hand = plugins.soft_hand.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
links_to_check = np.array([
3, 4, 6, 8, 10, 13, 15, 17, 20, 22, 24, 27, 29, 31, 33, 35, 37
]) + 6
#vis.add("world", world)
#vis.show()
for i in range(100):
#vis.lock()
hand.setCommand([np.min((i / 10., 1.0))])
sim.simulate(0.01)
sim.updateWorld()
#vis.unlock()
c_p, c_f = getObjectPhalanxMeanContactPoint(
sim, obj, robot, links_to_check)
c_p_all, c_f_all = getObjectPhalanxMeanContactPoint(
sim, obj, robot)
if i == 0:
self.assertEqual(countContactPoints(c_p), 0)
self.assertEqual(countContactPoints(c_p_all), 0)
elif i == 99:
self.assertTrue(countContactPoints(c_p) > 0)
self.assertTrue(countContactPoints(c_p_all) > 0)
def _load_mvbbs_simulated(self):
try:
f = open(self.filename_simulated)
reader = csv.reader(f)
self.db_simulated = {}
for row in reader:
object_dims = tuple(
[float(v) for i, v in enumerate(row) if i in range(3)])
t = [float(v) for i, v in enumerate(row) if i in range(3, 6)]
q = [float(row[9])] + [
float(v) for i, v in enumerate(row) if i in range(6, 9)
]
T = np.array(se3.homogeneous((so3.from_quaternion(q), t)))
t = [float(v) for i, v in enumerate(row) if i in range(10, 13)]
q = [float(row[16])] + [
float(v) for i, v in enumerate(row) if i in range(13, 16)
]
h_T_o = np.array(se3.homogeneous((so3.from_quaternion(q), t)))
q_hand = [
float(v) for i, v in enumerate(row)
if i in range(17, 17 + self.n_dofs)
]
c_p = [
float(v) for i, v in enumerate(row)
if i in range(17 + self.n_dofs, 17 + self.n_dofs +
3 * self.n_l)
]
c_f = [
float(v) for i, v in enumerate(row)
if i in range(17 + self.n_dofs + 3 * self.n_l, 17 +
self.n_dofs + 9 * self.n_l)
]
if tuple(object_dims) not in self.db_simulated:
self.db_simulated[tuple(object_dims)] = []
obj_pose_q_c_p_c_f = {
'T': T,
'h_T_o': h_T_o,
'q': q_hand,
'c_p': c_p,
'c_f': c_f
}
self.db_simulated[tuple(object_dims)].append(
obj_pose_q_c_p_c_f)
except:
pass
def display(self):
if self.running:
self.world.drawGL()
w_T_o = np.array(se3.homogeneous(self.obj.getTransform()))
for pose in self.poses:
w_T_p_des = w_T_o.dot(pose)
w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des)
draw_GL_frame(w_T_p__des_se3, color=(0.5,0.5,0.5))
if self.curr_pose is not None:
w_T_p_des = w_T_o.dot(self.curr_pose)
w_T_p__des_se3 = se3.from_homogeneous(w_T_p_des)
draw_GL_frame(w_T_p__des_se3)
hand_xform = get_moving_base_xform(self.robot)
w_T_p = np.array(se3.homogeneous(hand_xform)).dot(self.h_T_p)
w_T_p_se3 = se3.from_homogeneous(w_T_p)
draw_GL_frame(w_T_p_se3)
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glEnable(GL_POINT_SMOOTH)
glColor3f(1,1,0)
glLineWidth(1.0)
glPointSize(5.0)
forceLen = 0.01 # scale of forces
if self.sim is not None and self.obj is not None and self.robot is not None:
c_p, c_f = getObjectPhalanxMeanContactPoint(self.sim, self.obj, self.robot, self.links_to_check)
n_c_p = countContactPoints(c_p)
if countContactPoints(c_p) > 0:
glBegin(GL_POINTS)
for i in range(len(c_p)/3):
o_c_p_i = c_p[3*i:3*i+3]
if np.all([False if math.isnan(p) else True for p in o_c_p_i]):
w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i)
glVertex3f(*w_c_p_i)
glEnd()
glBegin(GL_LINES)
for i in range(len(c_p)/3):
o_c_p_i = c_p[3 * i:3 * i + 3]
o_c_f_i = c_f[6 * i:6 * i + 3]
if np.all([False if math.isnan(f) else True for f in o_c_f_i]):
if np.all([False if math.isnan(p) else True for p in o_c_p_i]):
w_c_p_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_p_i)
w_c_f_i = se3.apply(se3.from_homogeneous(w_T_o), o_c_f_i)
glVertex3f(*w_c_p_i)
glVertex3f(*vectorops.madd(w_c_p_i, w_c_f_i, forceLen))
glEnd()
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 draw_wand(self,xform,size = 0.2):
#draw the wand pointing along the -x axis (+x toward the user)
gldraw.setcolor(1,0.8,0.8)
mat = zip(*se3.homogeneous(xform))
mat = sum([list(coli) for coli in mat],[])
glPushMatrix()
glMultMatrixf(mat)
gldraw.box([-size,-size*0.05,-size*0.05],[size*0.5,size*0.05,size*0.05])
glPopMatrix()
def has_simulation(self, box_dims, pose):
self._load_mvbbs_simulated()
box_dims = tuple(box_dims)
if box_dims in self.db_simulated:
if not isinstance(pose, np.ndarray):
pose = np.array(se3.homogeneous(pose))
poses = [p['T'] for p in self.db_simulated[box_dims]]
for p in poses:
if np.all((pose - p) < 1e-12):
return True
return False
def redraw_pcd(pcd_pair, Toct_needle):
"""
:param pcd_pair: {list} -- The list of Open3D.PointCloud
:param Toct_needle: {list} -- The transfer matrix from OCT frame to needle frame
:return: None
"""
source = pcd_pair[0]
target = pcd_pair[1]
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
target_temp.transform(se3.homogeneous(Toct_needle))
return [source_temp, target_temp]
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 __init__(self,
box_dims,
poses,
world,
p_T_h,
module,
box_db,
links_to_check=None):
GLRealtimeProgram.__init__(self, "FilteredMVBBTEsterVisualizer")
self.world = world
self.p_T_h = p_T_h
self.h_T_p = np.linalg.inv(self.p_T_h)
self.poses = []
self.hand = None
self.is_simulating = False
self.curr_pose = None
self.robot = self.world.robot(0)
self.q_0 = self.robot.getConfig()
self.w_T_o = None
self.obj = None
self.box_dims = box_dims
self.t_0 = None
self.object_com_z_0 = None
self.object_fell = None
self.sim = None
self.module = module
self.running = True
self.db = box_db
self.links_to_check = links_to_check
if self.world.numRigidObjects() > 0:
self.obj = self.world.rigidObject(0)
self.w_T_o = np.array(se3.homogeneous(self.obj.getTransform()))
for p in poses:
if not self.db.has_simulation(self.box_dims, p):
self.poses.append(p)
else:
print "Pose", p, "already simulated"
else:
"Warning: during initialization of visualizer object is still not loaded in world"
self.poses = poses
print "Will simulate", len(self.poses), "poses,"
def _load_mvbbs(self):
try:
f = open(self.filename)
self.db = {}
reader = csv.reader(f)
for row in reader:
object_dims = tuple(
[float(v) for i, v in enumerate(row) if i in range(3)])
t = [float(v) for i, v in enumerate(row) if i in range(3, 6)]
q = [float(row[9])] + [
float(v) for i, v in enumerate(row) if i in range(6, 9)
]
T = (so3.from_quaternion(q), t)
if tuple(object_dims) not in self.db:
self.db[object_dims] = []
self.db[object_dims].append(np.array(se3.homogeneous(T)))
except:
pass
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 idle(self):
if not self.running:
return
if self.world.numRigidObjects() > 0:
self.obj = self.world.rigidObject(0)
else:
return
if not self.is_simulating:
if len(self.poses) > 0:
self.curr_pose = self.poses.pop(0)
print "\n\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "Simulating Next Pose Grasp"
print "Dims:\n", self.box_dims
print "Pose:\n", self.curr_pose
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!"
print "!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n\n"
else:
print "Done testing all", len(
self.poses), "poses for object", self.box_dims
print "Quitting"
self.running = False
vis.show(hidden=True)
return
w_T_o_se3 = se3.from_homogeneous(self.w_T_o)
self.obj.setTransform(w_T_o_se3[0], w_T_o_se3[1])
w_T_h_des_se3 = se3.from_homogeneous(
self.w_T_o.dot(self.curr_pose).dot(self.p_T_h))
self.robot.setConfig(self.q_0)
set_moving_base_xform(self.robot, w_T_h_des_se3[0],
w_T_h_des_se3[1])
if self.sim is None:
self.sim = SimpleSimulator(self.world)
self.hand = self.module.HandEmulator(self.sim, 0, 6, 6)
self.sim.addEmulator(0, self.hand)
# the next line latches the current configuration in the PID controller...
self.sim.controller(0).setPIDCommand(
self.robot.getConfig(),
vectorops.mul(self.robot.getVelocity(), 0.0))
obj_b = self.sim.body(self.obj)
obj_b.setVelocity([0., 0., 0.], [0., 0., 0.])
# setup the preshrink
visPreshrink = False # turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(self.robot.numLinks()):
self.sim.body(
self.robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(self.world.numRigidObjects()):
self.sim.body(self.world.rigidObject(
l)).setCollisionPreshrink(visPreshrink)
self.object_com_z_0 = getObjectGlobalCom(self.obj)[2]
self.object_fell = False
self.t_0 = self.sim.getTime()
self.is_simulating = True
if self.is_simulating:
t_lift = 1.3 # when to lift
d_lift = 1.0 # duration
# print "t:", self.sim.getTime() - self.t_0
object_com_z = getObjectGlobalCom(self.obj)[2]
w_T_h_curr_se3 = get_moving_base_xform(self.robot)
w_T_h_des_se3 = se3.from_homogeneous(
self.w_T_o.dot(self.curr_pose).dot(self.p_T_h))
if self.sim.getTime() - self.t_0 == 0:
# print "Closing hand"
self.hand.setCommand([1.0])
elif (self.sim.getTime() - self.t_0) >= t_lift and (
self.sim.getTime() - self.t_0) <= t_lift + d_lift:
# print "Lifting"
t_i = w_T_h_des_se3[1]
t_f = vectorops.add(t_i, (0, 0, 0.2))
u = np.min((self.sim.getTime() - self.t_0 - t_lift, 1.))
send_moving_base_xform_PID(self.sim.controller(0),
w_T_h_des_se3[0],
vectorops.interpolate(t_i, t_f, u))
if (self.sim.getTime() - self.t_0
) >= t_lift: # wait for a lift before checking if object fell
d_hand = w_T_h_curr_se3[1][2] - w_T_h_des_se3[1][2]
d_com = object_com_z - self.object_com_z_0
if d_hand - d_com > 0.1:
self.object_fell = True
print "!!!!!!!!!!!!!!!!!!"
print "Grasp Unsuccessful"
print "!!!!!!!!!!!!!!!!!!"
self.sim.simulate(0.01)
self.sim.updateWorld()
if not vis.shown() or (self.sim.getTime() -
self.t_0) >= 2.5 or self.object_fell:
if vis.shown(): # simulation stopped because it was successful
print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%"
print "Saving grasp, status:", "failure" if self.object_fell else "success"
print "%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\n\n\n"
w_T_h_curr = np.array(se3.homogeneous(w_T_h_curr_se3))
w_T_o_curr = np.array(
se3.homogeneous(self.obj.getTransform()))
h_T_o = np.linalg.inv(w_T_h_curr).dot(w_T_o_curr)
if self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs:
q_grasp = [
float('nan')
] * self.db.n_dofs if self.object_fell else self.hand.getConfiguration(
)
elif self.db.n_dofs == self.hand.d_dofs + self.hand.u_dofs + self.hand.m_dofs:
q_grasp = [
float('nan')
] * self.db.n_dofs if self.object_fell else self.hand.getFullConfiguration(
)
else:
raise Exception(
'Error: unexcpeted number of joints for hand')
c_p, c_f = getObjectPhalanxMeanContactPoint(
self.sim, self.obj, self.robot, self.links_to_check)
try:
self.db.save_simulation(self.box_dims, self.curr_pose,
h_T_o, q_grasp, c_p, c_f)
except:
print "\nXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"
print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"
print "X Error while calling save_simulation X"
print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX"
traceback.print_exc()
print "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX\n"
self.is_simulating = False
self.sim = None
self.robot.setConfig(self.q_0)
def to_numpy(obj, type='auto'):
"""Converts a Klamp't object to a numpy array or multiple numpy arrays.
Supports:
* lists and tuples
* RigidTransform: returned as 4x4 homogeneous coordinate transform
* Matrix3, Rotation: returned as 3x3 matrix. Can't be determined
with 'auto', need to specify type='Matrix3' or 'Rotation'.
* Configs
* Trajectory: returns a pair (times,milestones)
* TriangleMesh: returns a pair (verts,indices)
* PointCloud: returns a n x (3+k) array, where k is the # of properties
* VolumeGrid: returns a triple (bmin,bmax,array)
* Geometry3D: returns a pair (T,geomdata)
If you want to get a transformed point cloud or mesh, you can pass in a
Geometry3D as the obj, and its geometry data type as the type.
"""
global supportedTypes
if type == 'auto':
otype = types.objectToTypes(obj)
if isinstance(otype, (list, tuple)):
for t in otype:
if t in supportedTypes:
type = t
break
if type == 'auto':
raise ValueError('obj is not a supported type: ' +
', '.join(otype))
else:
type = otype
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return np.array(se3.homogeneous(obj))
elif type == 'Rotation' or type == 'Matrix3':
return np.array(so3.matrix(obj))
elif type == 'Trajectory':
return np.array(obj.times), np.array(obj.milestones)
elif type == 'TriangleMesh':
from klampt import Geometry3D
if isinstance(obj, Geometry3D):
res = to_numpy(obj.getTriangleMesh(), type)
R = to_numpy(obj.getCurrentTransform()[0], 'Matrix3')
t = to_numpy(obj.getCurrentTransform()[1], 'Vector3')
return (np.dot(R, res[0]) + t, res[1])
return (np.array(obj.vertices).reshape((len(obj.vertices) // 3, 3)),
np.array(obj.indices, dtype=np.int32).reshape(
(len(obj.indices) // 3, 3)))
elif type == 'PointCloud':
from klampt import Geometry3D
if isinstance(obj, Geometry3D):
res = to_numpy(obj.getPointCloud(), type)
R = to_numpy(obj.getCurrentTransform()[0], 'Matrix3')
t = to_numpy(obj.getCurrentTransform()[1], 'Vector3')
res[:, :3] = np.dot(R, res[:, :3]) + t
return res
points = np.array(obj.vertices).reshape((obj.numPoints(), 3))
if obj.numProperties() == 0:
return points
properties = np.array(obj.properties).reshape(
(obj.numPoints(), obj.numProperties()))
return np.hstack((points, properties))
elif type == 'VolumeGrid':
bmin = np.array(obj.bbox)[:3]
bmax = np.array(obj.bbox)[3:]
values = np.array(obj.values).reshape(
(obj.dims[0], obj.dims[1], obj.dims[2]))
return (bmin, bmax, values)
elif type == 'Geometry3D':
if obj.type() == 'PointCloud':
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), to_numpy(
obj.getPointCloud(), obj.type())
elif obj.type() == 'TriangleMesh':
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), to_numpy(
obj.getTriangleMesh(), obj.type())
elif obj.type() == 'VolumeGrid':
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), to_numpy(
obj.getVolumeGrid(), obj.type())
elif obj.type() == 'Group':
arrays = []
for i in range(obj.numElements()):
arrays.append(to_numpy(obj.getElement(i), 'Geometry3D'))
return to_numpy(obj.getCurrentTransform(),
'RigidTransform'), arrays
else:
return np.array(obj)
def optimization(self):
"""
:return: None
"""
time_stamp = time.time()
min_error = 10e5
for k in range(25):
random.seed(time.time())
est_input = [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \
[random.uniform(-0.8, 0.8) for i in range(3)] + \
[random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \
[random.uniform(-0.2, 0.2) for i in range(3)]
print("We will start " + str(k + 1) + "th minimize!")
res = root(opt_error_fun,
np.array(est_input),
args=(self.Tlink_set, self.trans_list),
method='lm',
options={})
print("The reason for termination: \n", res.message,
"\nAnd the number of the iterations are: ", res.nfev)
error = np.sum(np.absolute(res.fun))
print("The minimize is end, and the error is: ", error)
if error <= min_error:
min_error = error
self.min_res = res
print("The optimization uses: ", time.time() - time_stamp, "seconds")
print("The error is: ", np.sum(np.absolute(self.min_res.fun)))
print("The optimized T_oct is: ",
(self.min_res.x[0:3], self.min_res.x[3:6]))
print(
"And the matrix form is: \n",
np.array(
se3.homogeneous(
(so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6]))))
print("The optimized T_needle_end is: ",
(self.min_res.x[6:9], self.min_res.x[9:12]))
print(
"And the matrix form is: \n",
np.array(
se3.homogeneous((so3.from_rpy(self.min_res.x[6:9]),
self.min_res.x[9:12]))))
# os.makedirs("../../data/suture_experiment/calibration_result_files/" + self.serial_num + "/", exist_ok=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_result.npy", self.min_res.x, allow_pickle=True)
vis.spin(float('inf'))
vis.clear()
self.robot.setConfig(self.qstart)
vis.add("world", self.world)
World_base = model.coordinates.Frame("Base Frame",
worldCoordinates=(so3.from_rpy(
[0, 0, 0]), [0, 0, 0]))
vis.add('World_base', World_base)
est_OCT_coordinate = model.coordinates.Frame(
"est OCT Frame",
worldCoordinates=(so3.from_rpy(self.min_res.x[0:3]),
self.min_res.x[3:6]))
vis.add("est OCT Frame", est_OCT_coordinate)
Tlink_6 = model.coordinates.Frame(
"Link 6 Frame",
worldCoordinates=self.robot.link('link_6').getTransform())
vis.add('Link 6 Frame', Tlink_6)
est_Tneedle_world = se3.mul(
self.robot.link('link_6').getTransform(),
(so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12]))
est_needle_coordinate = model.coordinates.Frame(
"est needle Frame", worldCoordinates=est_Tneedle_world)
vis.add("est needle Frame", est_needle_coordinate)
vis.spin(float('inf'))
def needle2oct_icp(self, thres_t, thres_s):
"""
:return: None
"""
threshold = 50
for i in range(0, self.num_pcd):
pcd_s = read_point_cloud(
"../../data/point_clouds/source_point_clouds/" +
self.serial_num + "/cropped_OCT_config_" + str(i + 1) + ".ply")
pcd_s.points = Vector3dVector(
np.multiply(np.asarray(pcd_s.points), 1e-3))
if i < thres_t:
pcd_t = read_point_cloud(
"../../data/point_clouds/target_point_clouds/TF-1-cut.ply")
else:
pcd_t = read_point_cloud(
"../../data/point_clouds/target_point_clouds/TF-1.ply")
pcd_t.points = Vector3dVector(
np.multiply(np.asarray(pcd_t.points), 1e-3))
temp_rmse = 1.
count = 0
termination_threshold = 2.0e-5
while temp_rmse > termination_threshold:
print("ICP implementation is in " + str(i + 1) +
"th point clouds.")
random.seed(count * 5 + 7 - count / 3)
if i > thres_s:
trans_init = np.asarray(
se3.homogeneous((so3.from_rpy([
random.uniform(-0.5 * np.pi, 0.5 * np.pi)
for kk in range(3)
]), [
random.uniform(-5. * 1e-3, 5. * 1e-3)
for kk in range(3)
])))
else:
trans_init = np.asarray(
se3.homogeneous((so3.from_rpy([
random.uniform(-0.5 * np.pi, 0.5 * np.pi) + np.pi
] + [
random.uniform(-0.5 * np.pi, 0.5 * np.pi)
for kk in range(2)
]), [random.uniform(-5. * 1e-3, 5. * 1e-3)] + [
random.uniform(-5. * 1e-3, 5. * 1e-3) + 15. * 1e-3
] + [random.uniform(-5. * 1e-3, 5. * 1e-3)])))
reg_p2p = registration_icp(
pcd_s, pcd_t, threshold, trans_init,
TransformationEstimationPointToPoint(),
ICPConvergenceCriteria(max_iteration=300,
relative_rmse=1e-15,
relative_fitness=1e-15))
# ***DEBUG*** #
# draw_registration_result(pcd_s, pcd_t, reg_p2p.transformation)
modified_pcd_s = duplicate_part_pcd(reg_p2p.transformation,
pcd_s, -0.2 * 1e-3,
0.0 * 1e-3, 30)
reg_p2p_2 = registration_icp(
modified_pcd_s, pcd_t, threshold, reg_p2p.transformation,
TransformationEstimationPointToPoint(),
ICPConvergenceCriteria(max_iteration=100,
relative_rmse=1e-15,
relative_fitness=1e-15))
# ***DEBUG*** #
# draw_registration_result(modified_pcd_s, pcd_t, reg_p2p_2.transformation)
print("The RMSE is: ", reg_p2p_2.inlier_rmse)
temp_rmse = reg_p2p_2.inlier_rmse
count += 1
if count == 15:
termination_threshold = 2.5e-5
trans = se3.from_homogeneous(reg_p2p_2.transformation)
self.trans_list.append(trans)
point_oct = []
pcd_s_copy = PointCloud()
pcd_p = PointCloud()
pcd_s_copy.points = Vector3dVector(
np.multiply(np.asarray(modified_pcd_s.points), 1))
pcd_t_copy = read_point_cloud(
"../../data/point_clouds/target_point_clouds/TF-1.ply")
pcd_t_copy.points = Vector3dVector(
np.multiply(np.asarray(pcd_t_copy.points), 1e-3))
pcd_p.points = Vector3dVector(point_oct)
# ***DEBUG*** #
# draw_registration_result(pcd_s_copy, pcd_t_copy, se3.homogeneous(trans))
self.pcd_list.append([pcd_s_copy, pcd_t_copy])
