def to_Quaternion(*args, **kwargs):
'''
converts the input to a geometry_msgs/Quaternion message
supported input:
- ROS Pose message (position ignored)
- ROS PoseStamped message (position ignored)
- ROS Transform message (position ignored)
- ROS TransformStamped message (position ignored)
- ROS Quaternion message
- ROS QuaternionStamped message
- kdl Frame (position ignored)
- kdl Rotation
- tf transform (position ignored)
- iterable of length 4 (list, tuple, np.array, ...) (x, y, z, w)
- a 3x3 or 4x4 numpy array
- individual x, y, z, w values
- named x, y, z, w arguments. (minimum of 1 can be specified)
- no arguments will result in an identity rotation
keyword args:
- x, y, z
Note: if more than 1 argument then they should be in the order they appear
'''
# None passed in
if (len(args) == 1 and args[0] == None):
return geometry_msgs.Quaternion(0, 0, 0, 1)
if (len(args) == 1):
# Quaternion
if (type(args[0]) == geometry_msgs.Quaternion):
return copy.deepcopy(args[0])
# QuaternionStamped
if (type(args[0]) == geometry_msgs.QuaternionStamped):
return copy.deepcopy(args[0].quaternion)
# Pose
if (type(args[0]) == geometry_msgs.Pose):
return copy.deepcopy(args[0].orientation)
# PoseStamped
if (type(args[0]) == geometry_msgs.PoseStamped):
return copy.deepcopy(args[0].pose.orientation)
# Transform
if (type(args[0]) == geometry_msgs.Transform):
return copy.deepcopy(args[0].rotation)
# TransformStamped
if (type(args[0]) == geometry_msgs.TransformStamped):
return copy.deepcopy(args[0].transform.rotation)
# kdl Frame
if (type(args[0]) == kdl.Frame):
return geometry_msgs.Quaternion(*args[0].M.GetQuaternion())
# kdl Rotation
if (type(args[0]) == kdl.Rotation):
return geometry_msgs.Quaternion(*args[0].GetQuaternion())
# 3x3 or 4x4 numpy array
if (type(args[0]) == np.ndarray and args[0].ndim == 2):
(r, c) = args[0].shape
if (r >= 3 and c >= 3):
rot = kdl.Rotation(args[0][0, 0], args[0][0, 1], args[0][0, 2],
args[0][1, 0], args[0][1, 1], args[0][1, 2],
args[0][2, 0], args[0][2, 1], args[0][2, 2])
return geometry_msgs.Quaternion(*rot.GetQuaternion())
# tf transform ((x,y,z), (x,y,z,w))
if (hasattr(args[0], '__iter__') and len(args[0]) == 2
and len(args[0][1]) == 4):
return geometry_msgs.Quaternion(*args[0][1][:4])
# iterable length 4
if (hasattr(args[0], '__iter__') and len(args[0]) >= 4):
return geometry_msgs.Quaternion(*args[0][:4])
# 4 arguments
elif (len(args) == 4):
return geometry_msgs.Quaternion(*args)
# x, y, z, or w keyword arguments
elif ('x' in kwargs or 'y' in kwargs or 'z' in kwargs or 'w' in kwargs):
x = 0.0
y = 0.0
z = 0.0
w = 0.0
if ('x' in kwargs):
x = kwargs['x']
if ('y' in kwargs):
y = kwargs['y']
if ('z' in kwargs):
z = kwargs['z']
if ('w' in kwargs):
w = kwargs['w']
return geometry_msgs.Quaternion(x, y, z, w)
elif (len(args) == 0):
return geometry_msgs.Quaternion(0.0, 0.0, 0.0, 1.0)
import numpy as np import PyKDL as kdl # When taking the rotation vector directly from KDL, there are # numeric problems when the rotation matrix is diagonal, for example # 180 degree rotation around any or the axes. In this case the vector # is set to zero even if it should be for example (0, 0, pi) for a # 180 degree rotation around z # frames.cpp line 325 GetRot rot = kdl.Rotation() rot.DoRotZ(np.pi) print rot.GetRot() rot = kdl.Rotation() rot.DoRotZ(np.pi + 0.001) print rot.GetRot() rot3 = kdl.Rotation() rot3.DoRotX(np.pi) print rot3.GetRotAngle() rot2 = kdl.Rotation() rot2.DoRotY(np.pi) print rot2.GetRotAngle() rot4 = kdl.Rotation() rot4.DoRotZ(np.pi) print rot4.GetRotAngle()
def step(self,
dt,
lin_vel=(0, 0, 0),
rot_vel=(0, 0, 0),
check=False,
base_time=None):
q0 = self._q
X0 = self._x
dist = np.linalg.norm(lin_vel)
if dist > 0:
lin_step = kdl.Vector(*(np.array(lin_vel) * dt))
else:
lin_step = kdl.Vector()
angle = np.linalg.norm(rot_vel)
if angle > 0:
axis = np.array(rot_vel) / angle
rot_step = kdl.Rotation.Rot(kdl.Vector(*axis), angle * dt)
else:
rot_step = kdl.Rotation()
base_tf = kdl.Frame(kdl.Rotation(), lin_step)
tool_tf = kdl.Frame(rot_step, kdl.Vector())
X1 = base_tf * X0 * tool_tf
X2 = base_tf * X1 * tool_tf
p = np.array([X1.p.x(), X1.p.y(), X1.p.z()])
if np.any(p < self._workspace[0, :] - 0.01) or np.any(
p > self._workspace[1, :] + 0.01):
# raise ControllerException(
# 'Target outside workspace bounds: {} / {}'.format(p, self._workspace))
return
p = np.array([X2.p.x(), X2.p.y(), X2.p.z()])
if np.any(p < self._workspace[0, :] - 0.01) or np.any(
p > self._workspace[1, :] + 0.01):
# raise ControllerException(
# 'Target outside workspace bounds: {} / {}'.format(p, self._workspace))
return
q1 = self._kin.inverse(pm.toMatrix(X1), q_guess=q0)
if q1 is None:
raise ControllerException('IK solution not found')
jerk = np.max(np.abs(np.subtract(q0, q1)))
if jerk > 0.2:
raise ControllerException('IK solution not continuous')
q2 = self._kin.inverse(pm.toMatrix(X2), q_guess=q1)
if q2 is None:
raise ControllerException('IK solution not found')
jerk = np.max(np.abs(np.subtract(q1, q2)))
if jerk > 0.2:
raise ControllerException('IK solution not continuous')
if check:
points = [pm.toMsg(w) for w in [X1, X2]]
(_, frac) = self._group.compute_cartesian_path(points, 0.01, 5.0)
if frac < 1.0:
raise ControllerException('Moveit planner failed')
self._q = q1
self._x = X1
traj = JointTrajectory()
if base_time is not None:
traj.header.stamp = base_time
traj.joint_names = self._joint_names
# target position
point = JointTrajectoryPoint()
point.positions = q1
point.time_from_start = rospy.Duration(dt)
traj.points.append(point)
# prediction for future on case huge delays
point = JointTrajectoryPoint()
point.positions = q2
point.time_from_start = rospy.Duration(dt * 2)
traj.points.append(point)
self._publisher.publish(traj)
def add_mat(mat1, mat2):
out = PyKDL.Rotation()
for i in range(0, 3):
for j in range(0, 3):
out[i, j] = mat1[i, j] + mat2[i, j]
return out
def skew_mat(v):
return PyKDL.Rotation(0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0)
def test_initialize2(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(20, 20, 2)), "/map"
)
self.tour_guide.initialize()
self.assertListEqual(self.tour_guide._passed_room_ids, [])
def setUp(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(-1, -1, 0)), "/map"
)
self.tour_guide.initialize()
def np_rot_to_kdl(rot):
return kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1],
rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2])
kdl_pose = psm2.get_current_position()
print("Current Position:")
pprint.pprint(kdl_pose)
""" An arbitrary z starting point that is not too close/far from the platform.
When the gripper picks up a needle, it moves up to this point and then releases the needle.
Change if it is too high/low above the platform. """
z_upper = -0.08
# z_final = -0.06
""" Where PSM2 touches the platform """
# For white background:
z_lower = -0.1233
# For phantom background:
# z_lower = -0.128
""" POSE PSM2 WAS CALIBRATED IN """
pos = PyKDL.Vector(-0.118749, 0.0203151, -0.081688)
rot = PyKDL.Rotation(-0.988883, -0.00205771, -0.148682, -0.00509171,
0.999786, 0.0200282, 0.148609, 0.0205626, -0.988682)
pos2 = PyKDL.Vector(-0.0972128, -0.0170138, -0.106974)
sideways = PyKDL.Rotation(-0.453413, 0.428549, -0.781513, -0.17203,
0.818259, 0.548505, 0.874541, 0.383143,
-0.297286)
""" Move to arbitrary start position (near upper left corner) & release anything gripper is
holding. """
# home(psm2, pos, rot)
home(psm2, pos2, sideways)
""" Get PSM and endoscope calibration data (25 corresponding chess points) """
psm2_calibration_data = list(
transform.load_all('../utils/psm2_recordings.txt'))
psm2_calibration_matrix = transform.psm_data_to_matrix(
psm2_calibration_data)
endoscope_calibration_matrix = np.matrix(
def displayRegistration(cams,
camModel,
toolOffset,
camTransform,
tfSync,
started=False,
affine=None,
targetLink=None):
rate = rospy.Rate(15) # 15hz
while not rospy.is_shutdown():
# Get last images
imageL = cams.camL.image
imageR = cams.camR.image
# Wait for images to exist
if type(imageR) == type(None) or type(imageL) == type(None):
rate.sleep()
continue
# Check we have valid images
(rows, cols, channels) = imageL.shape
if cols < 60 or rows < 60 or imageL.shape != imageR.shape:
rate.sleep()
continue
# Get position
if targetLink is None:
poseMsg = tfSync.synchedMessages[0].pose
robotPosition = posemath.fromMsg(poseMsg)
else:
transforms = tfSync.getTransforms([targetLink], tfSync.frames[0])
pos = (transforms[0].transform.translation.x,
transforms[0].transform.translation.y,
transforms[0].transform.translation.z)
rot = (transforms[0].transform.rotation.x,
transforms[0].transform.rotation.y,
transforms[0].transform.rotation.z,
transforms[0].transform.rotation.w)
robotPosition = posemath.fromTf((pos, rot))
# Find 3D position of end effector
offset = getOffset(robotPosition, toolOffset)
pVector = robotPosition.p
pos = np.matrix([pVector.x(), pVector.y(),
pVector.z()]) + offset
pos = np.vstack((pos.transpose(), [1]))
# Publish TFs for easier debugging
pubTF(robotPosition, tfSync.frames[0], 'get_position')
pubTF(
PyKDL.Frame(PyKDL.Rotation(), PyKDL.Vector(pos[0], pos[1],
pos[2])),
tfSync.frames[0], 'tip_pose')
# Calculate 3D point in camera space
if affine is None:
pos = np.linalg.inv(camTransform) * pos
else:
pos = np.linalg.inv(affine * camTransform) * pos
# Project position into 2d coordinates
posL = camModel.left.project3dToPixel(pos)
posL = [int(l) for l in posL]
posR = camModel.right.project3dToPixel(pos)
posR = [int(l) for l in posR]
(rows, cols) = imageL.shape[0:2]
posR = (posR[0] + cols, posR[1])
transforms = tfSync.getTransforms()
posEnd = posL
for i in range(0, len(transforms) - 1):
start = [
transforms[i].transform.translation.x,
transforms[i].transform.translation.y,
transforms[i].transform.translation.z
]
end = [
transforms[i + 1].transform.translation.x,
transforms[i + 1].transform.translation.y,
transforms[i + 1].transform.translation.z
]
# Project position into 2d coordinates
posStartL = camModel.left.project3dToPixel(start)
posEndL = camModel.left.project3dToPixel(end)
posStartR = camModel.right.project3dToPixel(start)
posEndR = camModel.right.project3dToPixel(end)
# Draw on left and right images
if not np.isnan(posStartL + posEndL + posStartR + posEndR).any():
posStartL = [int(l) for l in posStartL]
posEndL = [int(l) for l in posEndL]
cv2.line(imageL, tuple(posStartL), tuple(posEndL), (0, 255, 0),
1)
posStartR = [int(l) for l in posStartR]
posEndR = [int(l) for l in posEndR]
cv2.line(imageR, tuple(posStartR), tuple(posEndR), (0, 255, 0),
1)
cv2.line(imageL, tuple(posEnd), tuple(posL), (0, 255, 0), 1)
point3d, image = calculate3DPoint(imageL, imageR, camModel)
# Draw images and display them
cv2.circle(image, tuple(posL), 2, (255, 255, 0), -1)
cv2.circle(image, tuple(posR), 2, (255, 255, 0), -1)
cv2.circle(image, tuple(posL), 7, (255, 255, 0), 2)
cv2.circle(image, tuple(posR), 7, (255, 255, 0), 2)
if not started:
message = "Press s to start registration. Robot will move to its joint limits."
cv2.putText(image, message, (50, 50), cv2.FONT_HERSHEY_DUPLEX, 1,
[0, 0, 255])
message = "MAKE SURE AREA IS CLEAR"
cv2.putText(image, message, (50, 100), cv2.FONT_HERSHEY_DUPLEX, 1,
[0, 0, 255])
cv2.imshow(_WINDOW_NAME, image)
key = cv2.waitKey(1)
if key == 27:
cv2.destroyAllWindows()
quit() # esc to quit
elif not started and (chr(key % 256) == 's' or chr(key % 256) == 'S'):
break # s to continue
rate.sleep()
fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(arm_chain)
Jnt_list = PyKDL.JntArray(7)
Jnt_list_dmp = PyKDL.JntArray(7)
pose = []
pose_dmp = []
for i in range(train_len):
Jnt_list[0] = joint0_data[i]
Jnt_list[1] = joint1_data[i]
Jnt_list[2] = joint2_data[i]
Jnt_list[3] = joint3_data[i]
Jnt_list[4] = joint4_data[i]
Jnt_list[5] = joint5_data[i]
Jnt_list[6] = joint6_data[i]
fk_p_kdl.JntToCart(Jnt_list, end_frame)
pos = end_frame.p
rot = PyKDL.Rotation(end_frame.M)
rot = rot.GetQuaternion()
pose.append(
np.array([pos[0], pos[1], pos[2], rot[0], rot[1], rot[2], rot[3]]))
xs = []
ys = []
zs = []
for j in range(train_len):
position_x = pose[j][0]
position_y = pose[j][1]
position_z = pose[j][2]
xs.append(position_x)
ys.append(position_y)
zs.append(position_z)
def __init__(self, vol_radius, vol_samples_count, T_H_O, orientations_angle, vertices_obj, faces_obj):
self.vol_radius = vol_radius
self.vol_samples_count = vol_samples_count
self.index_factor = float(self.vol_samples_count)/(2.0*self.vol_radius)
self.vol_samples = []
for x in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
self.vol_samples.append([])
for y in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
self.vol_samples[-1].append([])
for z in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
self.vol_samples[-1][-1].append([])
self.vol_samples[-1][-1][-1] = {}
self.vol_sample_points = []
for xi in range(self.vol_samples_count):
for yi in range(self.vol_samples_count):
for zi in range(self.vol_samples_count):
self.vol_sample_points.append( self.getVolPoint(xi,yi,zi) )
self.T_H_O = T_H_O
self.T_O_H = self.T_H_O.Inverse()
# generate the set of orientations
self.orientations_angle = orientations_angle
normals_sphere = velmautils.generateNormalsSphere(self.orientations_angle)
orientations1 = velmautils.generateFramesForNormals(self.orientations_angle, normals_sphere)
orientations2 = []
for ori in orientations1:
x_axis = ori * PyKDL.Vector(1,0,0)
if x_axis.z() > 0.0:
orientations2.append(ori)
self.orientations = {}
for ori_idx in range(len(orientations2)):
self.orientations[ori_idx] = orientations2[ori_idx]
# generate a set of surface points
self.surface_points_obj = surfaceutils.sampleMeshDetailedRays(vertices_obj, faces_obj, 0.0015)
print "surface of the object has %s points"%(len(self.surface_points_obj))
# disallow contact with the surface points beyond the key handle
for p in self.surface_points_obj:
if p.pos.x() > 0.0:
p.allowed = False
surface_points_obj_init = []
surface_points2_obj_init = []
for sp in self.surface_points_obj:
if sp.allowed:
surface_points_obj_init.append(sp)
else:
surface_points2_obj_init.append(sp)
print "generating a subset of surface points of the object..."
while True:
p_idx = random.randint(0, len(self.surface_points_obj)-1)
if self.surface_points_obj[p_idx].allowed:
break
p_dist = 0.003
self.sampled_points_obj = []
while True:
self.sampled_points_obj.append(p_idx)
surface_points_obj2 = []
for sp in surface_points_obj_init:
if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist:
surface_points_obj2.append(sp)
if len(surface_points_obj2) == 0:
break
surface_points_obj_init = surface_points_obj2
p_idx = surface_points_obj_init[0].id
print "subset size: %s"%(len(self.sampled_points_obj))
print "generating a subset of other surface points of the object..."
p_dist2 = 0.006
while True:
p_idx = random.randint(0, len(self.surface_points_obj)-1)
if not self.surface_points_obj[p_idx].allowed:
break
self.sampled_points2_obj = []
while True:
self.sampled_points2_obj.append(p_idx)
surface_points2_obj2 = []
for sp in surface_points2_obj_init:
if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist2:
surface_points2_obj2.append(sp)
if len(surface_points2_obj2) == 0:
break
surface_points2_obj_init = surface_points2_obj2
p_idx = surface_points2_obj_init[0].id
# test volumetric model
if False:
for pt_idx in self.sampled_points2_obj:
pt = self.surface_points_obj[pt_idx]
m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None)
rospy.sleep(0.001)
for pt_idx in self.sampled_points_obj:
pt = self.surface_points_obj[pt_idx]
m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None)
rospy.sleep(0.001)
print "subset size: %s"%(len(self.sampled_points2_obj))
print "calculating surface curvature at sampled points of the obj..."
m_id = 0
planes = 0
edges = 0
points = 0
for pt_idx in range(len(self.surface_points_obj)):
indices, nx, pc1, pc2 = surfaceutils.pclPrincipalCurvaturesEstimation(self.surface_points_obj, pt_idx, 5, 0.003)
# m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + nx*0.004), m_id, 1, 0, 0, frame='world', namespace='default', scale=0.0002)
# m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + self.surface_points_obj[pt_idx].normal*0.004), m_id, 0, 0, 1, frame='world', namespace='default', scale=0.0002)
self.surface_points_obj[pt_idx].frame = PyKDL.Frame(PyKDL.Rotation(nx, self.surface_points_obj[pt_idx].normal * nx, self.surface_points_obj[pt_idx].normal), self.surface_points_obj[pt_idx].pos)
self.surface_points_obj[pt_idx].pc1 = pc1
self.surface_points_obj[pt_idx].pc2 = pc2
if pc1 < 0.2:
self.surface_points_obj[pt_idx].is_plane = True
self.surface_points_obj[pt_idx].is_edge = False
self.surface_points_obj[pt_idx].is_point = False
planes += 1
elif pc2 < 0.2:
self.surface_points_obj[pt_idx].is_plane = False
self.surface_points_obj[pt_idx].is_edge = True
self.surface_points_obj[pt_idx].is_point = False
edges += 1
else:
self.surface_points_obj[pt_idx].is_plane = False
self.surface_points_obj[pt_idx].is_edge = False
self.surface_points_obj[pt_idx].is_point = True
points += 1
print "obj planes: %s edges: %s points: %s"%(planes, edges, points)
import rospy
import dvrk
import numpy as np
import signal
import PyKDL
from sensor_msgs.msg import Joy
def get_cartesian(pose):
position = pose.p
x = position.x()
y = position.y()
z = position.z()
output = np.array([x, y, z])
return output
"""define the waypoint positions for the PSMs for this load test"""
cart1 = PyKDL.Vector(0.14514, -0.0666, -0.09294)
rot1 = PyKDL.Rotation()
rot1 = rot1.Quaternion(0.7209, -0.0192, 0.6925, 0.0152)
pos1 = PyKDL.Frame(rot1, cart1)
p2 = dvrk.psm('PSM2')
# set our rate to 30hz
rate = rospy.Rate(30)
print 'homing to position...'
p2.move(pos1)
print(Xb)
z = Xa*Xb
print(z)
y = Xa*z
print(y)
z_norm = z.Normalize()
y_norm = y.Normalize()
Xa_norm = Xa.Normalize()
print("X normalized: " + str(Xa))
print("Y normalized: " + str(y))
print("Z normalized: " + str(z))
#vectors = numpy.array([[xyz1.p[0],xyz1.p[1],xyz1.p[2],Xa[0],Xa[1],Xa[2]],
# [xyz1.p[0],xyz1.p[1],xyz1.p[2],y[0],y[1],y[2]],
# [xyz1.p[0],xyz1.p[1],xyz1.p[2],z[0],z[1],z[2]]])
#X,Y,Z,U,V,W = zip(*vectors)
#fig=plt.figure()
#ax = fig.add_subplot(111,projection='3d')
#ax.quiver(X,Y,Z,U,V,W)
#plt.show()
ori_v = PyKDL.Vector(xyz1.p[0],xyz1.p[1],xyz1.p[2])
ori_r = PyKDL.Rotation(Xa[0],y[0],z[0],Xa[1],y[1],z[1],Xa[2],y[2],z[2])
surgical_cs = PyKDL.Frame(ori_r,ori_v)
#ori_xyz_c = numpy.array( [[ Xa[0], y[0], z[0]] ,[Xa[1], y[1] ,z[1]], [Xa[2], y[2] ,z[2]]])
print(surgical_cs)
#print(ori_xyz_c)
def __init__(self,
base=PyKDL.Frame(),
size=np.array([1, 1, 1]),
size_m=np.array([0.05, 0.05, 0.05]),
indices_offset=np.array([0, 0, 1]),
relative_frames=True,
execution_order=[0, 1, 2],
name_matrix=""):
super(TfMatrixSphere, self).__init__(base=base,
size=size,
size_m=size_m)
a_lb = range(0, self.size[0])
b_lb = range(0, self.size[1])
c_lb = range(0, self.size[2])
n = self.size[0] * self.size[1] * self.size[2]
c_list = c_lb
a_list = []
b_list = []
for b in range(0, self.size[1] * self.size[2]):
a_list.extend(a_lb)
a_lb.reverse()
for c in range(0, self.size[2]):
b_list.extend(b_lb)
b_lb.reverse()
indices_list = []
for s in range(0, n):
a = s
b = int(s / self.size[0])
c = int(s / (self.size[0] * self.size[1]))
index = (a_list[a], b_list[b], c_list[c])
indices_list.append(index)
for index in indices_list:
ithetaZ = index[execution_order[0]]
ithetaY = index[execution_order[1]]
iz = index[execution_order[2]]
dthetaZ = size_m[0] * (ithetaZ + 1 + indices_offset[0])
dthetaY = size_m[1] * (ithetaY + 1 + indices_offset[1])
dz = size_m[2] * (iz + 1 + indices_offset[2])
if name_matrix != "":
name_matrix = "_" + name_matrix
else:
name = ""
current_name = "matrix_sphere_{}_{}_{}{}".format(
ithetaZ, ithetaY, iz, name)
Tr_rz = PyKDL.Frame(PyKDL.Rotation().RotZ(dthetaZ))
Tr_ry = PyKDL.Frame(PyKDL.Rotation().RotY(dthetaY))
Tr_z = PyKDL.Frame(PyKDL.Vector(0, 0, dz))
Tr = Tr_rz * Tr_ry * Tr_z
if relative_frames:
current_frame = self.base * Tr
else:
current_frame = self.base + Tr
self.frames_names.append(current_name)
self.frames_map[current_name] = current_frame
for v in eigVectors:
m = markerVector(id, v * eigValues[id - 1], mean)
id = id + 1
points_pub.publish(m)
#building the rotation matrix
eigx_n = PyKDL.Vector(Eigenvectors[0, 0], Eigenvectors[0, 1],
Eigenvectors[0, 2])
eigy_n = -PyKDL.Vector(Eigenvectors[1, 0], Eigenvectors[1, 1],
Eigenvectors[1, 2])
eigz_n = PyKDL.Vector(Eigenvectors[2, 0], Eigenvectors[2, 1],
Eigenvectors[2, 2])
eigx_n.Normalize()
eigy_n.Normalize()
eigz_n.Normalize()
rot = PyKDL.Rotation(eigx_n, eigy_n, eigz_n)
quat = rot.GetQuaternion()
rospy.loginfo(quat)
rospy.loginfo(Eigenvectors[0, 2])
#painting the Gaussian Ellipsoid Marker
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = eigValues[0] * 2
marker.scale.y = eigValues[1] * 2
marker.scale.z = eigValues[2] * 2
marker.color.a = 0.5
marker.color.r = 0.0
import math
# ROS
import PyKDL as kdl
# TU/e Robotics
from robot_skills.util.kdl_conversions import FrameStamped
from robocup_knowledge import knowledge_loader
# Common knowledge
common = knowledge_loader.load_knowledge("common")
# Detection
cabinet_amcl = "bookcase"
cabinet_poses = [
FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(0.144, 3.574,
0.0)),
frame_id="map"),
FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 1.570796),
kdl.Vector(2.271, -1.258, 0.0)),
frame_id="map")
]
object_shelves = ["shelf2", "shelf3", "shelf4", "shelf5"]
object_types = [obj["name"] for obj in common.objects]
# Grasping
grasp_surface = "cabinet"
room = "dining_room"
# Placing
default_place_entity = cabinet_amcl
#from Roll Pitch, Yaw angles to a rotation rpy = r1.GetRPY() print "rpy:", rpy # create a rotation from XYZ Euler angles r2 = PyKDL.Rotation.EulerZYX(0, 1, 0) print"r2:\n", r2 #from XYZ Euler angles to a rotation EulerZYX = r2.GetEulerZYX() print "EulerZYX:", EulerZYX print "Quaternion:", r2.GetQuaternion() # create a rotation from a rotation matrix r3 = PyKDL.Rotation(1, 0, 0, 2, 1, 0, 3, 0, 1) print "r3:\n", r3 #create a rotation from a Quaternion r4 = PyKDL.Rotation.Quaternion(0.0, 0.47942553860420295, 0.0, 0.8775825618903728) print "r4:\n", r4 #from a rotation to a Quaternion Quaternion = r4.GetEulerZYX() print "Quaternion:", Quaternion #create a RotAngle from a rotation [rot_angle, rot_vector] = r4.GetRotAngle() print "rot_angle:", rot_angle print "rot_vector:", rot_vector
def test_describe_near_objects2(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(6, 4, 0)), "/map")
self.assertEqual("We now enter the bedroom",
self.tour_guide.describe_near_objects())
def np_array2PyKDL_Rotation(np_array):
return PyKDL.Rotation(np_array[0,0], np_array[0,1], np_array[0,2],
np_array[1,0], np_array[1,1], np_array[1,2],
np_array[2,0], np_array[2,1], np_array[2,2])
def arrayToPyKDLRotation(array):
x = PyKDL.Vector(array[0][0], array[1][0], array[2][0])
y = PyKDL.Vector(array[0][1], array[1][1], array[2][1])
z = PyKDL.Vector(array[0][2], array[1][2], array[2][2])
return PyKDL.Rotation(x, y, z)
def limbPose(kdl_tree, base_link, limb_interface, limb='right'):
tip_link = limb + '_gripper'
tip_frame = PyKDL.Frame()
arm_chain = kdl_tree.getChain(base_link, tip_link)
# Baxter Interface Limb Instances
#limb_interface = baxter_interface.Limb(limb)
joint_names = limb_interface.joint_names()
num_jnts = len(joint_names)
if limb == 'right':
limb_link = [
'base', 'torso', 'right_arm_mount', 'right_upper_shoulder',
'right_lower_shoulder', 'right_upper_elbow', 'right_lower_elbow',
'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
'right_hand', 'right_gripper_base', 'right_gripper'
]
else:
limb_link = [
'base', 'torso', 'left_arm_mount', 'left_upper_shoulder',
'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow',
'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
'left_hand', 'left_gripper_base', 'left_gripper'
]
limb_frame = []
limb_chain = []
limb_pose = []
limb_fk = []
for idx in xrange(arm_chain.getNrOfSegments()):
linkname = limb_link[idx]
limb_frame.append(PyKDL.Frame())
limb_chain.append(kdl_tree.getChain(base_link, linkname))
limb_fk.append(
PyKDL.ChainFkSolverPos_recursive(
kdl_tree.getChain(base_link, linkname)))
# get the joint positions
cur_type_values = limb_interface.joint_angles()
while len(cur_type_values) != 7:
print "Joint angles error!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
cur_type_values = limb_interface.joint_angles()
kdl_array = PyKDL.JntArray(num_jnts)
for idx, name in enumerate(joint_names):
kdl_array[idx] = cur_type_values[name]
limb_joint = [
PyKDL.JntArray(1),
PyKDL.JntArray(2),
PyKDL.JntArray(3),
PyKDL.JntArray(4),
PyKDL.JntArray(5),
PyKDL.JntArray(6),
PyKDL.JntArray(7)
]
for i in range(7):
for j in range(i + 1):
limb_joint[i][j] = kdl_array[j]
for i in range(arm_chain.getNrOfSegments()):
joint_array = limb_joint[limb_chain[i].getNrOfJoints() - 1]
limb_fk[i].JntToCart(joint_array, limb_frame[i])
pos = limb_frame[i].p
rot = PyKDL.Rotation(limb_frame[i].M)
rot = rot.GetQuaternion()
limb_pose.append([pos[0], pos[1], pos[2]])
return np.asarray(limb_pose), kdl_array
def scalar_mul(mat, s):
out = PyKDL.Rotation()
for i in range(0, 3):
for j in range(0, 3):
out[i, j] = mat[i, j] * s
return out
if __name__ == '__main__':
psm2 = robot.robot('PSM2')
kdl_pose = psm2.get_current_position().p
print("Current Position:")
pprint.pprint(kdl_pose)
pprint.pprint(psm2.get_current_position().M)
""" An arbitrary z starting point that is not too close/far from the platform.
When the gripper picks up a needle, it moves up to this point and then releases the needle.
Change if it is too high/low above the platform. """
z_upper = -0.112688
""" POSE PSM2 WAS CALIBRATED IN """
pos = PyKDL.Vector(-0.118749, 0.0203151, -0.111688)
rot = PyKDL.Rotation(-0.940475, 0.313929, -0.130214, 0.330906, 0.933198,
-0.140157, 0.0775163, -0.174902, -0.98153)
pos2 = PyKDL.Vector(-0.0972128, -0.0170138, -0.106974)
sideways = PyKDL.Rotation(-0.453413, 0.428549, -0.781513, -0.17203,
0.818259, 0.548505, 0.874541, 0.383143,
-0.297286)
""" Move to arbitrary start position (near upper left corner) & release anything gripper is
holding. """
home(psm2, pos2, sideways)
""" Get PSM and endoscope calibration data (25 corresponding chess points) """
psm2_calibration_data = list(
transform.load_all('utils/psm2_recordings.txt'))
psm2_calibration_matrix = transform.fit_to_plane(
transform.psm_data_to_matrix(psm2_calibration_data))
endoscope_calibration_matrix = transform.fit_to_plane(
np.matrix(
def main():
rospy.init_node('controller', anonymous=True)
robot = psm('PSM1')
rate = rospy.Rate(100) # 10hz
robot.home()
position_start = robot.get_current_position()
safe_pos = PyKDL.Frame(
PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0),
PyKDL.Vector(0, 0, -1)),
PyKDL.Vector(0.02, -0.02, -0.09))
robot.move(safe_pos)
# just some stuff to aid prototyping and visualization
# has nothing to do with dVRK controls
fig = plt.figure()
ax = fig.gca(projection='3d')
coefs = (100, 200, 100
) # Coefficients in a0/c x**2 + a1/c y**2 + a2/c z**2 = 1
# Radii corresponding to the coefficients:
rx, ry, rz = 1 / np.sqrt(coefs)
# Set of all spherical angles:
u = np.linspace(0, 2 * np.pi, 100)
v = np.linspace(0, 2 * np.pi, 100)
#v = np.linspace(0, np.pi, 100)
# Cartesian coordinates that correspond to the spherical angles:
# (this is the equation of an ellipsoid):
# https://stackoverflow.com/questions/7819498/plotting-ellipsoid-with-matplotlib
x = rx * np.outer(np.cos(u), np.sin(v))
y = ry * np.outer(np.sin(u), np.sin(v))
z = rz * np.outer(np.ones_like(u), np.cos(v))
# print(np.amax(z))
z_max = np.amax(z)
# retain just the positive half of ellipsoid
z[z < 0] = 0
# ensure it sits on the xy plane
z = z - z_max
# Plot:
ax.plot_wireframe(x,
y,
z,
rstride=4,
cstride=4,
color='r',
edgecolor='None')
ax.scatter3D(x, y, z, c='r', s=5, zorder=10)
# Adjustment of the axes, so that they all have the same span:
max_radius = max(rx, ry, rz)
for axis in 'xyz':
getattr(ax, 'set_{}lim'.format(axis))((-max_radius, max_radius))
## random points on surface
xc = rx * np.outer(np.cos(u), np.sin(v))
yc = ry * np.outer(np.sin(u), np.sin(v))
zc = rz * np.outer(np.ones_like(u), np.cos(v))
zc[zc < 0] = 0
zc = zc - z_max
i = np.random.choice(5000, 100)
xr = np.ravel(xc)[i]
yr = np.ravel(yc)[i]
zr = np.ravel(zc)[i]
ax.scatter(xr, yr, zr, s=50, c='b', zorder=10)
plt.show()
# CODE TO SCAN
# dVRK controls begin here
# make 3D points
segmented_points = np.vstack(
(xr, yr, zr)).T # should be in robot's coordinates
# width of the liver = 8" = 20 cm
# length of the liver = 4" = 10 cm
startPos_x = np.amax(xr)
frontPos_y = np.amax(yr)
rearPos_y = np.amin(yr)
endPos_x = np.amin(xr)
# xr, yr, zr must be in METERS
# Adjustments in positions and heights will have to be made before running on dVRK
startPos = PyKDL.Frame(
PyKDL.Rotation(PyKDL.Vector(0, 1, 0), PyKDL.Vector(1, 0, 0),
PyKDL.Vector(0, 0, -1)),
PyKDL.Vector(startPos_x, rearPos_y, -0.08))
# These step sizes below are calculated based on the width and length of the liver
# will have to tune when run on the dVRK
step_val_x = 0.004
step_val_y = 0.001
step_val_z = 0.0001
dir = 1
z_dir = 1
x_iter = int((startPos_x - endPos_x) / step_val_x)
y_iter = int(
(frontPos_y - rearPos_y) / step_val_y
) # we can change from where it needs to start scanning in the y-coordinate
for i in range(x_iter): # [width/step_val_x]
for j in range(
y_iter - 75
): # [len/step_val_y] # will have to tune these values on dVRK
if j > (y_iter / 2):
z_dir = -1
robot.dmove(PyKDL.Vector(0, step_val_y * dir, step_val_z * z_dir))
robot.dmove(PyKDL.Vector(-step_val_x, 0, 0))
dir *= -1
z_dir = 1
while not rospy.is_shutdown():
print("Scan Complete")
rate.sleep()
def predict(self, img, box_3d_color, box_3d_color1):
ori_img = img
img = cv2.resize(img,(self.test_width,self.test_height))
trans = transforms.Compose([transforms.ToTensor(),])
data = trans(img)
data = data.resize_((1,data.shape[0],data.shape[1],data.shape[2]))
# Pass data to GPU
if self.use_cuda:
data = data.cuda()
# Wrap tensors in Variable class, set volatile=True for inference mode and to use minimal memory during inference
data = Variable(data, volatile=True)
# Forward pass
output = self.model(data).data
all_boxes = get_region_boxes(output, self.conf_thresh, self.num_classes)
for i in range(output.size(0)):
# For each image, get all the predictions
boxes = all_boxes[i]
best_conf_est = -1
# If the prediction has the highest confidence, choose it as our prediction for single object pose estimation
for j in range(len(boxes)):
if (boxes[j][18] > best_conf_est):
box_pr = boxes[j]
best_conf_est = boxes[j][18]
# Denormalize the corner predictions
corners2D_pr = np.array(np.reshape(box_pr[:18], [9, 2]), dtype='float32')
corners2D_pr[:, 0] = corners2D_pr[:, 0] * 640
corners2D_pr[:, 1] = corners2D_pr[:, 1] * 480
R_pr, t_pr = pnp(np.array(np.transpose(np.concatenate((np.zeros((3, 1)), self.corners3D[:3, :]), axis=1)), dtype='float32'), corners2D_pr, np.array(self.internal_calibration, dtype='float32'))
proj_2d_gt = corners2D_pr
R_tmp = []
for i in range(3):
for j in range(3):
R_tmp.append(R_pr[i][j])
rotation = PyKDL.Rotation(R_tmp[0],R_tmp[1],R_tmp[2],R_tmp[3],R_tmp[4],R_tmp[5],R_tmp[6],R_tmp[7],R_tmp[8])
self.rotation_rpy = rotation.GetRPY()
print(self.rotation_rpy)
x1 = int(proj_2d_gt[1][0])
y1 = int(proj_2d_gt[1][1])
x2 = int(proj_2d_gt[3][0])
y2 = int(proj_2d_gt[3][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[5][0])
y1 = int(proj_2d_gt[5][1])
x2 = int(proj_2d_gt[7][0])
y2 = int(proj_2d_gt[7][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[1][0])
y1 = int(proj_2d_gt[1][1])
x2 = int(proj_2d_gt[5][0])
y2 = int(proj_2d_gt[5][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[3][0])
y1 = int(proj_2d_gt[3][1])
x2 = int(proj_2d_gt[7][0])
y2 = int(proj_2d_gt[7][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[1][0])
y1 = int(proj_2d_gt[1][1])
x2 = int(proj_2d_gt[2][0])
y2 = int(proj_2d_gt[2][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[3][0])
y1 = int(proj_2d_gt[3][1])
x2 = int(proj_2d_gt[4][0])
y2 = int(proj_2d_gt[4][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[5][0])
y1 = int(proj_2d_gt[5][1])
x2 = int(proj_2d_gt[6][0])
y2 = int(proj_2d_gt[6][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[7][0])
y1 = int(proj_2d_gt[7][1])
x2 = int(proj_2d_gt[8][0])
y2 = int(proj_2d_gt[8][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[2][0])
y1 = int(proj_2d_gt[2][1])
x2 = int(proj_2d_gt[4][0])
y2 = int(proj_2d_gt[4][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[6][0])
y1 = int(proj_2d_gt[6][1])
x2 = int(proj_2d_gt[8][0])
y2 = int(proj_2d_gt[8][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[2][0])
y1 = int(proj_2d_gt[2][1])
x2 = int(proj_2d_gt[6][0])
y2 = int(proj_2d_gt[6][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[4][0])
y1 = int(proj_2d_gt[4][1])
x2 = int(proj_2d_gt[8][0])
y2 = int(proj_2d_gt[8][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color,2)
x1 = int(proj_2d_gt[2][0])
y1 = int(proj_2d_gt[2][1])
x2 = int(proj_2d_gt[8][0])
y2 = int(proj_2d_gt[8][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2)
x1 = int(proj_2d_gt[4][0])
y1 = int(proj_2d_gt[4][1])
x2 = int(proj_2d_gt[6][0])
y2 = int(proj_2d_gt[6][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2)
x1 = int(proj_2d_gt[3][0])
y1 = int(proj_2d_gt[3][1])
x2 = int(proj_2d_gt[8][0])
y2 = int(proj_2d_gt[8][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2)
x1 = int(proj_2d_gt[4][0])
y1 = int(proj_2d_gt[4][1])
x2 = int(proj_2d_gt[7][0])
y2 = int(proj_2d_gt[7][1])
cv2.line(ori_img,(x1,y1),(x2,y2),box_3d_color1,2)
return ori_img, R_pr, t_pr
output = np.array([x, y, z])
return output
sampling_period = 1 / 30
time_for_stretch = 2
target_displacement = 0.02
delta_displacement = -(target_displacement / time_for_stretch) * 1 / 30
translation = PyKDL.Vector(delta_displacement, 0.0, 0.0)
translation2 = PyKDL.Vector(-delta_displacement, 0.0, 0.0)
"""define the waypoint positions for the PSMs for this load test"""
cart1 = PyKDL.Vector(0.1399, -0.0674, -0.0864)
rot1 = PyKDL.Rotation()
rot1 = rot1.Quaternion(0.732, 0.013, 0.677, 0.0710)
pos1 = PyKDL.Frame(rot1, cart1)
cart2 = PyKDL.Vector(0.14425, -0.0676, -0.08934)
rot2 = PyKDL.Rotation()
rot2 = rot1.Quaternion(0.725, -0.0026, 0.6881, -0.0166)
pos2 = PyKDL.Frame(rot2, cart2)
cart3 = PyKDL.Vector(0.14025, -0.0676, -0.08934)
rot3 = PyKDL.Rotation()
rot3 = rot1.Quaternion(0.14425, -0.0676, -0.08934)
pos3 = PyKDL.Frame(rot3, cart3)
p2 = dvrk.psm('PSM2')
# move all joints - relative, incremental in joint space
p.dmove_joint(np.array([0.0, 0.0, -0.05, 0.0, 0.0, 0.0, 0.0]),
interpolate=True)
# move in cartesian space
# there are only 2 methods available, dmove and move
# both accept PyKDL Frame, Vector or Rotation
# Absolute translation in cartesian space.
p.move(
PyKDL.Vector(0.0, 0.0, -0.05),
interpolate=True) # move such that end effector becomes at (0, 0, -0.05)
# Incremental motion in cartesian space.
p.dmove(PyKDL.Vector(0.0, 0.05, 0.0),
interpolate=True) # move by 5 cm in Y direction
# save current orientation
old_orientation = p.get_desired_position().M
# save current XYZ position
old_position = p.get_desired_position().p
# rotate about an axis
r = PyKDL.Rotation()
r.DoRotX(math.pi * 0.25) # rotate about the X axis
p.dmove(r)
# p.move(old_orientation)
raw_input('Press enter to shutdown')
# Stop providing power to the arm
p.shutdown()
return
def get_pose(frame):
x = frame.p.x()
y = frame.p.y()
z = frame.p.z()
q1,q2,q3,q4 = frame.M.GetQuaternion()
output = (x,y,z,q1,q2,q3,q4)
return output
""" MTM home rough position """
''' We use this to initialize a position for the MTMR'''
MTMR_cart = PyKDL.Vector(0.055288515671, -0.0508310176185, -0.0659661913251)
MTMR_rot = PyKDL.Rotation()
MTMR_rot = MTMR_rot.Quaternion(0.750403138242, -0.0111643539824, 0.657383142871, -0.0679550644629)
MTMR_pos = PyKDL.Frame(MTMR_rot, MTMR_cart)
c = dvrk.console()
p2 = dvrk.psm('PSM2')
mtm = dvrk.mtm('MTMR')
print('initializing approximate MTMR position')
mtm.move(MTMR_pos)
c.teleop_start()
sub = rospy.Subscriber('/dvrk/footpedals/camera', Joy, trigger_callback)
trigger = False
trigger_time = 0
filename = './manipulator_homing/'
def fromMatrix(m):
return PyKDL.Frame(PyKDL.Rotation(m[0,0], m[0,1], m[0,2],
m[1,0], m[1,1], m[1,2],
m[2,0], m[2,1], m[2,2]),
PyKDL.Vector(m[0,3], m[1, 3], m[2, 3]))
