def __init__(self, robot_filename, serial_num, rotation, radius):
"""
:param robot_filename: {string} -- the location of the Klampt's robot files
:param serial_num: {string} -- the serial number of the calibration
:param rotation: {int} -- the rotation angle of the needle
:param radius: {float} -- the radius of the needle
:return: None
"""
self.world_file = robot_filename + "/worlds/flatworld.xml"
self.robot_file = robot_filename + "/robots/irb120_icp.rob"
self.world = get_world(self.world_file, self.robot_file, visualize_robot=True)
self.robot = self.world.robot(0)
self.suture_center = np.array(pd.read_csv("../../data/suture_experiment/suture_center_files/" +
serial_num + "_suture_center.csv", header=None))
print(self.suture_center)
self.calibration_res = np.load("../../data/suture_experiment/calibration_result_files/" +
serial_num + "/calibration_result.npy")
self.Toct = (so3.from_rpy(self.calibration_res[0:3]), self.calibration_res[3:6])
self.Tneedle = (so3.from_rpy(self.calibration_res[6:9]), self.calibration_res[9:12])
self.rotation = rotation
self.radius = radius
self.dim_arr = np.array(pd.read_csv("../../data/oct_volume_calibration/" +
serial_num + "/dimension.csv", header=None))
seed_config = np.multiply([-92.1441, 60.1883, 22.6847, 1.83303, -82.2083, 59.7906], np.pi / 180.).tolist()
self.robot.setConfig([0.] * 7 + seed_config + [0.])
self.robot.link('tool0').setParentTransform(self.Tneedle[0], self.Tneedle[1])
def opt_error_fun(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
Roct = so3.from_rpy(est_input[0:3])
toct = est_input[3:6]
Rn = so3.from_rpy(est_input[6:9])
tn = est_input[9:12]
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
fun_list = np.append(
fun_list,
np.multiply(
so3.error(so3.inv(Tneedle2oct_icp[i][0]),
so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]),
Rn)), 1))
fun_list = np.append(
fun_list,
np.multiply(
vectorops.sub(
vectorops.sub([0., 0., 0.],
so3.apply(so3.inv(Tneedle2oct_icp[i][0]),
Tneedle2oct_icp[i][1])),
so3.apply(
so3.inv(Roct),
vectorops.add(vectorops.sub(Tlink_set[i][1], toct),
so3.apply(Tlink_set[i][0], tn)))), 1000))
return fun_list
def opt_error_fun_penalize(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
est_Toct = (so3.from_rpy(est_input[0:3]), est_input[3:6])
est_Tneedle = (so3.from_rpy(est_input[6:9]), est_input[9:12])
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
Toct2needle_est = se3.mul(se3.mul(se3.inv(est_Toct), Tlink_set[i]),
est_Tneedle)
fun_list = np.append(
fun_list,
np.absolute(
np.multiply(
se3.error(Tneedle2oct_icp[i],
se3.inv(Toct2needle_est))[0:3],
[1, args[2], 1])))
fun_list = np.append(
fun_list,
np.absolute(
np.multiply(
se3.error(Tneedle2oct_icp[i],
se3.inv(Toct2needle_est))[3:6], 1000)))
return fun_list
def error_analysis(self):
"""
:return: None
"""
Toct = (so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6])
Tneedle = (so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12])
trans_error_list = []
trans_error_mat = []
rot_error_list = []
rot_error_mat = []
redraw_list = []
for i in range(len(self.Tlink_set)):
Toct_needle_est = se3.mul(
se3.mul(se3.inv(Toct), self.Tlink_set[i]), Tneedle)
trans_error = vectorops.sub(
se3.inv(self.trans_list[i])[1], Toct_needle_est[1])
rot_error = so3.error(
se3.inv(self.trans_list[i])[0], Toct_needle_est[0])
trans_error_list.append(vectorops.normSquared(trans_error))
trans_error_mat.append(np.absolute(trans_error))
rot_error_list.append(vectorops.normSquared(rot_error))
rot_error_mat.append(np.absolute(rot_error))
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[0])
redraw_list.append(
redraw_pcd(self.pcd_list[i], Toct_needle_est)[1])
redraw_list.append(
create_mesh_coordinate_frame(size=0.0015, origin=[0, 0, 0]))
draw_geometries(redraw_list)
trans_error_list = np.array(trans_error_list)
trans_error_mat = np.array(trans_error_mat)
rot_error_list = np.array(rot_error_list)
rot_error_mat = np.array(rot_error_mat)
rmse_trans = np.sqrt(np.mean(trans_error_list))
rmse_rot = np.sqrt(np.mean(rot_error_list))
print("The squared translation error list is:\n ",
np.sqrt(trans_error_list), "\nAnd the its RMSE is ", rmse_trans)
print("The mean error in XYZ directions is: ",
np.mean(trans_error_mat, axis=0))
print("The squared rotation error list is:\n ",
np.sqrt(rot_error_list), "\nAnd the its RMSE is ", rmse_rot)
print("The mean error in three rotation vectors is: ",
np.mean(rot_error_mat, axis=0))
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_rmse.npy", np.array([rmse_trans, rmse_rot]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_list.npy", np.array([np.sqrt(trans_error_list),
# np.sqrt(rot_error_list)]), allow_pickle=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_error_mat.npy", np.array([np.mean(trans_error_mat, axis=0),
# np.mean(rot_error_mat, axis=0)]), allow_pickle=True)
trans_all2needle(self.Tlink_set, Toct, Tneedle, self.pcd_list)
def draw_part_circle_3d(radius, center, start_Z, rot_angle, start_angle, rot_x,
pix_x, pix_h, pix_z):
"""
:param radius: {float} -- the radius of suture path
:param center: {list} -- the suture center in the OCT frame
:param start_Z: {float} -- the suture path Z axis value in the OCT frame
:param rot_angle: {float} -- the rotation angle of the suture path
:param start_angle: {float} -- the start suture angle
:param rot_x: {float} -- the suture path rotation on the OCT frame X axis
:param pix_x: {float} -- the length in pixels x length
:param pix_h: {float} -- the length in pixels y length
:param pix_z: {float} -- the length in pixels z length
:return: suture_path: {list} -- the suture milestones in the world frame
"""
suture_path = []
rot_mat = (so3.from_rpy([rot_x, 0.0, 0.0]), [0.0] * 3)
for i in range(0, rot_angle * 10):
current_angle = start_angle + i * 0.1
pos_x_0 = center[0] - radius * np.cos((current_angle * np.pi) / 180.)
pos_y_0 = center[1] + radius * np.sin((current_angle * np.pi) / 180.)
milestone = np.divide(se3.apply(rot_mat, [pos_x_0, pos_y_0, start_Z]),
[pix_x, pix_h, pix_z])
suture_path.append(milestone)
return suture_path
def cal_est_config_list(robot, serial_num):
est_config_list = []
local_pos = [[1, 0.0, 0.0], [0.0, 1, 0.0], [0.0, 0.0, 1]]
Ticp_list = np.load("../../data/suture_experiment/standard_trans_list/trans_array.npy")
est_calibration = np.load("../../data/suture_experiment/calibration_result_files/" +
serial_num + "B/calibration_result.npy")
Toct = (so3.from_rpy(est_calibration[0:3]), est_calibration[3:6])
Tneedle = (so3.from_rpy(est_calibration[6:9]), est_calibration[9:12])
for i in range(0, 9):
Tee = se3.mul(se3.mul(Toct, se3.inv(Ticp_list[i])), se3.inv(Tneedle))
world_pos = [se3.apply(Tee, local_pos[0]),
se3.apply(Tee, local_pos[1]),
se3.apply(Tee, local_pos[2])]
q = solve_ik(robot.link('link_6'), local_pos, world_pos, i)
est_config_list.append(q)
return est_config_list
def _do_rotation_change(index, element):
rpy[index] = element
value[0][:] = so3.from_rpy(rpy)
if klampt_widget:
klampt_widget.setTransform(name=xform_name, R=value[0], t=value[1])
if callback:
callback(value)
def 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 solve_placement(self, goal_bounds):
"""Implemented for you: come up with a collision-free target placement"""
xmin, ymin, zmin = goal_bounds[0]
xmax, ymax, zmax = goal_bounds[1]
#center_sampling_range = [(xmin+min_dims/2,xmax-min_dims/2),(ymin+min_dims/2,ymax-min_dims/2)]
center_sampling_range = [(xmin, xmax), (ymin, ymax), (zmin, zmax)]
Tobj_feasible = []
original_obj_tarnsform = self.object.getTransform()
for iters in range(20):
crand = [random.uniform(b[0], b[1]) for b in center_sampling_range]
if self.robot0:
Robj = so3.from_rpy((0, np.pi / 2, 0))
else:
Robj = so3.identity()
tobj = [crand[0], crand[1], crand[2]]
self.object.setTransform(Robj, tobj)
if not self.check_feasible():
continue
Tobj_feasible.append((Robj, tobj))
print("Found", len(Tobj_feasible), "valid object placements")
self.object.setTransform(original_obj_tarnsform[0],
original_obj_tarnsform[1])
print("Tobj_feasible", Tobj_feasible)
return Tobj_feasible
def _do_rotation_change(index,element):
rpy[index] = element
value[0][:] = so3.from_rpy(rpy)
if klampt_widget:
klampt_widget.setTransform(name=xform_name,R=value[0],t=value[1])
if callback:
callback(value)
def addTable(self, x=0, y=0):
"""
Add a table to the world
Parameters:
--------------
x,y: floats, the x,y position of the center of the table
"""
table_top = Geometry3D()
table_leg_1 = Geometry3D()
table_leg_2 = Geometry3D()
table_leg_3 = Geometry3D()
table_leg_4 = Geometry3D()
table_top.loadFile(model_path + "cube.off")
table_leg_1.loadFile(model_path + "cube.off")
table_leg_2.loadFile(model_path + "cube.off")
table_leg_3.loadFile(model_path + "cube.off")
table_leg_4.loadFile(model_path + "cube.off")
table_top.transform([self.table_length,0,0,0,self.table_width,0,0,0,\
self.table_top_thickness],[0,0,self.table_height - self.table_top_thickness])
table_leg_1.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height\
- self.table_top_thickness],[0,0,0])
table_leg_2.transform([self.leg_width,0,0,0,self.leg_width,0,0,0,self.table_height - \
self.table_top_thickness],[self.table_length-self.leg_width,0,0])
table_leg_3.transform([
self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0,
self.table_height - self.table_top_thickness
], [
self.table_length - self.leg_width,
self.table_width - self.leg_width, 0
])
table_leg_4.transform([
self.leg_width, 0, 0, 0, self.leg_width, 0, 0, 0,
self.table_height - self.table_top_thickness
], [0, self.table_width - self.leg_width, 0])
table_geom = Geometry3D()
table_geom.setGroup()
for i, elem in enumerate(
[table_top, table_leg_1, table_leg_2, table_leg_3, table_leg_4]):
g = Geometry3D(elem)
table_geom.setElement(i, g)
#table_geom.transform([1,0,0,0,1,0,0,0,1],[x-self.table_length/2.0,y-self.table_width/2.0,0])
table_geom.transform(
so3.from_rpy([0, 0, math.pi / 2]),
[x - self.table_length / 2.0, y - self.table_width / 2.0, 0])
table = self.w.makeTerrain("table")
table.geometry().set(table_geom)
table.appearance().setColor(self.light_blue[0], self.light_blue[1],
self.light_blue[2], self.light_blue[3])
def test_from_rpy(self):
R = so3.from_rpy((0, 0, 0))
builder.addRobot("./Anthrax.urdf", None)
w = builder.getWorld()
robot = w.robot(0)
reset_arms(robot)
sim = klampt.Simulator(w)
# setup simulated camera
cam = klampt.SimRobotSensor(sim.controller(0), "rgbd_camera", "CameraSensor")
# mount camera in place
cam.setSetting("link", "4")
#cam.setSetting("Tsensor","0.0 -0.866 -0.5 1.0 0.0 0.0 0 -0.5 0.866 0.2 0.005 1.2")
cam.setSetting(
"Tsensor",
transform_to_string(so3.from_rpy((math.pi, radians(-30), radians(270))),
[0.2, 0.005, 1.2]))
# minimum range
cam.setSetting("zmin", "0.1")
# x field of view
cam.setSetting("xfov", "1.5")
# add to visualization
vis.add("world", w)
vis.add("cam", cam)
# Take a picture!
cam.kinematicSimulate(w, 0.01)
rgb, depth = sensing.camera_to_images(cam)
#plt.imshow(rgb)
#plt.show()
def solve_qplace(self, Tplacement):
if self.robot0:
v = list(np.array(Tplacement[1]) - np.array([0.16, 0, 0]))
objective = ik.objective(self.robot.link(9), R=Tplacement[0], t=v)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('qplace')
return None
qpreplace = self.robot.getConfig()
# add a waypoint to insert swab into human face
face_trans = [0.34, -0.3, 1.05] # [0.5, -0.35, 1.35]
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy((np.pi, -np.pi / 2, 0)),
t=face_trans)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab near mouth')
return None
self.qmouth = self.robot.getConfig()
self.qmouth = self.gripper.set_finger_config(
self.qmouth,
self.gripper.partway_open_config(self.close_amount))
face_trans = [0.34, -0.4, 1.05] # [0.5, -0.35, 1.35]
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy((np.pi, -np.pi / 2, 0)),
t=face_trans)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7])
if not solved:
print('cannot insert swab into mouth')
return None
self.qmouth_in = self.robot.getConfig()
self.qmouth_in = self.gripper.set_finger_config(
self.qmouth_in,
self.gripper.partway_open_config(self.close_amount))
# add a waypoint to lower down the gripper
v1 = list(np.array(v) - np.array([0, 0, 0.1]))
# vis.add('v1', v1, color=[0, 0, 1, 1])
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, 0, -np.pi / 2)),
t=v1)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('qlower')
return None
self.qlower = self.robot.getConfig()
self.qlower = self.gripper.set_finger_config(
self.qlower,
self.gripper.partway_open_config(self.close_amount))
# add waypoints to dip the swab into the plate
goal = (0.7, 0.35, 0.9)
t = list(np.array(goal) - np.array([0.16, 0, 0]))
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, np.pi / 2, -np.pi / 2)),
t=t)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab into plate')
return None
self.qplaceplate = self.robot.getConfig()
self.qplaceplate = self.gripper.set_finger_config(
self.qplaceplate,
self.gripper.partway_open_config(self.close_amount))
t1 = list(np.array(t) - np.array([0, 0, 0.06]))
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, np.pi / 2, -np.pi / 2)),
t=t1)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab into plate')
return None
self.qplaceplate_lower = self.robot.getConfig()
self.qplaceplate_lower = self.gripper.set_finger_config(
self.qplaceplate_lower,
self.gripper.partway_open_config(self.close_amount))
# add waypoints to throw the swab into the trash can
goal = (0.1, -0.4, 0.8)
t = list(np.array(goal) - np.array([0.16, 0, 0]))
objective = ik.objective(self.robot.link(9),
R=so3.from_rpy(
(-np.pi / 2, np.pi / 2, -np.pi / 2)),
t=t)
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved:
print('cannot place swab into plate')
return None
self.qplace_trash = self.robot.getConfig()
self.qplace_trash = self.gripper.set_finger_config(
self.qplace_trash,
self.gripper.partway_open_config(self.close_amount))
self.qplace = self.gripper.set_finger_config(
self.qplace_trash, self.gripper.partway_open_config(1))
else:
Tplacement_x = se3.mul(Tplacement, se3.inv(self.Tobject_gripper))
objective = ik.objective(self.robot.link(9),
R=Tplacement_x[0],
t=Tplacement_x[1])
solved = ik.solve_global(objectives=objective,
iters=50,
numRestarts=5,
activeDofs=[1, 2, 3, 4, 5, 6, 7],
feasibilityCheck=self.feasible)
if not solved: return None
qpreplace = self.robot.getConfig()
qplace = self.gripper.set_finger_config(
qpreplace, self.gripper.partway_open_config(1))
return qplace
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])
def test_from_rpy(self):
R = so3.from_rpy((0,0,0))
def integrate_velocities(controller, sim, dt):
"""The make() function returns a 1-argument function that takes a SimRobotController and performs whatever
processing is needed when it is invoked."""
global start_time
global now_dur
global syn_curr
global syn_next
global entered_once
global got_syn
if not entered_once:
syn_vec = controller.getCommandedConfig()
if not syn_vec:
got_syn = False
entered_once = False
else:
got_syn = True
entered_once = True
syn_curr = syn_vec[34]
start_time = sim.getTime()
else:
syn_curr = syn_next
now_dur = sim.getTime() - start_time
# print 'The current simulation duration is', now_dur
rob = sim.controller(0).model()
palm_curr = mv_el.get_moving_base_xform(rob)
R = palm_curr[0]
t = palm_curr[1]
if DEBUG:
print 'The current palm rotation is ', R
print 'The current palm translation is ', t
print 'The simulation dt is ', dt
# Converting to rpy
euler = so3.rpy(R)
# Checking if list empty and returning false
if not got_syn:
return False, None, None
if DEBUG:
print 'The integration time is ', dt
print 'The adaptive velocity of hand is ', global_vars.hand_command
print 'The adaptive twist of palm is \n', global_vars.arm_command
# print 'The commanded position of the hand encoder (in memory) is ', syn_curr
# print 'The present position of the hand encoder (sensed) is ', controller.getCommandedConfig()[34]
v = rob.getVelocity()
print 'The actual velocity of the robot is ', v
# print 'The present pose of the palm is \n', palm_curr
# print 'The present position of the palm is ', t, 'and its orientation is', euler
# Getting linear and angular velocities
lin_vel_vec = global_vars.arm_command.linear
ang_vel_vec = global_vars.arm_command.angular
lin_vel = [lin_vel_vec.x, lin_vel_vec.y, lin_vel_vec.z]
ang_vel = [ang_vel_vec.x, ang_vel_vec.y, ang_vel_vec.z]
ang_vel_loc = so3.apply(so3.inv(R),
ang_vel) # rotating ang vel to local frame
# Transform ang vel into rpy vel
euler_vel = transform_ang_vel(euler, ang_vel_loc)
palm_vel = []
palm_vel.extend(lin_vel)
palm_vel.extend([euler_vel[0], euler_vel[1], euler_vel[2]]) # appending
syn_vel = global_vars.hand_command
# Integrating
syn_next = syn_curr + syn_vel * int_const_syn * dt
t_next = vectorops.madd(t, lin_vel, int_const_t * dt)
euler_next = vectorops.madd(euler, euler_vel, int_const_eul * dt)
# Saturating synergy
if syn_next >= 1.0:
syn_next = 1.0
# Convert back for send xform
palm_R_next = so3.from_rpy(euler_next)
palm_t_next = t_next
palm_next = (palm_R_next, palm_t_next)
if DEBUG or True:
print 'euler is ', euler, ' and is of type ', type(euler)
print 'euler_vel is ', euler_vel, ' and is of type ', type(euler_vel)
print 'euler_next is ', euler_next, ' and is of type ', type(
euler_next)
#
# print 't is ', t, ' and is of type ', type(t)
# print 't_next is ', t_next, ' and is of type ', type(t_next)
#
# print 'R is ', R, ' and is of type ', type(R)
# print 'palm_R_next is ', palm_R_next, ' and is of type ', type(palm_R_next)
#
# print 'palm_curr is ', palm_curr, ' and is of type ', type(palm_curr)
# print 'palm_next is ', palm_next, ' and is of type ', type(palm_next)
return True, syn_next, palm_next, syn_vel, palm_vel
