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 dalk_path_generation(path_filename, calib_filename):
data = np.loadtxt(path_filename)
oct2robot = np.loadtxt(calib_filename)[0, :].reshape((4, 4))
robot2oct = np.linalg.inv(oct2robot)
robot_target = []
xyzrpy = []
# dalk_frame = (so3.from_rpy([0, 0, -np.pi/2]), [52.5 * 0.0254, 6.5 * 0.0251, 0.75 * 0.0254])
dalk_frame = (so3.from_rpy([0, np.pi, 0]), [0.0, 0.0, 0.0])
for row in data[::1]:
x_target = row[12 + 6:12 + 6 + 6]
# xyzrpy.append(x_target)
T = np.eye(4)
T = rotation_matrix(x_target[3], [1, 0, 0]).dot(
rotation_matrix(x_target[4], [0, 1, 0])).dot(
rotation_matrix(x_target[5], [0, 0, 1]))
T[:3, 3] = x_target[0:3]
T = robot2oct.dot(T)
T[:3, 3] = T[:3, 3] * 1
T = oct2robot.dot(T)
robot_target.append(T)
T2rpyxyz = list(se3.from_homogeneous(T)[1]) + list(
so3.rpy(se3.from_homogeneous(T)[0]))
print(T2rpyxyz)
xyzrpy.append(T2rpyxyz)
# T_m = se3.mul(se3.inv(dalk_frame), se3.from_homogeneous(T))
# T_h = se3.homogeneous(T_m)
# T2rpyxyz = list(T_m[1]) + list(so3.rpy(T_m[0]))
# xyzrpy.append(T2rpyxyz)
# robot_target.append(T_h)
return robot_target, xyzrpy
def split_db(self):
filenames = []
precision = int(np.ceil(np.log10(len(self.db.keys()))))
n = 0
for k in sorted(self.db.keys()):
fname = os.path.splitext(
self.filename)[0] + '_' + str(n).zfill(precision)
filenames.append(fname)
f = open(fname + '.csv', 'w')
for pose in self.db[k]:
f.write(','.join([str(d) for d in k]))
f.write(',')
values = []
pose = se3.from_homogeneous(pose)
# saving pose.t
values += pose[1]
# saving pose.q
q = np.array(so3.quaternion(pose[0]))
values += list(q[1:4])
values.append(q[0])
f.write(','.join([str(v) for v in values]))
f.write('\n')
f.close()
n = n + 1
return filenames
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 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 save_simulation(self, box_dims, pose, h_T_o, q, c_p, c_f):
if len(c_p) != self.n_l * 3:
raise RuntimeError(
'Incorrect size of c_p when calling save_simulation()')
if len(c_f) != self.n_l * 6:
raise RuntimeError(
'Incorrect size of c_f when calling save_simulation()')
if len(q) != self.n_dofs:
raise RuntimeError(
'Incorrect size of q when calling save_simulation()')
if not self.has_simulation(box_dims, pose):
f = open(self.filename_simulated, 'a')
values = list(box_dims)
if isinstance(pose, np.ndarray):
pose = se3.from_homogeneous(pose)
# saving pose.t
values += pose[1]
# saving pose.q
q_wxyz = np.array(so3.quaternion(pose[0]))
values += list(q_wxyz[1:4])
values.append(q_wxyz[0])
if isinstance(h_T_o, np.ndarray):
h_T_o = se3.from_homogeneous(h_T_o)
# saving h_T_o.t
values += h_T_o[1]
# saving h_T_o.q
q_wxyz = np.array(so3.quaternion(h_T_o[0]))
values += list(q_wxyz[1:4])
values.append(q_wxyz[0])
# saving q
values += list(q)
values += list(c_p)
values += list(c_f)
f.write(','.join([str(v) for v in values]))
f.write('\n')
f.close()
self._load_mvbbs_simulated()
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 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 from_numpy(obj, type='auto', template=None):
"""Converts a numpy array or multiple numpy arrays to a Klamp't object.
Supports:
* lists and tuples
* RigidTransform: accepts a 4x4 homogeneous coordinate transform
* Matrix3, Rotation: accepts a 3x3 matrix.
* Configs
* Trajectory: accepts a pair (times,milestones)
* TriangleMesh: accepts a pair (verts,indices)
* PointCloud: accepts a n x (3+k) array, where k is the # of properties
* VolumeGrid: accepts a triple (bmin,bmax,array)
* Geometry3D: accepts a pair (T,geomdata)
"""
global supportedTypes
if type == 'auto' and template is not None:
otype = types.objectToTypes(template)
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 == 'auto':
if isinstance(obj, (tuple, list)):
if all(isinstance(v, np.ndarray) for v in obj):
if len(obj) == 2:
if len(obj[0].shape) == 1 and len(obj[1].shape) == 2:
type = 'Trajectory'
elif len(obj[0].shape) == 2 and len(
obj[1].shape
) == 2 and obj[0].shape[1] == 3 and obj[1].shape[1] == 3:
type = 'TriangleMesh'
if len(obj) == 3:
if obj[0].shape == (3, ) and obj[1].shape == (3, ):
type = 'VolumeGrid'
if type == 'auto':
raise ValueError(
"Can't auto-detect type of list of shapes" +
', '.join(str(v.shape) for v in obj))
else:
if isinstance(obj[0], np.ndarray) and obj[0].shape == (4, 4):
type = 'Geometry3D'
else:
raise ValueError(
"Can't auto-detect type of irregular list")
else:
assert isinstance(
obj, np.ndarray
), "Can only convert lists, tuples, and arrays from numpy"
if obj.shape == (3, 3):
type = 'Matrix3'
elif obj.shape == (4, 4):
type = 'RigidTransform'
elif len(obj.shape) == 1:
type = 'Config'
else:
raise ValueError("Can't auto-detect type of matrix of shape " +
str(obj.shape))
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return se3.from_homogeneous(obj)
elif type == 'Rotation' or type == 'Matrix3':
return so3.from_matrix(obj)
elif type == 'Trajectory':
assert len(obj) == 2, "Trajectory format is (times,milestones)"
times = obj[0].tolist()
milestones = obj[1].tolist()
if template is not None:
return template.constructor()(times, milestones)
from klampt.model.trajectory import Trajectory
return Trajectory(times, milestones)
elif type == 'TriangleMesh':
from klampt import TriangleMesh
res = TriangleMesh()
vflat = obj[0].flatten()
res.vertices.resize(len(vflat))
for i, v in enumerate(vflat):
res.vertices[i] = float(v)
iflat = obj[1].flatten()
res.indices.resize(len(iflat))
for i, v in enumerate(iflat):
res.indices[i] = int(v)
return res
elif type == 'PointCloud':
from klampt import PointCloud
assert len(obj.shape) == 2, "PointCloud array must be a 2D array"
assert obj.shape[1] >= 3, "PointCloud array must have at least 3 values"
points = obj[:, :3]
properties = obj[:, 3:]
res = PointCloud()
res.setPoints(points.shape[0], points.flatten())
if template is not None:
if len(template.propertyNames) != properties.shape[1]:
raise ValueError(
"Template object doesn't have the same properties as the numpy object"
)
for i in range(len(template.propertyNames)):
res.propertyNames[i] = template.propertyNames[i]
else:
for i in range(properties.shape[1]):
res.propertyNames.append('property %d' % (i + 1))
if len(res.propertyNames) > 0:
res.properties.resize(len(res.propertyNames) * points.shape[0])
if obj.shape[1] >= 3:
res.setProperties(properties.flatten())
return res
elif type == 'VolumeGrid':
from klampt import VolumeGrid
assert len(obj) == 3, "VolumeGrid format is (bmin,bmax,values)"
assert len(obj[2].shape) == 3, "VolumeGrid values must be a 3D array"
bmin = obj[0]
bmax = obj[1]
values = obj[2]
res = VolumeGrid()
res.bbox.append(bmin[0])
res.bbox.append(bmin[1])
res.bbox.append(bmin[2])
res.bbox.append(bmax[0])
res.bbox.append(bmax[1])
res.bbox.append(bmax[2])
res.dims.append(values.shape[0])
res.dims.append(values.shape[1])
res.dims.append(values.shape[2])
vflat = values.flatten()
res.values.resize(len(vflat))
for i, v in enumerate(vflat):
res.values[i] = v
return res
elif type == 'Group':
from klampt import Geometry3D
res = Geometry3D()
assert isinstance(
obj,
(list, tuple)), "Group format is a list or tuple of Geometry3D's"
for i in range(len(obj)):
res.setElement(i, from_numpy(obj[i], 'Geometry3D'))
return res
elif type == 'Geometry3D':
from klampt import Geometry3D
if not isinstance(obj, (list, tuple)) or len(obj) != 2:
raise ValueError("Geometry3D must be a (transform,geometry) tuple")
T = from_numpy(obj[0], 'RigidTransform')
geomdata = obj[1]
subtype = None
if template is not None:
subtype = template.type()
if subtype == 'PointCloud':
g = Geometry3D(
from_numpy(geomdata, subtype, template.getPointCloud()))
else:
g = Geometry3D(from_numpy(geomdata, subtype))
g.setCurrentTransform(*T)
return g
subtype = 'Group'
if all(isinstance(v, np.ndarray) for v in geomdata):
if len(geomdata) == 2:
if len(geomdata[0].shape) == 1 and len(geomdata[1].shape) == 2:
subtype = 'Trajectory'
elif len(geomdata[0].shape) == 2 and len(
geomdata[1].shape) == 2 and geomdata[0].shape[
1] == 3 and geomdata[1].shape[1] == 3:
subtype = 'TriangleMesh'
if len(geomdata) == 3:
if geomdata[0].shape == (3, ) and geomdata[1].shape == (3, ):
subtype = 'VolumeGrid'
g = Geometry3D(from_numpy(obj, subtype))
g.setCurrentTransform(*T)
return g
else:
return obj.flatten()
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])