def compute_cam_transfrom_from_gesture(self):
if self._footpedals.cam_btn_pressed:
# Take the Norm length
init_vec = Vector(self._init_vec[0], self._init_vec[1], self._init_vec[2])
init_length = PyKDL.dot(init_vec, init_vec)
mtmr_cur_pose = self._mtmr.get_adjusted_pose()
mtml_cur_pose = self._mtml.get_adjusted_pose()
r_cur_pos = mtmr_cur_pose.p
l_cur_pos = mtml_cur_pose.p
cur_vec = l_cur_pos - r_cur_pos
cur_length = PyKDL.dot(cur_vec, cur_vec)
delta_len = cur_length - init_length
delta_rot = get_rot_mat_from_vecs(cur_vec, init_vec)
r = round(180.0/1.57079 * delta_rot.GetRPY()[0], 2)
p = round(180.0/1.57079 * delta_rot.GetRPY()[1], 2)
y = round(180.0/1.57079 * delta_rot.GetRPY()[2], 2)
# print 'ZOOM', round(delta_len, 3), 'RPY', r, p, y
else:
mtmr_init_pose = self._mtmr.get_adjusted_pose()
mtml_init_pose = self._mtml.get_adjusted_pose()
self._r_init_pos = mtmr_init_pose.p
self._l_init_pos = mtml_init_pose.p
self._init_vec = self._l_init_pos - self._r_init_pos
def updateCom(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights, m_id=0, pub_marker=None):
diff_B = PyKDL.diff(T_B_O, T_B_O_2)
up_v_B = PyKDL.Vector(0, 0, 1)
n_v_B = diff_B.rot * up_v_B
if pub_marker != None:
m_id = pub_marker.publishVectorMarker(
PyKDL.Vector(), diff_B.rot, m_id, r=0, g=1, b=0, frame="world", namespace="default", scale=0.01
)
m_id = pub_marker.publishVectorMarker(
PyKDL.Vector(), n_v_B, m_id, r=1, g=1, b=1, frame="world", namespace="default", scale=0.01
)
n_O = PyKDL.Frame(T_B_O.Inverse().M) * n_v_B
n_O_2 = PyKDL.Frame(T_B_O_2.Inverse().M) * n_v_B
# get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2
c_dot_list = []
c_dot_list_2 = []
for c in contacts_O:
dot = PyKDL.dot(c, n_O)
c_dot_list.append(dot)
dot_2 = PyKDL.dot(c, n_O_2)
c_dot_list_2.append(dot_2)
# get all com points that lies in the positive direction from given contact
for contact_idx in range(0, len(c_dot_list)):
for com_idx in range(0, len(com_pt)):
c = com_pt[com_idx]
dot = PyKDL.dot(c, n_O)
dot_2 = PyKDL.dot(c, n_O_2)
if dot > c_dot_list[contact_idx] and dot_2 > c_dot_list_2[contact_idx]:
com_weights[com_idx] += 1
return m_id
def rot_matrix_from_vecs(vec_a, vec_b):
out = PyKDL.Rotation()
vec_a.Normalize()
vec_b.Normalize()
vcross = vec_a * vec_b
vdot = PyKDL.dot(vec_a, vec_b)
# Check if the vectors are in the same direction
if 1.0 - vdot < 0.1:
return out
# Or in the opposite direction
elif 1.0 + vdot < 0.1:
nx = PyKDL.Vector(1, 0, 0)
temp_dot = PyKDL.dot(vec_a, nx)
if -0.9 < abs(temp_dot) < 0.9:
axis = vec_a * nx
out = out.Rot(axis, 3.14)
else:
ny = PyKDL.Vector(0, 1, 0)
axis = vec_a * ny
out = out.Rot(axis, 3.14)
else:
skew_v = skew_mat(vcross)
out = add_mat(add_mat(PyKDL.Rotation(), skew_v), scalar_mul(
skew_v * skew_v, (1 - vdot) / (vcross.Norm() ** 2)))
return out
def projectNullSpace(projDir, vector):
# this static method compute the projection of a vector
if projDir.Norm() < 0.00001:
projectedVector = vector
else:
# onto the null space of a direction
# Step ONE - find two unit vectors that represents the null space
# we use two candidates(initializers) vectors to find the null space
# checkout Gram-Schmidt on wikepedia
candidateVect1 = PyKDL.Vector(1.0, 1.0, 1.0)
candidateVect1.Normalize()
candidateVect2 = PyKDL.Vector(0.5, -1.0, 0.5)
candidateVect2.Normalize()
if abs(PyKDL.dot(candidateVect1, projDir)) < abs(
PyKDL.dot(candidateVect2, projDir)):
candidateVect = candidateVect1
else:
candidateVect = candidateVect2
axis1 = candidateVect * projDir
axis1.Normalize()
projDir.Normalize()
axis2 = projDir * axis1 # think of projDir as z axis
# Step TWO - find the prjection magnitude onto the two null space axes
projMagAxis1 = PyKDL.dot(axis1, vector)
projMagAxis2 = PyKDL.dot(axis2, vector)
# Step THREE - apply the magnitudes to the axes and superimpose them
projectedVector = projMagAxis1 * axis1 + projMagAxis2 * axis2
return projectedVector
def _project_goal_orientation_into_arm_subspace(self, goal):
y_t_hat = goal.M.UnitY() # y vector of the rotation matrix
z_t_hat = goal.M.UnitZ() # z vector of the rotation matrix
# m_hat is the normal of the "arm plane"
m_hat = kdl.Vector(0, -1, 0)
# k_hat is the vector about which rotation of the goal frame is performed
k_hat = m_hat * z_t_hat # cross product
# z_t_hat_tick is the new pointing direction of the arm
z_t_hat_tick = k_hat * m_hat # cross product
# the amount of rotation
cos_theta = kdl.dot(z_t_hat, z_t_hat_tick)
# first cross product then dot product
sin_theta = kdl.dot(z_t_hat * z_t_hat_tick, k_hat)
# use Rodrigues' rotation formula to perform the rotation
y_t_hat_tick = (cos_theta * y_t_hat) \
+ (sin_theta * (k_hat * y_t_hat)) \
+ (1 - cos_theta) * (kdl.dot(k_hat, y_t_hat)) * k_hat
# k_hat * y_t_hat is cross product
x_t_hat_tick = y_t_hat_tick * z_t_hat_tick # cross product
rot = kdl.Rotation(x_t_hat_tick, y_t_hat_tick, z_t_hat_tick)
# the frame uses the old position but has the new, projected orientation
return kdl.Frame(rot, goal.p)
def updateBowlcenFrames(self, bowl_frame):
bowl_cen_frame = copy.deepcopy(bowl_frame)
## Rotation
rot = bowl_cen_frame.M
## bowl_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
bowl_cen_y = rot.UnitY()
bowl_cen_z = PyKDL.Vector(px, py, 0.0)
bowl_cen_z.Normalize()
bowl_cen_x = bowl_cen_y * bowl_cen_z
bowl_cen_y = bowl_cen_z * bowl_cen_x
bowl_cen_rot = PyKDL.Rotation(bowl_cen_x, bowl_cen_y, bowl_cen_z)
bowl_cen_frame.M = bowl_cen_rot
## Position
bowl_cen_frame.p[2] -= self.bowl_z_offset
bowl_cen_frame.p += bowl_cen_z * self.bowl_forward_offset
if bowl_cen_y[2] > bowl_cen_x[2]:
# -90 x axis rotation
bowl_cen_frame = bowl_cen_frame * self.x_neg90_frame
else:
# 90 y axis rotation
bowl_cen_frame = bowl_cen_frame * self.y_90_frame
bowl_cen_frame_off = bowl_frame.Inverse() * bowl_cen_frame
if self.bowl_cen_frame_off == None:
self.bowl_cen_frame_off = bowl_cen_frame_off
else:
self.bowl_cen_frame_off.p = (self.bowl_cen_frame_off.p +
bowl_cen_frame_off.p) / 2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bowl_cen_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.bowl_cen_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.bowl_cen_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.bowl_cen_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = bowl_cen_frame_off.M.GetQuaternion()[0]
cur_quat.y = bowl_cen_frame_off.M.GetQuaternion()[1]
cur_quat.z = bowl_cen_frame_off.M.GetQuaternion()[2]
cur_quat.w = bowl_cen_frame_off.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bowl_cen_frame_off.M = PyKDL.Rotation.Quaternion(
quat.x, quat.y, quat.z, quat.w)
def updateMouthFrames(self, head_frame):
mouth_frame = copy.deepcopy(head_frame)
## Position
mouth_frame.p[2] -= self.head_z_offset
## Rotation
rot = mouth_frame.M
## head_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
mouth_y = rot.UnitY()
mouth_z = PyKDL.Vector(px, py, 0.0)
mouth_z.Normalize()
mouth_x = mouth_y * mouth_z
mouth_y = mouth_z * mouth_x
mouth_rot = PyKDL.Rotation(mouth_x, mouth_y, mouth_z)
mouth_frame.M = mouth_rot
mouth_frame_off = head_frame.Inverse()*mouth_frame
if self.mouth_frame_off == None:
self.mouth_frame_off = mouth_frame_off
else:
self.mouth_frame_off.p = (self.mouth_frame_off.p + mouth_frame_off.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.mouth_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.mouth_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.mouth_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.mouth_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = mouth_frame_off.M.GetQuaternion()[0]
cur_quat.y = mouth_frame_off.M.GetQuaternion()[1]
cur_quat.z = mouth_frame_off.M.GetQuaternion()[2]
cur_quat.w = mouth_frame_off.M.GetQuaternion()[3]
# check close quaternion and inverse
if np.dot(self.mouth_frame_off.M.GetQuaternion(), mouth_frame_off.M.GetQuaternion()) < 0.0:
cur_quat.x *= -1.
cur_quat.y *= -1.
cur_quat.z *= -1.
cur_quat.w *= -1.
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.mouth_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
def updateBowlcenFrames(self, bowl_frame):
bowl_cen_frame = copy.deepcopy(bowl_frame)
## Rotation
rot = bowl_cen_frame.M
## bowl_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
bowl_cen_y = rot.UnitY()
bowl_cen_z = PyKDL.Vector(px, py, 0.0)
bowl_cen_z.Normalize()
bowl_cen_x = bowl_cen_y * bowl_cen_z
bowl_cen_y = bowl_cen_z * bowl_cen_x
bowl_cen_rot = PyKDL.Rotation(bowl_cen_x, bowl_cen_y, bowl_cen_z)
bowl_cen_frame.M = bowl_cen_rot
## Position
bowl_cen_frame.p[2] -= self.bowl_z_offset
bowl_cen_frame.p += bowl_cen_z * self.bowl_forward_offset
if bowl_cen_y[2] > bowl_cen_x[2]:
# -90 x axis rotation
bowl_cen_frame = bowl_cen_frame * self.x_neg90_frame
else:
# 90 y axis rotation
bowl_cen_frame = bowl_cen_frame * self.y_90_frame
bowl_cen_frame_off = bowl_frame.Inverse()*bowl_cen_frame
if self.bowl_cen_frame_off == None:
self.bowl_cen_frame_off = bowl_cen_frame_off
else:
self.bowl_cen_frame_off.p = (self.bowl_cen_frame_off.p + bowl_cen_frame_off.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bowl_cen_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.bowl_cen_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.bowl_cen_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.bowl_cen_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = bowl_cen_frame_off.M.GetQuaternion()[0]
cur_quat.y = bowl_cen_frame_off.M.GetQuaternion()[1]
cur_quat.z = bowl_cen_frame_off.M.GetQuaternion()[2]
cur_quat.w = bowl_cen_frame_off.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bowl_cen_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
def getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed):
'''
this function assumes everything is represented in the quaternions in the /base_link frame
'''
if object_name.endswith('_scan'):
object_name = object_name[:-5]
dictObj = objDict()
objSpec = dictObj.getEntry(object_name)
F_bin_frame = posemath.fromTf(bin_frame)
F_obj_frame = posemath.fromTf(obj_frame)
objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0)
objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0)
objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0)
binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0)
binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0)
binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0)
rRProj = kdl.dot(objRed , binRed)
gRProj = kdl.dot(objGreen, binRed)
bRProj = kdl.dot(objBlue, binRed)
tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)]
if simtrackUsed:
for i in range(3):
if i in objSpec.invalidApproachAxis:
tmpApproach1[i] = 0
tmpApproach2 = [rRProj, gRProj, bRProj]
axisApproach = tmpApproach1.index(max(tmpApproach1))
objAxes = [objRed, objGreen, objBlue]
tmpGrasp1 = []
for i in range(3):
if simtrackUsed:
if i == axisApproach or i in objSpec.invalidGraspAxis:
tmpGrasp1.append(0)
continue
tmpGrasp1.append(kdl.dot(objAxes[i], binBlue))
tmpGrasp2 = [abs(t) for t in tmpGrasp1]
axisGrasp = tmpGrasp2.index(max(tmpGrasp2))
return axisApproach, tmpApproach2[axisApproach]/abs(tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp]/abs(tmpGrasp1[axisGrasp])
def getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed):
'''
this function assumes everything is represented in the quaternions in the /base_link frame
'''
if object_name.endswith('_scan'):
object_name = object_name[:-5]
dictObj = objDict()
objSpec = dictObj.getEntry(object_name)
F_bin_frame = posemath.fromTf(bin_frame)
F_obj_frame = posemath.fromTf(obj_frame)
objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0)
objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0)
objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0)
binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0)
binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0)
binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0)
rRProj = kdl.dot(objRed, binRed)
gRProj = kdl.dot(objGreen, binRed)
bRProj = kdl.dot(objBlue, binRed)
tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)]
if simtrackUsed:
for i in range(3):
if i in objSpec.invalidApproachAxis:
tmpApproach1[i] = 0
tmpApproach2 = [rRProj, gRProj, bRProj]
axisApproach = tmpApproach1.index(max(tmpApproach1))
objAxes = [objRed, objGreen, objBlue]
tmpGrasp1 = []
for i in range(3):
if simtrackUsed:
if i == axisApproach or i in objSpec.invalidGraspAxis:
tmpGrasp1.append(0)
continue
tmpGrasp1.append(kdl.dot(objAxes[i], binBlue))
tmpGrasp2 = [abs(t) for t in tmpGrasp1]
axisGrasp = tmpGrasp2.index(max(tmpGrasp2))
return axisApproach, tmpApproach2[axisApproach] / abs(
tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp] / abs(
tmpGrasp1[axisGrasp])
def move(self, desiredPose, maxForce):
currentPose = self.robot.get_desired_position()
currentPose.p = currentPose.p
forceArray = np.empty((0, 4), float)
displacements = np.array([0], float)
# Remove z rotation
angle = np.arccos(
PyKDL.dot(desiredPose.M.UnitX(), currentPose.M.UnitX()))
rot = PyKDL.Rotation.Rot(desiredPose.M.UnitZ(), angle)
# Added offset representing tooltip
desiredPosition = desiredPose.p - desiredPose.M.UnitZ(
) * self.toolOffset
desiredPoseWithOffset = PyKDL.Frame(desiredPose.M, desiredPosition)
measuredPose_previous = self.robot.get_current_position()
startForce = self.force[1]
while not rospy.is_shutdown():
# get current and desired robot pose (desired is the top of queue)
# compute the desired twist "x_dot" from motion command
xDotMotion = resolvedRates(
self.resolvedRatesConfig, currentPose,
desiredPoseWithOffset) # xDotMotion is type [PyKDL.Twist]
currentPose = PyKDL.addDelta(currentPose, xDotMotion,
self.resolvedRatesConfig['dt'])
#Save displacement and force in different arrays
if type(self.force) == type(None):
rospy.logwarn(
"Probe Service: No forces detected. Not moving and returning None"
)
return None, None
data = np.array([self.force])
if xDotMotion.vel.Norm() <= 0.001 and xDotMotion.rot.Norm() <= 0.1:
break
if self.force[1] - startForce > maxForce:
rospy.logwarn("Max Force Exceeded: " + str(self.force))
break
self.robot.move(currentPose, interpolate=False)
self.rate.sleep()
measuredPose_current = self.robot.get_current_position()
currentDisplacement = measuredPose_current.p - measuredPose_previous.p
# currentDisplacement = xDotMotion.vel.Norm() * self.resolvedRatesConfig['dt']
currentDisplacement = PyKDL.dot(currentDisplacement,
desiredPose.M.UnitZ())
# currentDisplacement = displacements[len(displacements)-1] + currentDisplacement
forceArray = np.append(forceArray, data, axis=0)
displacements = np.append(displacements, [currentDisplacement])
return displacements.tolist(), forceArray.tolist()
def check_if_new_goal(self, frame):
dist_p = kdl.diff(self.dst_pose.p, frame.p)
dist_M = kdl.diff(self.dst_pose.M, frame.M)
if dist_M[0] != dist_M[0]: #got a nan
rospy.logwarn("Got a NAN in frame.M distance")
return True
dist_p_scalar = kdl.dot(dist_p, dist_p)
dist_M_scalar = kdl.dot(dist_M, dist_M)
if (dist_p_scalar > 0.00001) or (dist_M_scalar > 0.00001):
return True
else:
return False
def distanceLinePoint(self, line, pt):
a, b = line
v = b - a
ta = PyKDL.dot(v, a)
tb = PyKDL.dot(v, b)
tpt = PyKDL.dot(v, pt)
if tpt <= ta:
return (a-pt).Norm(), a, pt
elif tpt >= tb:
return (b-pt).Norm(), b, pt
else:
n = PyKDL.Vector(v.y(), -v.x(), v.z())
n.Normalize()
diff = PyKDL.dot(n, a) - PyKDL.dot(n, pt)
return abs(diff), pt + (diff * n), pt
def sampleOptimalZAngle(moveItInterface, R_bin_sample, R_eef_gripper_base,
desiredZAxis, numAngles):
'''
Find optimal angle for rotating a pose sample around Z-axis so that the gripper base's
z axis aligns as much as possible with the desired z axis w.r.t the bin.
@param R_bin_sample: rotation of the sample w.r.t. bin frame (kdl.Rotation)
@param R_eef_gripper_base: rotation of the eef (tool-tip) w.r.t. gripper base frame (kdl.Rotation)
@return: optimal angle (float)
'''
z_bin_gripper_base_desired = kdl.Vector(*(desiredZAxis))
# evaluate for a series of rotations around z axis
thetas_z = numpy.linspace(0, 2 * numpy.pi, numAngles)
dot_products = numpy.array([])
for angle in thetas_z:
moveItInterface.checkPreemption()
R_bin_sample_rotated = R_bin_sample * kdl.Rotation.RotZ(angle)
# resulting Z-axis of gripper_base
z_bin_gripper_base = R_bin_sample_rotated * R_eef_gripper_base * kdl.Vector(
0.0, 0.0, 1.0)
dot_products = numpy.append(
dot_products,
kdl.dot(z_bin_gripper_base, z_bin_gripper_base_desired))
optimal_angle = thetas_z[dot_products.argmax()]
return optimal_angle
def get_frame_normal(door):
"""Get the normal of the door frame.
@type door: door_msgs.msg.Door
@rtype: kdl.Vector
"""
# normal on frame
p12 = kdl.Vector(
door.frame_p1.x - door.frame_p2.x,
door.frame_p1.y - door.frame_p2.y,
door.frame_p1.z - door.frame_p2.z)
p12.Normalize()
z_axis = kdl.Vector(0,0,1)
normal = p12 * z_axis
# make normal point in direction we travel through door
direction = kdl.Vector(
door.travel_dir.x,
door.travel_dir.y,
door.travel_dir.z)
if kdl.dot(direction, normal) < 0:
normal = normal * -1.0
return normal
def calc_R(xa, ya):
ret = []
""" calculate the minimum distance of each contact point from jar surface pt """
n = PyKDL.Frame(PyKDL.Rotation.RotX(xa)) * PyKDL.Frame(PyKDL.Rotation.RotY(ya)) * PyKDL.Vector(0, 0, 1)
for p in points:
ret.append(PyKDL.dot(n, p))
return numpy.array(ret)
def sampleOptimalZAngle(moveItInterface, R_bin_sample, R_eef_gripper_base, desiredZAxis, numAngles):
'''
Find optimal angle for rotating a pose sample around Z-axis so that the gripper base's
z axis aligns as much as possible with the desired z axis w.r.t the bin.
@param R_bin_sample: rotation of the sample w.r.t. bin frame (kdl.Rotation)
@param R_eef_gripper_base: rotation of the eef (tool-tip) w.r.t. gripper base frame (kdl.Rotation)
@return: optimal angle (float)
'''
z_bin_gripper_base_desired = kdl.Vector(*(desiredZAxis))
# evaluate for a series of rotations around z axis
thetas_z = numpy.linspace(0, 2 * numpy.pi, numAngles)
dot_products = numpy.array([])
for angle in thetas_z:
moveItInterface.checkPreemption()
R_bin_sample_rotated = R_bin_sample * kdl.Rotation.RotZ(angle)
# resulting Z-axis of gripper_base
z_bin_gripper_base = R_bin_sample_rotated * R_eef_gripper_base * kdl.Vector(0.0, 0.0, 1.0)
dot_products = numpy.append(dot_products, kdl.dot(z_bin_gripper_base, z_bin_gripper_base_desired))
optimal_angle = thetas_z[dot_products.argmax()]
return optimal_angle
def compute(self):
vec1 = self.vec1.compute()
vec2 = self.vec2.compute()
denom = vec1.Norm() * vec2.Norm()
if denom == 0:
return 0
else:
return kdl.dot(vec1, vec2) / denom
def instaneous_cost(self, x, u, t, aux): if t < self.T - 1: return u.dot(u)*self.weight_u else: temp_dest = PyKDL.Frame() self.fksolver.JntToCart(self.NpToJnt(x+u), temp_dest) x_position = temp_dest.p return PyKDL.dot(self.fin_position-x_position, self.fin_position-x_position)*self.weight_x + u.dot(u)*self.weight_u
def getAngle(v1, v2):
"""!
Calculate angle between two vectors.
@param v1 PyKDL.Vector: first vector
@param v2 PyKDL.Vector: second vector
@return float: angle between two vectors
"""
return math.atan2((v1*v2).Norm(), PyKDL.dot(v1,v2))
def get_angle(v1, v2):
dot = PyKDL.dot(v1, v2)
if 1.0 - abs(dot) < 0.001:
return 0
elif dot < 1:
v2 = -v2
angle = math.acos(dot / (v1.Norm() * v2.Norm()))
return angle
def compute(self):
vec = self.vec.compute()
ref = self.ref.compute()
denom = ref.dir.Norm()**2
if denom == 0:
res = kdl.Vector()
else:
res = ref.dir * (kdl.dot(ref.dir, vec.dir) / denom)
return LocatedVector(vec.pos, res)
def axis_marker(tw, id = 0, ns = 'twist'):
""" make a marker message showing the instantaneous
rotation axis of a twist message"""
t = kdl.Twist(kdl.Vector(tw.linear.x, tw.linear.y, tw.linear.z),
kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z))
try:
(x, rot) = listener.lookupTransform(target_frame, ref_frame, rospy.Time(0))
(trans, rotp) = listener.lookupTransform(target_frame, ref_point, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
print 'tf exception!'
return marker.create(id=id, ns=ns, action=Marker.DELETE)
# put the twist in the right frame
f = posemath.fromTf( (trans, rot) )
t = f*t
direction = t.rot
location = t.rot * t.vel / kdl.dot(t.rot, t.rot)
lr = t.rot.Norm()
lt = t.vel.Norm()
m = marker.create(id=id, ns=ns, type=Marker.CYLINDER)
if lr < 0.0001 and lt < 0.0001:
return marker.create(id=id, ns=ns, action=Marker.DELETE)
if lr < 0.001:
# very short axis, assume linear movement
location = kdl.Vector(0,0,0)
direction = t.vel
if lt < min_length:
direction *= min_length / lt
m.type = Marker.CUBE
m = marker.align(m, location, location + direction, 0.02)
elif lr < min_length:
direction = direction / lr * min_length
m = marker.align(m, location - direction, location + direction, 0.02)
elif lr > max_length:
direction = direction / lr * max_length
m = marker.align(m, location - direction, location + direction, 0.02)
else:
#BAH! How do I make this better?
m = marker.align(m, location - direction, location + direction, 0.02)
m.header.frame_id = target_frame
m.frame_locked = True
if(use_colors):
m.color = colors[id % len(colors)]
else:
m.color = ColorRGBA(0.3, 0.3, 0.3, 1)
return m
def updateCom2(T_B_O, T_B_O_2, contacts_O, com_pt, com_weights):
diff = PyKDL.diff(T_B_O, T_B_O_2)
up_v = PyKDL.Vector(0, 0, 1)
n_v = diff.rot * up_v
n_O = T_B_O.Inverse() * n_v
n_O_2 = T_B_O_2.Inverse() * n_v
# get all com points that lies in the positive direction from the most negative contact point with respect to n_v in T_B_O and T_B_O_2
# get the minimum contact point for n_O
min_c_dot = None
for c in contacts_O:
dot = PyKDL.dot(c, n_O)
if min_c_dot == None or dot < min_c_dot:
min_c_dot = dot
# get all com points that lies in the positive direction from min_c_dot
com_2_idx = []
for c_idx in range(0, len(com_pt)):
c = com_pt[c_idx]
dot = PyKDL.dot(c, n_O)
if dot > min_c_dot:
com_2_idx.append(c_idx)
# get the minimum contact point for n_O
min_c_dot = None
for c in contacts_O:
dot = PyKDL.dot(c, n_O_2)
if min_c_dot == None or dot < min_c_dot:
min_c_dot = dot
# get all com points that lies in the positive direction from min_c_dot
com_3_idx = []
for c_idx in com_2_idx:
c = com_pt[c_idx]
dot = PyKDL.dot(c, n_O_2)
if dot > min_c_dot:
com_3_idx.append(c_idx)
for c_idx in range(0, len(com_pt)):
com_weights[c_idx] -= 1
for c_idx in com_3_idx:
com_weights[c_idx] += 2
def intersect(self, ray_dir_frame, **kwargs):
if 'normal' in kwargs and kwargs['normal']:
normal = kwargs['normal']
else:
normal = self._normal
if 'point' in kwargs and kwargs['point']:
point = kwargs['point']
else:
point = self._point
if not (normal and point):
return None
u = ray_dir_frame.p - ray_dir_frame * kdl.Vector(1.0, 0.0, 0.0)
# normal.Normalize()
n = normal
w = ray_dir_frame.p - point # - n * self._distance
D = kdl.dot(n, u)
N = -kdl.dot(n, w)
# Check if the vector is parallel to the plane
if math.fabs(D) > sys.float_info.epsilon:
sI = N / D
# Is the intersection on the back?
if sI >= 0.0:
return None
# Point on the plane
p = ray_dir_frame.p + sI * u
yaw = math.atan2(p.y(), p.x())
# Turn along the pointed direction in horizontal plane
q = kdl.Rotation.RPY(0.0, 0.0, yaw)
return kdl.Frame(q, p)
return None
def instaneous_cost(self, x, u, t, aux):
if t < self.T - 1:
#return u.dot(u)*self.weight_u
temp_dest = PyKDL.Frame()
self.fksolver.JntToCart(self.NpToJnt(self.plant_dyn(x, u)),
temp_dest)
x_position = temp_dest.p
return PyKDL.dot(
self.fin_position - x_position, self.fin_position -
x_position) * self.weight_x + u.dot(u) * self.weight_u
else:
temp_dest = PyKDL.Frame()
#self.fksolver.JntToCart(self.NpToJnt(x+u+rd.gauss(0,1e-8)*np.ones(u.shape)), temp_dest)
self.fksolver.JntToCart(self.NpToJnt(self.plant_dyn(x, u)),
temp_dest)
x_position = temp_dest.p
return PyKDL.dot(
self.fin_position - x_position, self.fin_position -
x_position) * self.weight_x + u.dot(u) * self.weight_u
def projectDirection(projDir, vector):
# this static method compute the projection of a vector onto a direction
# both variables need to be in format of PyKDL.Vector
# compute the projection magnitude of vector onto the direction
if projDir.Norm() < 0.000001:
projectedVector = PyKDL.Vector(0, 0, 0)
else:
projectMag = PyKDL.dot(vector, projDir)
# apply the magnitude on the direction
projectedVector = projDir * projectMag
return projectedVector
def get_closest_waypoint(self, pose):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
#TODO implement
if waypoints==None or pose==None:
rospy.logerr("No waypoint list or pose specified in get_closest_waypoint")
return -1
#implement search, nearest
min_dist = float("inf")
min_idx = None
search_range = 300
for i, wp in enumerate(waypoints):
dist = self.get_distance_between_poses( wp.pose.pose, pose )
if (dist < min_dist) and (dist<search_range):
if (mode == None ):
min_dist = dist
min_idx = i
elif( mode == "forward" ):
po = pose.orientation #pose orientation
wpo = wp.pose.pose.orientation #waypoint orientation
wpp = wp.pose.pose.position
car_vector = PyKDL.Rotation.Quaternion(po.x,po.y,po.z,po.w) * PyKDL.Vector(1,0,0) # change the reference frame of 1,0,0 to the orientation of the car
wp_vector = PyKDL.Vector( wpp.x-pose.position.x, wpp.y-pose.position.y, 0 )
#dot product is the cosinus of angle between both
angle = np.arccos( PyKDL.dot( car_vector, wp_vector ) / car_vector.Norm() / wp_vector.Norm() )
if angle < np.pi/2:
min_dist = dist
min_idx = i
# we could use the raw for this math?
#
# xyz_position = pose.position
# quaternion_orientation = pose.orientation
# p = xyz_position
# qo = quaternion_orientation
# p_list = [p.x, p.y, p.z]
# qo_list = [qo.x, qo.y, qo.z, qo.w]
# euler = euler_from_quaternion(qo_list)
# yaw_rad = euler[2]
return min_idx
def frame_from_direction(self, orig_v, dir_v):
u = kdl.Vector(dir_v.x(), dir_v.y(), dir_v.z())
u.Normalize()
yaw = kdl.Rotation.RPY(0.0, 0.0, math.atan2(u.y(), u.x()))
# Convention: pointing ray is along Z-axis
new_x = yaw * kdl.Vector(1.0, 0.0, 0.0)
pitch = kdl.Rotation.RPY(0.0, math.atan2(-u.z(), kdl.dot(u, new_x)), 0.0)
frame = kdl.Frame(yaw * pitch, orig_v)
return frame
def mouthPoseCallback(self, data):
p = PyKDL.Vector(data.pose.position.x, data.pose.position.y,
data.pose.position.z)
M = PyKDL.Rotation.Quaternion(data.pose.orientation.x,
data.pose.orientation.y,
data.pose.orientation.z,
data.pose.orientation.w)
# get upright mouth frame
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, M.UnitZ())
py = PyKDL.dot(ty, M.UnitZ())
mouth_z = PyKDL.Vector(px, py, 0.0)
mouth_z.Normalize()
mouth_x = PyKDL.Vector(0.0, 0.0, 1.0)
mouth_y = mouth_z * mouth_x
M = PyKDL.Rotation(mouth_x, mouth_y, mouth_z)
self.mouth_frame_kinect = PyKDL.Frame(M, p)
# 4. (optional) publish pose for visualization
ps = PoseStamped()
ps.header.frame_id = 'torso_lift_link'
ps.header.stamp = rospy.Time.now()
ps.pose.position.x = p[0]
ps.pose.position.y = p[1]
ps.pose.position.z = p[2]
ps.pose.orientation.x = M.GetQuaternion()[0]
ps.pose.orientation.y = M.GetQuaternion()[1]
ps.pose.orientation.z = M.GetQuaternion()[2]
ps.pose.orientation.w = M.GetQuaternion()[3]
self.mouth_pub.publish(ps)
def coneLimit(zcur, zref, coneAngle):
zcur.Normalize()
zref.Normalize()
dotResult = PyKDL.dot(zcur, zref)
offset_angle = coneAngle / 2
if dotResult < np.cos(offset_angle):
axis = zref * zcur
axis.Normalize()
# ipdb.set_trace()
R = PyKDL.Rotation().Rot(axis, offset_angle)
zcur = R * zref
# ipdb.set_trace()
return zcur
def move(self, desiredPose, maxForce):
currentPose = self.robot.get_desired_position()
currentPose.p = currentPose.p
displacements = np.array([0], float)
# Remove z rotation
angle = np.arccos(
PyKDL.dot(desiredPose.M.UnitX(), currentPose.M.UnitX()))
rot = PyKDL.Rotation.Rot(desiredPose.M.UnitZ(), angle)
# Added offset representing tooltip
desiredPosition = desiredPose.p - desiredPose.M.UnitZ(
) * self.toolOffset
desiredPoseWithOffset = PyKDL.Frame(desiredPose.M, desiredPosition)
measuredPose_previous = self.robot.get_current_position()
startForce = self.force[1]
while not rospy.is_shutdown():
# get current and desired robot pose (desired is the top of queue)
# compute the desired twist "x_dot" from motion command
xDotMotion = resolvedRates(
self.resolvedRatesConfig, currentPose,
desiredPoseWithOffset) # xDotMotion is type [PyKDL.Twist]
currentPose = PyKDL.addDelta(currentPose, xDotMotion,
self.resolvedRatesConfig['dt'])
if xDotMotion.vel.Norm() <= 0.001 and xDotMotion.rot.Norm() <= 0.1:
break
self.robot.move(currentPose, interpolate=False)
self.rate.sleep()
measuredPose_current = self.robot.get_current_position()
currentDisplacement = measuredPose_current.p - measuredPose_previous.p
currentDisplacement = PyKDL.dot(currentDisplacement,
desiredPose.M.UnitZ())
displacements = np.append(displacements, [currentDisplacement])
return displacements.tolist()
def check_ik_result_using_fk(fk_solver, result_joint_state_kdl,
goal_frame_kdl):
fk_frame_kdl = PyKDL.Frame()
fk_solver.JntToCart(result_joint_state_kdl, fk_frame_kdl)
distance_vec = PyKDL.diff(fk_frame_kdl.p, goal_frame_kdl.p)
norm = math.sqrt(PyKDL.dot(distance_vec, distance_vec))
print "distance norm: " + str(norm)
relative_rotation = fk_frame_kdl.M.Inverse() * goal_frame_kdl.M
distance_angle, _ = relative_rotation.GetRotAngle()
print "distance angle: " + str(distance_angle)
goal_pose_reached = (norm < 0.005) and (abs(distance_angle) < 0.1)
return goal_pose_reached
def markers(self):
''' get the markers for visualization '''
self._compute_lengths()
frame = self._transform()
mrks = []
pos = kdl.Vector(0,0,0)
l_index = 0
for i,t in enumerate(self.jac):
twist = frame*t
lr = twist.rot.Norm()
lv = twist.vel.Norm()
if lr < self.eps and lv < self.eps:
# null twist, make a delete marker
mrks.append(marker.create(id=i, ns=self.name+str(i), action=Marker.DELETE))
continue
if lr < self.eps:
# pure translational twist
location = pos
direction = twist.vel / lv * self.lengths[l_index]
m = marker.create(id=i, ns=self.name+str(i), type=Marker.CUBE)
m = marker.align(m, location, location + direction, self.width)
l_index += 1
frame.p = frame.p - direction # move target point to end of this 'axis'
pos += direction # remember current end point
else:
# rotational twist
location = twist.rot * twist.vel / kdl.dot(twist.rot, twist.rot) + pos
if self.independent_directions:
direction = twist.rot * self.rot_length / lr
else:
# take axis from interaction matrix and transform to location
dir_H = self.H[i].rot - location*self.H[i].vel
direction = dir_H * self.rot_length / dir_H.Norm()
m = marker.create(id=i, ns=self.name+str(i), type=Marker.CYLINDER)
m = marker.align(m, location - direction, location + direction, self.width)
m.color = self._color(i)
m.header.frame_id = self.target_frame
mrks.append(m)
return mrks
def get_closest_waypoint(self, pose, waypoints, mode=None ):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
waypoints (list): the reference list of waypoints to search on
mode: "nearest" -> returns nearest waypoint regardless of direction.
"forward" -> returns nearest waypoint in forward direction
Returns:
int: index of the closest waypoint in self.waypoints
"""
#TODO implement
if waypoints==None or pose==None:
#rospy.logerr("No waypoint list or pose specified in get_closest_waypoint")
return -1
#Find nearest
max_range = 275
min_dist = float("inf")
min_idx = None
for i, wp in enumerate(waypoints):
dist = math.sqrt((wp.pose.pose.position.x - pose.position.x)**2 + (wp.pose.pose.position.y - pose.position.y)**2)
if (dist < min_dist) and (dist < max_range):
if (mode == None ):
min_dist = dist
min_idx = i
elif( mode == "forward" ):
po = pose.orientation
wpo = wp.pose.pose.orientation
wpp = wp.pose.pose.position
car_vector = PyKDL.Rotation.Quaternion(po.x,po.y,po.z,po.w) * PyKDL.Vector(1,0,0)
wp_vector = PyKDL.Vector( wpp.x-pose.position.x, wpp.y-pose.position.y, 0 )
angle = np.arccos( PyKDL.dot( car_vector, wp_vector ) / car_vector.Norm() / wp_vector.Norm() )
if angle < np.pi/2:
min_dist = dist
min_idx = i
return min_idx
def sector(vector1, vector2, base, ns='', id=1):
""" Create a sector between 'vector1', 'vector2' and 'base'.
This marker is a triangle list. The interpolation used
is SLERP and the vectors may be of different lengths.
"""
marker = create(ns=ns, id=id, type=Marker.TRIANGLE_LIST)
l_v1 = vector1.Norm()
l_v2 = vector2.Norm()
if l_v1 == 0 or l_v2 == 0:
marker.action = Marker.DELETE
return marker
l_arc = acos(kdl.dot(vector1, vector2) / (l_v1 * l_v2))
if l_arc == 0:
marker.action = Marker.DELETE
return marker
n_steps = int(l_arc / 0.1)
v_last = vector1 + base
for i in range(1,n_steps+1):
t = float(i) / n_steps
# perform SLERP
v = (1-t)*vector1 + t*vector2 # interpolate vectors
l = (1-t)*l_v1 + t*l_v2 # interpolate lengths
v = v * l / v.Norm() # set vector length
v = v + base # add origin
marker.points.append(Point(v_last.x(), v_last.y(), v_last.z()))
marker.points.append(Point(base.x(), base.y(), base.z()))
marker.points.append(Point(v.x(), v.y(), v.z()))
v_last = v
if marker.points == []:
marker.action = 2
return marker
def get_door_dir(door):
# get frame vector
if door.hinge == Door.HINGE_P1:
frame_vec = point2kdl(door.frame_p2) - point2kdl(door.frame_p1)
elif door.hinge == Door.HINGE_P2:
frame_vec = point2kdl(door.frame_p1) - point2kdl(door.frame_p2)
else:
rospy.logerr("Hinge side is not defined")
# rotate frame vector
if door.rot_dir == Door.ROT_DIR_CLOCKWISE:
frame_vec = kdl.Rotation.RotZ(-math.pi / 2) * frame_vec
elif door.rot_dir == Door.ROT_DIR_COUNTERCLOCKWISE:
frame_vec = kdl.Rotation.RotZ(math.pi / 2) * frame_vec
else:
rospy.logerr("Rot dir is not defined")
if kdl.dot(frame_vec, get_frame_normal(door)) < 0:
return -1.0
else:
return 1.0
def get_door_dir(door):
# get frame vector
if door.hinge == Door.HINGE_P1:
frame_vec = point2kdl(door.frame_p2) - point2kdl(door.frame_p1)
elif door.hinge == Door.HINGE_P2:
frame_vec = point2kdl(door.frame_p1) - point2kdl(door.frame_p2)
else:
rospy.logerr("Hinge side is not defined")
# rotate frame vector
if door.rot_dir == Door.ROT_DIR_CLOCKWISE:
frame_vec = kdl.Rotation.RotZ(-math.pi/2) * frame_vec
elif door.rot_dir == Door.ROT_DIR_COUNTERCLOCKWISE:
frame_vec = kdl.Rotation.RotZ(math.pi/2) * frame_vec
else:
rospy.logerr("Rot dir is not defined")
if kdl.dot(frame_vec, get_frame_normal(door)) < 0:
return -1.0
else:
return 1.0
def __init__(self, duration, current_pose, center_pose, loop=False):
"""
Creates a circular trajectory; orientation is kept from the current_pose, and the position
navigates a circular path with center on center_pose and radius given by the distance between the two frames
:param duration: the trajectory duration
:param current_pose: a kdl.Frame representing the actual position on the circle
:param center_pose: a kdl.Frame representing the desired center of the circle.
The circle will be drawn about the z-axis of the center Frame
:return:
"""
self.duration = float(duration)
self.w_T_ee = current_pose
self.radius = (center_pose.p - current_pose.p).Norm()
self.w_T_c = center_pose
assert(self.w_T_c.p != self.w_T_ee.p)
assert( np.abs(kdl.dot((center_pose.p - current_pose.p), self.w_T_c.M.UnitZ())) < 1.0)
print "Creating trajectory generator for circle with center:"
print center_pose
print "Trajectory will take ", duration, "s"
def get_frame_normal(door):
"""Get the normal of the door frame.
@type door: door_msgs.msg.Door
@rtype: kdl.Vector
"""
# normal on frame
p12 = kdl.Vector(door.frame_p1.x - door.frame_p2.x,
door.frame_p1.y - door.frame_p2.y,
door.frame_p1.z - door.frame_p2.z)
p12.Normalize()
z_axis = kdl.Vector(0, 0, 1)
normal = p12 * z_axis
# make normal point in direction we travel through door
direction = kdl.Vector(door.travel_dir.x, door.travel_dir.y,
door.travel_dir.z)
if kdl.dot(direction, normal) < 0:
normal = normal * -1.0
return normal
def pointInMesh(vertices, faces, point):
pos = 0
for f in faces:
A = vertices[f[0]]
B = vertices[f[1]]
C = vertices[f[2]]
a = PyKDL.Vector(A[0], A[1], A[2])
b = PyKDL.Vector(B[0], B[1], B[2])
c = PyKDL.Vector(C[0], C[1], C[2])
n = (b - a) * (c - a)
n.Normalize()
if n.z() == 0.0:
continue
if pointInTriangle([a.x(), a.y()], [b.x(), b.y()], [c.x(), c.y()], [point.x(), point.y()]):
d = -PyKDL.dot(a, n)
z = -(n.x() * point.x() + n.y() * point.y() + d) / n.z()
if z > point.z():
pos += 1
if pos % 2 == 0:
return False
return True
def _get_viewing_angle(self, origin_frame, target_frame): origin_normal = origin_frame.M * PyKDL.Vector(1, 0, 0) target_normal = target_frame.M * PyKDL.Vector(1, 0, 0) return angles.normalize_angle(math.acos(PyKDL.dot(target_normal, origin_normal)) - math.pi)
def getAngle(v1, v2):
return math.atan2((v1*v2).Norm(), PyKDL.dot(v1,v2))
def teleop_joints(self):
# Fixed Joints: Set position to 0 radians.
self.cmd_joints.position[self.cmd_joints.name.index('left_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_roll_joint')] = 2
self.cmd_joints.position[self.cmd_joints.name.index('right_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_roll_joint')] = 2
self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = 2
self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = 2
# Torso Rotation
try:
torso_quaternion = self.skeleton['orientation']['torso']
torso_rpy = torso_quaternion.GetRPY()
self.cmd_joints.position[self.cmd_joints.name.index(self.skel_to_joint_map['torso'])] = torso_rpy[1] / 2.0
self.cmd_joints.velocity[self.cmd_joints.name.index('torso_joint')] = self.default_joint_speed
except:
pass
# Head Pan
try:
head_quaternion = self.skeleton['orientation']['head']
head_rpy = head_quaternion.GetRPY()
self.cmd_joints.position[self.cmd_joints.name.index('head_pan_joint')] = head_rpy[1]
self.cmd_joints.velocity[self.cmd_joints.name.index('head_pan_joint')] = self.default_joint_speed
except:
pass
# Head Tilt
try:
self.cmd_joints.position[self.cmd_joints.name.index('head_tilt_joint')] = head_rpy[0]
self.cmd_joints.velocity[self.cmd_joints.name.index('head_tilt_joint')] = self.default_joint_speed
except:
pass
# Left Arm
try:
left_shoulder_neck = self.skeleton['position']['neck'] - self.skeleton['position']['left_shoulder']
left_shoulder_elbow = self.skeleton['position']['left_elbow'] - self.skeleton['position']['left_shoulder']
left_elbow_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_elbow']
left_shoulder_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_shoulder']
left_shoulder_neck.Normalize()
left_shoulder_elbow.Normalize()
lh = left_elbow_hand.Normalize()
left_shoulder_hand.Normalize()
left_shoulder_lift_angle = asin(left_shoulder_elbow.y()) + self.HALF_PI
left_shoulder_pan_angle = asin(left_elbow_hand.x())
left_elbow_angle = -acos(KDL.dot(left_shoulder_elbow, left_elbow_hand))
left_wrist_angle = -left_elbow_angle / 2.0
left_elbow_angle = left_elbow_angle / 4.0
self.cmd_joints.position[self.cmd_joints.name.index('left_shoulder_lift_joint')] = left_shoulder_lift_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('left_shoulder_lift_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index('left_shoulder_pan_joint')] = left_shoulder_pan_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('left_shoulder_pan_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index('left_elbow_joint')] = left_elbow_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('left_elbow_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = left_wrist_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = self.default_joint_speed
# left_elbow_rpy = [0]*3
# left_elbow_rpy = euler_from_quaternion(self.skeleton['orientation']['left_elbow'])
# left_arm_roll_angle = -left_elbow_rpy[2]
left_arm_roll_angle = acos(KDL.dot(left_shoulder_elbow, left_shoulder_neck))
#left_arm_roll_angle = -asin(left_shoulder_hand.x())
self.cmd_joints.position[self.cmd_joints.name.index('left_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_roll_joint')] = self.default_joint_speed
# left_wrist_angle = -acos(min(1, abs((lh - 0.265) / (0.30 - 0.265)))) + QUARTER_PI
# self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = 0
# self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = self.default_joint_speed
except KeyError:
pass
# Right Arm
try:
right_shoulder_neck = self.skeleton['position']['neck'] - self.skeleton['position']['right_shoulder']
right_shoulder_elbow = self.skeleton['position']['right_elbow'] - self.skeleton['position']['right_shoulder']
right_elbow_hand = self.skeleton['position']['right_hand'] - self.skeleton['position']['right_elbow']
right_shoulder_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_shoulder']
right_shoulder_neck.Normalize()
right_shoulder_elbow.Normalize()
rh = right_elbow_hand.Normalize()
right_shoulder_hand.Normalize()
right_shoulder_lift_angle = -(asin(right_shoulder_elbow.y()) + self.HALF_PI)
right_shoulder_pan_angle = -asin(right_elbow_hand.x())
right_elbow_angle = acos(KDL.dot(right_shoulder_elbow, right_elbow_hand))
right_wrist_angle = -right_elbow_angle / 2.0
right_elbow_angle = right_elbow_angle / 4.0
self.cmd_joints.position[self.cmd_joints.name.index('right_shoulder_lift_joint')] = right_shoulder_lift_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('right_shoulder_lift_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index('right_shoulder_pan_joint')] = right_shoulder_pan_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('right_shoulder_pan_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index('right_elbow_joint')] = right_elbow_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('right_elbow_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = right_wrist_angle
self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = self.default_joint_speed
right_arm_roll_angle = -asin(right_shoulder_hand.x())
self.cmd_joints.position[self.cmd_joints.name.index('right_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_roll_joint')] = self.default_joint_speed
# right_wrist_angle = acos(min(1, abs((rh - 0.265) / (0.30 - 0.265)))) - QUARTER_PI
# self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = 0
# self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = self.default_joint_speed
except KeyError:
pass
def perpendicular(feature1, feature2): sign = feature1.sign*feature2.sign return _inv(kdl.dot(feature1.direction, feature2.direction), sign)
def height(feature1, feature2): p = feature1.position - feature2.position # compute distance perpendicular to plane return kdl.dot(p, feature1.direction)
def spin(self):
simulation_only = False
key_endpoint_T = PyKDL.Vector(0,0,0.2)
T_B_P = PyKDL.Frame(PyKDL.Rotation(), PyKDL.Vector(1,0,1))
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
print "creating interface for Velma..."
# create the interface for Velma robot
self.velma = Velma()
print "done."
m_id = 0
self.pub_marker.eraseMarkers(0,3000, frame_id='world')
# key and grasp parameters
self.T_E_O = PyKDL.Frame()
self.T_E_Orot = PyKDL.Frame(PyKDL.Vector(0, 0, 0.07))
self.key_axis_O = PyKDL.Vector(0,0,1)
self.key_up_O = PyKDL.Vector(1,0,0)
self.key_side_O = self.key_axis_O * self.key_up_O
# change the tool - the safe way
print "switching to joint impedance..."
if not self.velma.switchToJoint():
print "ERROR: switchToJoint"
exit(0)
rospy.sleep(0.5)
print "updating tool..."
self.velma.updateTransformations()
self.velma.updateAndMoveToolOnly(PyKDL.Frame(self.velma.T_W_E.p+PyKDL.Vector(0.1,0,0)), 0.1)
rospy.sleep(0.5)
print "done."
print "switching to cartesian impedance..."
if not self.velma.switchToCart():
print "ERROR: switchToCart"
exit(0)
rospy.sleep(0.5)
raw_input("Press ENTER to continue...")
q_grasp = [1.4141768883517725, 1.4179811607057289, 1.4377081536379384, 0]
q_pre = 10.0/180.0*math.pi
hv = [1.2, 1.2, 1.2, 1.2]
ht = [3000, 3000, 3000, 3000]
# set gripper configuration
if True:
self.velma.moveHand([0.0, 0.0, 0.0, 0.0/180.0*math.pi], hv, ht, 5000, True)
rospy.sleep(3)
self.velma.moveHand([q_grasp[0]-q_pre, q_grasp[1]-q_pre, q_grasp[2]-q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(5)
self.velma.moveHand([q_grasp[0]+q_pre, q_grasp[1]+q_pre, q_grasp[2]+q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(3)
if False:
# start with very low stiffness
print "setting stiffness to very low value"
k_low = Wrench(Vector3(1.0, 1.0, 1.0), Vector3(0.5, 0.5, 0.5))
self.velma.moveImpedance(k_low, 0.5)
if self.velma.checkStopCondition(0.5):
exit(0)
print "done."
print "identifying the parameters of tool in..."
wait_time = 20
for i in range(wait_time):
print "%s s"%(wait_time-i)
rospy.sleep(1.0)
print "collecting points..."
self.collect_poses = True
T_B_T_list = []
thread.start_new_thread(self.posesCollectorThread, (T_B_T_list,))
collect_time = 10
for i in range(collect_time):
print "%s s"%(collect_time-i)
rospy.sleep(1.0)
self.collect_poses = False
rospy.sleep(0.5)
key_endpoint_T, error = self.estCommonPoint(T_B_T_list)
print key_endpoint_T, error
self.key_endpoint_O = self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() * self.velma.T_W_T * key_endpoint_T
print "self.key_endpoint_O = PyKDL.Vector(%s, %s, %s)"%(self.key_endpoint_O[0], self.key_endpoint_O[1], self.key_endpoint_O[2])
print "done."
else:
# self.key_endpoint_O = PyKDL.Vector(-0.00248664992634, -6.7348683886e-05, 0.232163117525)
self.key_endpoint_O = PyKDL.Vector(0.000256401261281, -0.000625166847342, 0.232297442735)
k_high = Wrench(Vector3(1000.0, 1000.0, 1000.0), Vector3(150, 150, 150))
max_force = 12.0
max_torque = 12.0
path_tol = (PyKDL.Vector(max_force/k_high.force.x, max_force/k_high.force.y, max_force/k_high.force.z), PyKDL.Vector(max_torque/k_high.torque.x, max_torque/k_high.torque.y, max_torque/k_high.torque.z))
max_force2 = 20.0
max_torque2 = 20.0
path_tol2 = (PyKDL.Vector(max_force2/k_high.force.x, max_force2/k_high.force.y, max_force2/k_high.force.z), PyKDL.Vector(max_torque2/k_high.torque.x, max_torque2/k_high.torque.y, max_torque2/k_high.torque.z))
path_tol3 = (PyKDL.Vector(max_force/k_high.force.x, max_force/k_high.force.y, max_force2/k_high.force.z), PyKDL.Vector(max_torque/k_high.torque.x, max_torque/k_high.torque.y, max_torque/k_high.torque.z))
print "path tolerance:", path_tol
# k_hole_in = Wrench(Vector3(1000.0, 500.0, 500.0), Vector3(200, 200, 200))
k_hole_in = Wrench(Vector3(100.0, 1000.0, 1000.0), Vector3(50, 5, 5))
path_tol_in = (PyKDL.Vector(max_force2/k_hole_in.force.x, max_force/k_hole_in.force.y, max_force/k_hole_in.force.z), PyKDL.Vector(max_torque2/k_hole_in.torque.x, max_torque2/k_hole_in.torque.y, max_torque2/k_hole_in.torque.z))
path_tol_in2 = (PyKDL.Vector(max_force2/k_hole_in.force.x, max_force2/k_hole_in.force.y, max_force2/k_hole_in.force.z), PyKDL.Vector(max_torque2/k_hole_in.torque.x, max_torque2/k_hole_in.torque.y, max_torque2/k_hole_in.torque.z))
if False:
k_increase = [
Wrench(Vector3(10.0, 10.0, 10.0), Vector3(5, 5, 5)),
Wrench(Vector3(50.0, 50.0, 50.0), Vector3(25, 25, 25)),
Wrench(Vector3(250.0, 250.0, 250.0), Vector3(100, 100, 100))
]
for k in k_increase:
raw_input("Press Enter to set bigger stiffness...")
self.velma.updateTransformations()
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
print "moving the wrist to the current pose in " + str(time) + " s..."
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)))
rospy.sleep(time)
print "done."
print "setting stiffness to bigger value"
self.velma.moveImpedance(k, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
print "move the grasped key near the key hole..."
self.follow_wrist_pose = True
thread.start_new_thread(self.wristFollowerThread, (None,))
raw_input("Press ENTER to save the pose...")
self.follow_wrist_pose = False
rospy.sleep(0.5)
self.velma.updateTransformations()
self.T_B_O_nearHole = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O
p = self.T_B_O_nearHole.p
q = self.T_B_O_nearHole.M.GetQuaternion()
print "self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(%s, %s, %s, %s), PyKDL.Vector(%s, %s, %s))"%(q[0], q[1], q[2], q[3], p[0], p[1], p[2])
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
print "moving the wrist to the current pose in " + str(time) + " s..."
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)))
status, feedback = self.velma.waitForCartEnd()
print "setting stiffness to", k_high
self.velma.moveImpedance(k_high, 1.0)
if self.velma.checkStopCondition(1.0):
exit(0)
print "done."
else:
# self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.697525674378, -0.00510212356955, 0.715527762697, 0.0381041038336), PyKDL.Vector(0.528142123375, 0.0071159410235, 1.31300679435))
# self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.699716675653, -0.0454110538496, 0.709529999372, 0.0700113561626), PyKDL.Vector(0.546491646893, -0.101297899801, 1.30959887442))
self.T_B_O_nearHole = PyKDL.Frame(PyKDL.Rotation.Quaternion(0.71891504857, -0.0529880479354, 0.691118088949, 0.0520500417212), PyKDL.Vector(0.883081316461, -0.100813768303, 0.95381559114))
k_increase = [
Wrench(Vector3(10.0, 10.0, 10.0), Vector3(5, 5, 5)),
Wrench(Vector3(50.0, 50.0, 50.0), Vector3(25, 25, 25)),
Wrench(Vector3(250.0, 250.0, 250.0), Vector3(100, 100, 100)),
k_high
]
for k in k_increase:
raw_input("Press Enter to set bigger stiffness...")
self.velma.updateTransformations()
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
print "moving the wrist to the current pose in " + str(time) + " s..."
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)))
rospy.sleep(time)
print "done."
print "setting stiffness to bigger value"
self.velma.moveImpedance(k, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
if True:
print "probing the door..."
search_radius = 0.02
min_dist = 0.005
probed_points = []
contact_points_B = []
# move to the center point and push the lock
x = 0
y = 0
T_O_Od = PyKDL.Frame(self.key_up_O*x + self.key_side_O*y)
T_O_Od2 = PyKDL.Frame(self.key_up_O*x + self.key_side_O*y + self.key_axis_O*0.1)
self.velma.updateTransformations()
T_B_Wd = self.T_B_O_nearHole * T_O_Od * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
print "unexpected contact", feedback
exit(0)
self.velma.updateTransformations()
T_B_Wd = self.T_B_O_nearHole * T_O_Od2 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_points_B.append(contact_B)
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
print feedback
else:
print "no contact"
exit(0)
T_O_Od3 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z)
self.velma.updateTransformations()
T_B_Wref = self.velma.T_B_W
T_B_Wd = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od3 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
print "unexpected high contact force", feedback
exit(0)
desired_push_dist = 5.0/k_high.force.z
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_points_B.append(contact_B)
# move along the spiral
hole_found = False
spiral = self.generateSpiral(search_radius, min_dist)
for pt in spiral:
pt_desired_O_in_ref = self.key_endpoint_O
pt_contact_O_in_ref = self.T_E_O.Inverse() * self.velma.T_W_E.Inverse() * T_B_Wref.Inverse() * contact_B
key_end_depth = PyKDL.dot(pt_contact_O_in_ref-pt_desired_O_in_ref, self.key_axis_O)
# print "key_end_depth", key_end_depth
T_O_Od4 = PyKDL.Frame(self.key_up_O*pt[0] + self.key_side_O*pt[1] + self.key_axis_O * (5.0/k_high.force.z + key_end_depth))
pt_desired_B = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od4 * self.key_endpoint_O
m_id = self.pub_marker.publishSinglePointMarker(pt_desired_B, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
T_B_Wd = T_B_Wref * self.velma.T_W_E * self.T_E_O * T_O_Od4 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol)
status, feedback = self.velma.waitForCartEnd()
print "status", status
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_points_B.append(contact_B)
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
# check if the key is in the hole
if status.error_code != 0:
self.velma.updateTransformations()
T_O_Od5 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z)
T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od5 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
if status.error_code != 0:
print "unexpected high contact force", feedback
exit(0)
self.velma.updateTransformations()
T_O_Od6 = PyKDL.Frame(self.key_up_O*0.02 + self.key_axis_O * 5.0/k_high.force.z)
T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od6 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol)
status, feedback = self.velma.waitForCartEnd()
if status.error_code != 0:
print "key is in a hole"
hole_found = True
break
print "hole_found", hole_found
T_O_Od3 = PyKDL.Frame(self.key_axis_O * 5.0/k_high.force.z)
self.velma.updateTransformations()
T_B_Wref = self.velma.T_B_W
print "moving the wrist to the current pose to reduce tension..."
T_B_Wd = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * T_O_Od3 * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.04)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status, feedback = self.velma.waitForCartEnd()
print "status", status
if status.error_code != 0:
print "unexpected high contact force", feedback
exit(0)
print "setting stiffness to bigger value"
self.velma.moveImpedance(k_hole_in, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
self.velma.updateTransformations()
# calculate the fixed lock frame T_B_L
# T_B_L the same orientation as the gripper (z axis pointing towards the door)
# and the origin at the key endpoint at the initial penetration
T_B_L = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * PyKDL.Frame(self.key_endpoint_O)
penetration_axis_L = PyKDL.Vector(0, 0, 1)
# get current contact point
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
contact_L = T_B_L.Inverse() * contact_B
contact_max_depth_L = PyKDL.dot(contact_L, penetration_axis_L)
contact_depth_L = contact_max_depth_L
prev_contact_depth_L = contact_depth_L
deepest_contact_L = copy.deepcopy(contact_L)
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
central_point_L = PyKDL.Vector()
force_key_axis = 17.0
force_key_up = 10.0
force_key_side = 10.0
xi = 7
yi = 7
explore_mode = "get_new"
while True:
# desired position of the key end
self.velma.updateTransformations()
T_B_Ocurrent = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * PyKDL.Frame(self.key_axis_O * (force_key_axis/k_hole_in.force.x))
diff_B = PyKDL.diff(T_B_Ocurrent, self.T_B_O_nearHole)
rot_diff_Ocurrent = PyKDL.Frame(T_B_Ocurrent.Inverse().M) * diff_B.rot
spiral_hole = self.generateSpiral(0.04, 0.005)
T_B_W_shake = []
for pt in spiral_hole:
T_B_Od_shake = T_B_Ocurrent * PyKDL.Frame(PyKDL.Rotation.RotZ(rot_diff_Ocurrent.z()-6.0/180.0*math.pi), self.key_up_O * pt[0] + self.key_side_O * pt[1])
T_B_W_shake.append(T_B_Od_shake * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse())
T_B_Od_shake = T_B_Ocurrent * PyKDL.Frame(PyKDL.Rotation.RotZ(rot_diff_Ocurrent.z()+6.0/180.0*math.pi), self.key_up_O * pt[0] + self.key_side_O * pt[1])
T_B_W_shake.append(T_B_Od_shake * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse())
print "shaking..."
counter = 0
for T_B_W in T_B_W_shake:
counter += 1
time = self.velma.getMovementTime(T_B_W, max_v_l = 0.4, max_v_r = 0.4)
# self.velma.moveWrist2(T_B_W * self.velma.T_W_T)
# raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_W, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
self.velma.updateTransformations()
contact_B = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
m_id = self.pub_marker.publishSinglePointMarker(contact_B, m_id, r=1, g=0, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
pt_desired_B = T_B_W * self.velma.T_W_E * self.T_E_O * self.key_endpoint_O
m_id = self.pub_marker.publishSinglePointMarker(pt_desired_B, m_id, r=0, g=1, b=0, a=1, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
contact_L = T_B_L.Inverse() * contact_B
contact_depth_L = PyKDL.dot(contact_L, penetration_axis_L)
if contact_depth_L > contact_max_depth_L+0.0001:
deepest_contact_L = copy.deepcopy(contact_L)
contact_max_depth_L = contact_depth_L
print "contact_depth_L: %s"%(contact_depth_L)
explore_mode = "push"
break
if status1.error_code != 0:
break
print "done."
score = contact_depth_L - prev_contact_depth_L
prev_contact_depth_L = contact_depth_L
if status1.error_code != 0 or counter == len(T_B_W_shake):
break
print "moving the key to the current pose..."
self.velma.updateTransformations()
T_B_Wd = self.velma.T_B_W
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.1, max_v_r = 0.1)
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in2)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
if status1.error_code != 0:
exit(0)
print "moving the key to the current pose..."
self.velma.updateTransformations()
T_B_Ocurrent = self.velma.T_B_W * self.velma.T_W_E * self.T_E_O
T_B_Od = T_B_Ocurrent * PyKDL.Frame(self.key_axis_O * (-2.0/k_hole_in.force.x))
T_B_Wd = T_B_Od * self.T_E_O.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.1, max_v_r = 0.1)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol_in)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
if status1.error_code != 0:
exit(0)
self.velma.moveHand([q_grasp[0]-q_pre, q_grasp[1]-q_pre, q_grasp[2]-q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(2)
print "setting stiffness to bigger value"
self.velma.moveImpedance(k_high, 1.0)
if self.velma.checkStopCondition(1.1):
exit(0)
print "done."
T_B_Wd = T_B_Ocurrent * self.T_E_Orot.Inverse() * self.velma.T_W_E.Inverse()
time = self.velma.getMovementTime(T_B_Wd, max_v_l = 0.02, max_v_r = 0.02)
raw_input("Press ENTER to movie the wrist in " + str(time) + " s...")
self.velma.moveWrist(T_B_Wd, time, Wrench(Vector3(40,40,40), Vector3(14,14,14)), path_tol=path_tol2)
status1, feedback = self.velma.waitForCartEnd()
print "status", status1
if status1.error_code != 0:
exit(0)
self.velma.moveHand([q_grasp[0]+q_pre, q_grasp[1]+q_pre, q_grasp[2]+q_pre, q_grasp[3]], hv, ht, 5000, True)
rospy.sleep(5)
# m_id = self.pub_marker.publishSinglePointMarker(T_B_L * PyKDL.Vector(0.01*xi+ori_map_vis_offset[0], 0.01*yi+ori_map_vis_offset[1], score), m_id, r=0, g=0, b=1, a=0.5, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
# m_id = self.pub_marker.publishSinglePointMarker(T_B_L * PyKDL.Vector(0.01*xi+ori_map_vis_offset[0], 0.01*yi+ori_map_vis_offset[1], ori_map[xi][yi][1]), m_id, r=0, g=1, b=0, a=0.5, namespace='default', frame_id='torso_base', m_type=Marker.SPHERE, scale=Vector3(0.003, 0.003, 0.003), T=None)
exit(0)
def aligned(feature1, feature2): ''' zero is aligned ''' sign = -feature1.sign*feature2.sign return _inv(kdl.dot(feature1.direction, feature2.direction), sign)
def place(self, targPoints):
N = 10
target_point_real_a = self.map_to_real(targPoints[0], N)
target_point_real_b = self.map_to_real(targPoints[1], N)
grasp_point_real_a = self.map_to_real(self.graspPoints[0], N)
grasp_point_real_b = self.map_to_real(self.graspPoints[1], N)
diff_a = grasp_point_real_a - target_point_real_a
line_a = (map(lambda a:a * 0.5, diff_a)) + target_point_real_a
diff_b = grasp_point_real_b - target_point_real_b
line_b = (map(lambda a:a * 0.5, diff_b)) + target_point_real_b
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
input_frame = self.pc_frame_id
input_g1 = PyKDL.Frame()
input_g2 = PyKDL.Frame()
input_p1 = PyKDL.Frame()
input_p2 = PyKDL.Frame()
gp1 = grasp_point_real_a
gp2 = grasp_point_real_b
Rz = PyKDL.Rotation().Identity()
Rz.DoRotZ(self.approach_angles[0][0])
kdl_grasp = PyKDL.Vector(self.graspPoints[0][0], self.graspPoints[0][1], 0)
kdl_v = PyKDL.Vector(20, 0, 0) #units are pixels
kdl_v = Rz * kdl_v
v = (kdl_v[0], kdl_v[1])
tp1 = self.map_to_real(kdl_v, N) - gp1
Rz = PyKDL.Rotation().Identity()
Rz.DoRotZ(self.approach_angles[1][0])
kdl_grasp = PyKDL.Vector(self.graspPoints[1][0], self.graspPoints[1][1], 0)
kdl_v = PyKDL.Vector(20, 0, 0) #units are pixels
kdl_v = Rz * kdl_v
v = (kdl_v[0], kdl_v[1])
tp2 = self.map_to_real(kdl_v, N) - gp2
#tp1 = target_point_real_a
#tp2 = target_point_real_b
input_g1.p = PyKDL.Vector(gp1[0], gp1[1], gp1[2])
z_ = PyKDL.Vector(tp1[0] - gp1[0], tp1[1] - gp1[1], tp1[2] - gp1[2])
z_.Normalize()
z = PyKDL.Vector(0, 0, 1)
v = z_ * z
v.Normalize()
angle = math.acos(PyKDL.dot(z_, z))
input_g1.M = PyKDL.Rotation.Rot(v, -angle)
input_g2.p = PyKDL.Vector(gp2[0], gp2[1], gp2[2])
z_ = PyKDL.Vector(tp2[0] - gp2[0], tp2[1] - gp2[1], tp2[2] - gp2[2])
z_.Normalize()
z = PyKDL.Vector(0, 0, 1)
v = z_ * z
v.Normalize()
angle = math.acos(PyKDL.dot(z_, z))
input_g2.M = PyKDL.Rotation.Rot(v, -angle)
input_p1.p = PyKDL.Vector(line_a[0], line_a[1], line_a[2])
input_p2.p = PyKDL.Vector(line_b[0], line_b[1], line_b[2])
input_p1.M = PyKDL.Rotation.Identity()
input_p2.M = PyKDL.Rotation.Identity()
tmp_pose = PoseStamped()
tmp_pose.header.frame_id = input_frame
tmp_pose.pose = posemath.toMsg(input_g1)
sm.userdata.g1 = copy.deepcopy(tmp_pose)
tmp_pose.pose = posemath.toMsg(input_g2)
sm.userdata.g2 = copy.deepcopy(tmp_pose)
tmp_pose.pose = posemath.toMsg(input_p1)
sm.userdata.p1 = copy.deepcopy(tmp_pose)
tmp_pose.pose = posemath.toMsg(input_p2)
sm.userdata.p2 = copy.deepcopy(tmp_pose)
tmp_pose.pose = posemath.toMsg(input_g1)
sm.userdata.g1_r = copy.deepcopy(tmp_pose)
tmp_pose.pose = posemath.toMsg(input_g2)
sm.userdata.g2_r = copy.deepcopy(tmp_pose)
sm.userdata.ik_link_1 = 'r1_ee'
sm.userdata.ik_link_2 = 'r2_ee'
sm.userdata.ik_link_1_r = 'r2_ee'
sm.userdata.ik_link_2_r = 'r1_ee'
sm.userdata.table_id = 't2'
sm.userdata.offset_plus = 0.02
sm.userdata.offset_minus = 0.02
sm.userdata.rotation_angle_1 = 0
sm.userdata.rotation_angle_2 = 0
table_offset = 0.075
sm.userdata.grasp_angle_allowed1 = self.approach_angles[0][1] * 0.9
sm.userdata.grasp_angle_allowed2 = self.approach_angles[1][1] * 0.9
br = tf.TransformBroadcaster()
rospy.sleep(1.0)
tmp = posemath.fromMsg(sm.userdata.g1.pose)
br.sendTransform(tmp.p, tmp.M.GetQuaternion() , rospy.Time.now(), 'G1' , sm.userdata.g1.header.frame_id)
tmp = posemath.fromMsg(sm.userdata.g2.pose)
br.sendTransform(tmp.p, tmp.M.GetQuaternion() , rospy.Time.now(), 'G2' , sm.userdata.g1.header.frame_id)
tmp = posemath.fromMsg(sm.userdata.p1.pose)
br.sendTransform(tmp.p, tmp.M.GetQuaternion() , rospy.Time.now(), 'P1' , sm.userdata.g1.header.frame_id)
tmp = posemath.fromMsg(sm.userdata.p2.pose)
br.sendTransform(tmp.p, tmp.M.GetQuaternion() , rospy.Time.now(), 'P2' , sm.userdata.g1.header.frame_id)
sm_go_home = gensm_plan_vis_exec(PlanToHomeState(), output_keys=['trajectory'])
with sm:
smach.StateMachine.add('GFOLD_REVERT', GFold2RobustState(True, True, table_offset, 0.01),
remapping={'ik_link_1':'ik_link_1_r', 'ik_link_2':'ik_link_2_r', 'g1':'g1_r', 'g2':'g2_r'},
transitions={'aborted':'GFOLD', 'succeeded':'succeeded'})
smach.StateMachine.add('GFOLD', GFold2RobustState(True, True, table_offset, 0.01))
outcome = sm.execute()
raw_input("Enter to continue")
def sampleMesh(vertices, indices, sample_dist, pt_list, radius, return_normals=False):
points = []
points_normals = []
for s2 in pt_list:
for face in indices:
A = vertices[face[0]]
B = vertices[face[1]]
C = vertices[face[2]]
pt_a = PyKDL.Vector(A[0], A[1], A[2])
pt_b = PyKDL.Vector(B[0], B[1], B[2])
pt_c = PyKDL.Vector(C[0], C[1], C[2])
v0 = pt_b - pt_a
v1 = pt_c - pt_a
# calculate face normal
normal = v0 * v1
normal.Normalize()
# calculate distance between the sphere center and the face
s_dist = PyKDL.dot(normal, s2) - PyKDL.dot(normal, pt_a)
# if the distance is greater than radius, ignore the face
if abs(s_dist) > radius:
continue
# calculate the projection of the sphere center to the face
s_on = s2 - s_dist * normal
# calculate the radius of circle being common part of sphere and face
radius2_square = radius * radius - s_dist * s_dist
if radius2_square < 0.0: # in case of numerical error
radius2_square = 0.0
radius2 = math.sqrt(radius2_square)
# TODO: check only the face's area of interest
v0p = v0 * v1 * v0
v0p.Normalize()
v1p = v1 * v0 * v1
v1p.Normalize()
d0 = PyKDL.dot(v0p, (s_on - pt_a))
d1 = PyKDL.dot(v1p, (s_on - pt_a))
n0 = v0.Norm()
steps0 = int(n0 / sample_dist)
if steps0 < 1:
steps0 = 1
step_len0 = n0 / steps0
n1 = v1.Norm()
angle = getAngle(v0, v1)
h = n1 * math.sin(angle)
steps1 = int(h / sample_dist)
if steps1 < 1:
steps1 = 1
step_len1 = h / steps1
x0_min = (d1 - radius) / math.sin(angle)
x0_max = (d1 + radius) / math.sin(angle)
x1_min = d0 - radius
x1_max = d0 + radius
x0_min2 = max(step_len0 / 2.0, x0_min)
x0_max2 = min(n0, x0_max)
if x0_min2 >= x0_max2:
continue
for x0 in np.arange(x0_min2, x0_max2, step_len0):
x1_min2 = max(step_len1 / 2.0, x1_min)
x1_max2 = min(h * (1.0 - x0 / n0), x1_max)
if x1_min2 >= x1_max2:
continue
for x1 in np.arange(x1_min2, x1_max2, step_len1):
point = pt_a + v0 * (x0 / n0) + v1 * (x1 / h)
in_range = False
for s2 in pt_list:
if (point - s2).Norm() < radius:
in_range = True
break
if in_range:
points.append(point)
points_normals.append(normal)
if len(pt_list) == 1:
if return_normals:
return points, points_normals
else:
return points
min_dists = []
min_dists_p_index = []
for s in pt_list:
min_dists.append(1000000.0)
min_dists_p_index.append(None)
i = 0
for s in pt_list:
p_index = 0
for p in points:
d = (s - p).Norm()
if d < min_dists[i]:
min_dists[i] = d
min_dists_p_index[i] = p_index
p_index += 1
i += 1
first_contact_index = None
for i in range(0, len(pt_list)):
if min_dists[i] < sample_dist * 2.0:
first_contact_index = i
break
if first_contact_index == None:
print "first_contact_index == None"
return points
init_pt = points[min_dists_p_index[first_contact_index]]
points_ret = []
list_to_check = []
list_check_from = []
for i in range(0, len(points)):
if (init_pt - points[i]).Norm() > radius:
continue
if i == min_dists_p_index[first_contact_index]:
list_check_from.append(points[i])
else:
list_to_check.append(points[i])
points_ret = []
added_point = True
iteration = 0
while added_point:
added_point = False
list_close = []
list_far = []
for p in list_to_check:
added_p = False
for check_from in list_check_from:
if (check_from - p).Norm() < sample_dist * 2.0:
added_point = True
added_p = True
list_close.append(p)
break
if not added_p:
list_far.append(p)
points_ret += list_check_from
list_to_check = copy.deepcopy(list_far)
list_check_from = copy.deepcopy(list_close)
iteration += 1
return points_ret
def get_closest_waypoint(self,
pose,
waypts,
direction=None,
search_radius=300.0):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
waypts (list): list of waypoints to search
direction: direction for the visibility check - 'F' for waypoint search, 'R' for traffic light search
search_radius: search radius, [m]
Returns:
int: index of the closest waypoint in self.waypoints
"""
# O2 performance brute force search, consider optimizing!
best_distance = np.Inf
best_angle = None
best_i = None
if self.waypoints:
for i, wpt in enumerate(waypts):
# import ipdb; ipdb.set_trace()
distance = np.sqrt(
(wpt.pose.pose.position.x - pose.position.x)**2 +
(wpt.pose.pose.position.y - pose.position.y)**2)
if (distance < best_distance) and (distance < search_radius):
# check for visibility:
if direction == 'F' or direction == 'R':
# pose quaternion
p_q = PyKDL.Rotation.Quaternion(
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
# waypoint quaternion
w_q = PyKDL.Rotation.Quaternion(
wpt.pose.pose.orientation.x,
wpt.pose.pose.orientation.y,
wpt.pose.pose.orientation.z,
wpt.pose.pose.orientation.w)
# import ipdb; ipdb.set_trace()
# let's use scalar product to find the angle between the car orientation vector and car/base_point vector
car_orientation = p_q * PyKDL.Vector(1.0, 0.0, 0.0)
wp_vector = PyKDL.Vector(
wpt.pose.pose.position.x - pose.position.x,
wpt.pose.pose.position.y - pose.position.y, 0.0)
cos_angle = PyKDL.dot(
car_orientation, wp_vector) / car_orientation.Norm(
) / wp_vector.Norm()
angle = np.arccos(cos_angle)
if direction == 'F':
if angle < np.pi / 2.0:
best_distance = distance
best_angle = angle
best_i = i
if direction == 'R':
if angle > np.pi / 2.0:
best_distance = distance
best_angle = angle
best_i = i
else:
angle = 0
best_distance = distance
best_angle = angle
best_i = i
return best_i, best_angle, best_distance
if not self.waypoints or not self.waypoints.waypoints:
rospy.logerr("Waypoints empty")
return None
my_waypoints = self.waypoints.waypoints
rospy.logdebug("waypoints: {}".format(len(my_waypoints)))
pos_x = pose.position.x
pos_y = pose.position.y
current_distance = sys.maxsize
current_waypoint = None
#Lets find where we are using the euclidean distance
for i in range(0, len(my_waypoints)):
waypoint_pos_x = my_waypoints[i].pose.pose.position.x
waypoint_pos_y = my_waypoints[i].pose.pose.position.y
pos_distance = math.sqrt(
math.pow(waypoint_pos_x - pos_x, 2) +
math.pow(waypoint_pos_y - pos_y, 2))
#find closest distance
if pos_distance < current_distance:
current_waypoint = i
current_distance = pos_distance
return current_waypoint
def teleop_joints(self):
# Fixed Joints: Set position to 0 radians.
self.cmd_joints.position[self.cmd_joints.name.index(
'left_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_arm_roll_joint')] = 2
self.cmd_joints.position[self.cmd_joints.name.index(
'right_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_arm_roll_joint')] = 2
self.cmd_joints.position[self.cmd_joints.name.index(
'left_wrist_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_wrist_joint')] = 2
self.cmd_joints.position[self.cmd_joints.name.index(
'right_wrist_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_wrist_joint')] = 2
# Torso Rotation
try:
torso_quaternion = self.skeleton['orientation']['torso']
torso_rpy = torso_quaternion.GetRPY()
self.cmd_joints.position[self.cmd_joints.name.index(
self.skel_to_joint_map['torso'])] = torso_rpy[1] / 2.0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'torso_joint')] = self.default_joint_speed
except:
pass
# Head Pan
try:
head_quaternion = self.skeleton['orientation']['head']
head_rpy = head_quaternion.GetRPY()
self.cmd_joints.position[self.cmd_joints.name.index(
'head_pan_joint')] = head_rpy[1]
self.cmd_joints.velocity[self.cmd_joints.name.index(
'head_pan_joint')] = self.default_joint_speed
except:
pass
# Head Tilt
try:
self.cmd_joints.position[self.cmd_joints.name.index(
'head_tilt_joint')] = head_rpy[0]
self.cmd_joints.velocity[self.cmd_joints.name.index(
'head_tilt_joint')] = self.default_joint_speed
except:
pass
# Left Arm
try:
left_shoulder_neck = self.skeleton['position'][
'neck'] - self.skeleton['position']['left_shoulder']
left_shoulder_elbow = self.skeleton['position'][
'left_elbow'] - self.skeleton['position']['left_shoulder']
left_elbow_hand = self.skeleton['position'][
'left_hand'] - self.skeleton['position']['left_elbow']
left_shoulder_hand = self.skeleton['position'][
'left_hand'] - self.skeleton['position']['left_shoulder']
left_shoulder_neck.Normalize()
left_shoulder_elbow.Normalize()
lh = left_elbow_hand.Normalize()
left_shoulder_hand.Normalize()
left_shoulder_lift_angle = asin(
left_shoulder_elbow.y()) + self.HALF_PI
left_shoulder_pan_angle = asin(left_elbow_hand.x())
left_elbow_angle = -acos(
KDL.dot(left_shoulder_elbow, left_elbow_hand))
left_wrist_angle = -left_elbow_angle / 2.0
left_elbow_angle = left_elbow_angle / 4.0
self.cmd_joints.position[self.cmd_joints.name.index(
'left_shoulder_lift_joint')] = left_shoulder_lift_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_shoulder_lift_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index(
'left_shoulder_pan_joint')] = left_shoulder_pan_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_shoulder_pan_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index(
'left_elbow_joint')] = left_elbow_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_elbow_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index(
'left_wrist_joint')] = left_wrist_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_wrist_joint')] = self.default_joint_speed
# left_elbow_rpy = [0]*3
# left_elbow_rpy = euler_from_quaternion(self.skeleton['orientation']['left_elbow'])
# left_arm_roll_angle = -left_elbow_rpy[2]
left_arm_roll_angle = acos(
KDL.dot(left_shoulder_elbow, left_shoulder_neck))
#left_arm_roll_angle = -asin(left_shoulder_hand.x())
self.cmd_joints.position[self.cmd_joints.name.index(
'left_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'left_arm_roll_joint')] = self.default_joint_speed
# left_wrist_angle = -acos(min(1, abs((lh - 0.265) / (0.30 - 0.265)))) + QUARTER_PI
# self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = 0
# self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = self.default_joint_speed
except KeyError:
pass
# Right Arm
try:
right_shoulder_neck = self.skeleton['position'][
'neck'] - self.skeleton['position']['right_shoulder']
right_shoulder_elbow = self.skeleton['position'][
'right_elbow'] - self.skeleton['position']['right_shoulder']
right_elbow_hand = self.skeleton['position'][
'right_hand'] - self.skeleton['position']['right_elbow']
right_shoulder_hand = self.skeleton['position'][
'left_hand'] - self.skeleton['position']['left_shoulder']
right_shoulder_neck.Normalize()
right_shoulder_elbow.Normalize()
rh = right_elbow_hand.Normalize()
right_shoulder_hand.Normalize()
right_shoulder_lift_angle = -(asin(right_shoulder_elbow.y()) +
self.HALF_PI)
right_shoulder_pan_angle = -asin(right_elbow_hand.x())
right_elbow_angle = acos(
KDL.dot(right_shoulder_elbow, right_elbow_hand))
right_wrist_angle = -right_elbow_angle / 2.0
right_elbow_angle = right_elbow_angle / 4.0
self.cmd_joints.position[self.cmd_joints.name.index(
'right_shoulder_lift_joint')] = right_shoulder_lift_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_shoulder_lift_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index(
'right_shoulder_pan_joint')] = right_shoulder_pan_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_shoulder_pan_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index(
'right_elbow_joint')] = right_elbow_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_elbow_joint')] = self.default_joint_speed
self.cmd_joints.position[self.cmd_joints.name.index(
'right_wrist_joint')] = right_wrist_angle
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_wrist_joint')] = self.default_joint_speed
right_arm_roll_angle = -asin(right_shoulder_hand.x())
self.cmd_joints.position[self.cmd_joints.name.index(
'right_arm_roll_joint')] = 0
self.cmd_joints.velocity[self.cmd_joints.name.index(
'right_arm_roll_joint')] = self.default_joint_speed
# right_wrist_angle = acos(min(1, abs((rh - 0.265) / (0.30 - 0.265)))) - QUARTER_PI
# self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = 0
# self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = self.default_joint_speed
except KeyError:
pass
def sampleMeshDetailedRays(vertices, faces, sample_dist):
dim_min = [None, None, None]
dim_max = [None, None, None]
for v in vertices:
for dim in range(3):
if dim_min[dim] == None or dim_min[dim] > v[dim]:
dim_min[dim] = v[dim]
if dim_max[dim] == None or dim_max[dim] < v[dim]:
dim_max[dim] = v[dim]
rays_dim = [None, None, None]
for dim in range(3):
dim_min[dim] += 0.5 * sample_dist
# rays_dim[i] = np.arange(dim_min[dim], dim_max[dim], sample_dist)
other_dim = [[1,2],[0,2],[0,1]]
point_id = 0
points = []
for face in faces:
A = vertices[face[0]]
B = vertices[face[1]]
C = vertices[face[2]]
pt_a = PyKDL.Vector(A[0],A[1],A[2])
pt_b = PyKDL.Vector(B[0],B[1],B[2])
pt_c = PyKDL.Vector(C[0],C[1],C[2])
v0 = pt_b - pt_a
v1 = pt_c - pt_a
# calculate the face normal
normal = v0 * v1
normal.Normalize()
# calculate the distance of the face from the origin
fd = -PyKDL.dot(pt_a, normal)
# get the direction (x,y,z) the face is pointing to
normal_max = 0.0
normal_max_dim = None
for dim in range(3):
if abs(normal[dim]) > normal_max:
normal_max = abs(normal[dim])
normal_max_dim = dim
dimensions = other_dim[normal_max_dim]
fdim_min = [None, None, None]
fdim_max = [None, None, None]
for pt in [pt_a, pt_b, pt_c]:
for dim in range(3):
if fdim_min[dim] == None or fdim_min[dim] > pt[dim]:
fdim_min[dim] = pt[dim]
if fdim_max[dim] == None or fdim_max[dim] < pt[dim]:
fdim_max[dim] = pt[dim]
n0_count = math.ceil((fdim_min[dimensions[0]] - dim_min[dimensions[0]])/sample_dist)
n1_count = math.ceil((fdim_min[dimensions[1]] - dim_min[dimensions[1]])/sample_dist)
for d0 in np.arange(dim_min[dimensions[0]] + sample_dist * n0_count, fdim_max[dimensions[0]], sample_dist):
for d1 in np.arange(dim_min[dimensions[1]] + sample_dist * n1_count, fdim_max[dimensions[1]], sample_dist):
pt_in = pointInTriangle([pt_a[dimensions[0]], pt_a[dimensions[1]]],
[pt_b[dimensions[0]], pt_b[dimensions[1]]],
[pt_c[dimensions[0]], pt_c[dimensions[1]]], [d0, d1])
if pt_in:
d2 = -(normal[dimensions[0]]*d0 + normal[dimensions[1]]*d1 + fd)/normal[normal_max_dim]
pt_tuple = [None, None, None]
pt_tuple[dimensions[0]] = d0
pt_tuple[dimensions[1]] = d1
pt_tuple[normal_max_dim] = d2
surf_pt = SurfacePoint()
surf_pt.id = point_id
surf_pt.pos = PyKDL.Vector(pt_tuple[0], pt_tuple[1], pt_tuple[2])
surf_pt.normal = normal
points.append(surf_pt)
point_id += 1
# put the points in voxels
for dim in range(3):
dim_min[dim] -= 0.5 * sample_dist
dim_max[dim] += 1.5 * sample_dist
def getPointIndex(pt):
voxel_size = 10.0
return [int((pt[0] - dim_min[0])/(sample_dist*voxel_size)), int((pt[1] - dim_min[1])/(sample_dist*voxel_size)), int((pt[2] - dim_min[2])/(sample_dist*voxel_size))]
voxel_map_size = getPointIndex(dim_max)
voxel_map_size[0] += 1
voxel_map_size[1] += 1
voxel_map_size[2] += 1
voxel_map = []
for x in range(voxel_map_size[0]):
voxel_map.append([])
for y in range(voxel_map_size[1]):
voxel_map[-1].append([])
for z in range(voxel_map_size[2]):
voxel_map[-1][-1].append([])
voxel_map[-1][-1][-1] = []
# add all points to the voxel map
for p in points:
idx = getPointIndex(p.pos)
voxel_map[idx[0]][idx[1]][idx[2]].append(p.id)
def getSector(pt):
x = pt[0]
ax = abs(pt[0])
y = pt[1]
ay = abs(pt[1])
z = pt[2]
az = abs(pt[2])
if ay <= x and az <= x:
return 0
elif ay <= -x and az <= -x:
return 1
elif az <= y and ax <= y:
return 2
elif az <= -y and ax <= -y:
return 3
elif ax <= z and ay <= z:
return 4
elif ax <= -z and ay <= -z:
return 5
# get neighbors
for p in points:
idx = getPointIndex(p.pos)
idx_min = [
idx[0]-1 if idx[0]>0 else idx[0],
idx[1]-1 if idx[1]>0 else idx[1],
idx[2]-1 if idx[2]>0 else idx[2]]
idx_max = [
idx[0]+1 if idx[0]<voxel_map_size[0] else idx[0],
idx[1]+1 if idx[1]<voxel_map_size[1] else idx[1],
idx[2]+1 if idx[2]<voxel_map_size[2] else idx[2]]
ids = []
for x in range(idx_min[0], idx_max[0]):
for y in range(idx_min[1], idx_max[1]):
for z in range(idx_min[2], idx_max[2]):
ids = ids + voxel_map[x][y][z]
sectors = [None,None,None,None,None,None] # +x -x +y -y +z -z
for p_id in ids:
diff = points[p_id].pos - p.pos
sect = getSector(diff)
dist = diff.Norm()
if dist > sample_dist * 2.0:
continue
if sectors[sect] == None or sectors[sect][0] > dist:
sectors[sect] = (dist, p_id)
for s in sectors:
if s == None:
continue
if not s[1] in p.neighbors_id:
p.neighbors_id.append(s[1])
if not p.id in points[s[1]].neighbors_id:
points[s[1]].neighbors_id.append(p.id)
return points
def handler(self, msg):
for joint in msg.name:
self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(
joint)]
self.skeleton['position'][joint] = KDL.Vector(
msg.position[msg.name.index(joint)].x,
msg.position[msg.name.index(joint)].y,
msg.position[msg.name.index(joint)].z)
self.skeleton['orientation'][joint] = KDL.Rotation.Quaternion(
msg.orientation[msg.name.index(joint)].x,
msg.orientation[msg.name.index(joint)].y,
msg.orientation[msg.name.index(joint)].z,
msg.orientation[msg.name.index(joint)].w)
#Right Arm
right_shoulder_elbow = self.skeleton['position'][
'right_elbow'] - self.skeleton['position']['right_shoulder']
right_arm_elbow_flex_hand = self.skeleton['position'][
'right_hand'] - self.skeleton['position']['right_elbow']
right_shoulder_hand = self.skeleton['position'][
'right_hand'] - self.skeleton['position']['right_shoulder']
right_shoulder_elbow.Normalize()
right_shoulder_hand.Normalize()
right_arm_elbow_flex_hand.Normalize()
right_arm_shoulder_lift_angle = math.degrees(
-(asin(right_shoulder_elbow.y()) + self.HALF_PI))
right_arm_shoulder_pan_angle = math.degrees(
-asin(right_arm_elbow_flex_hand.x()))
right_arm_elbow_flex_angle = 0.66 * math.degrees(
acos(KDL.dot(right_shoulder_elbow, right_arm_elbow_flex_hand)))
# Left Arm
left_shoulder_elbow = self.skeleton['position'][
'left_elbow'] - self.skeleton['position']['left_shoulder']
left_arm_elbow_flex_hand = self.skeleton['position'][
'left_hand'] - self.skeleton['position']['left_elbow']
left_shoulder_hand = self.skeleton['position'][
'left_hand'] - self.skeleton['position']['left_shoulder']
left_shoulder_elbow.Normalize()
left_shoulder_hand.Normalize()
left_arm_elbow_flex_hand.Normalize()
left_arm_shoulder_lift_angle = math.degrees(
asin(left_shoulder_elbow.y()) + self.HALF_PI)
left_arm_shoulder_pan_angle = math.degrees(
asin(left_arm_elbow_flex_hand.x()))
left_arm_elbow_flex_angle = 0.66 * math.degrees(
-acos(KDL.dot(left_shoulder_elbow, left_arm_elbow_flex_hand)))
right_femur = self.skeleton['position']['right_knee'] - self.skeleton[
'position']['right_hip']
right_femur.Normalize()
right_thigh_angle = math.degrees(
(asin(right_femur.y()) + self.HALF_PI))
left_femur = self.skeleton['position']['left_knee'] - self.skeleton[
'position']['left_hip']
left_femur.Normalize()
left_thigh_angle = math.degrees((asin(left_femur.y()) + self.HALF_PI))
waist = self.skeleton['position']['right_hip'] - self.skeleton[
'position']['left_hip']
waist.Normalize()
waist_angle = math.degrees((asin(waist.z())))
# print waist_angle
# print "Right Knee",
# print right_thigh_angle
# print "Left Knee",
# print left_thigh_angle
# print "Right Lift ",
# print right_arm_shoulder_lift_angle
# print "Right Pan ",
# print right_arm_shoulder_pan_angle
# print "Right Elbow ",
# print right_arm_elbow_flex_angle
#
# print "Left Lift ",
# print left_arm_shoulder_lift_angle
# print "Left Pan ",
# print left_arm_shoulder_pan_angle
# print "Left Elbow ",
# print left_arm_elbow_flex_angle
self.msg = " ".join(
map(str, [
right_arm_shoulder_lift_angle, left_arm_shoulder_lift_angle,
right_arm_shoulder_pan_angle, left_arm_shoulder_pan_angle,
right_arm_elbow_flex_angle, left_arm_elbow_flex_angle,
right_thigh_angle, left_thigh_angle, waist_angle
]))
def _get_facing_angle(self, origin_frame, target_frame): ray = origin_frame.p - target_frame.p ray.Normalize() target_normal = target_frame.M * PyKDL.Vector(1, 0, 0) return angles.normalize_angle(math.acos(PyKDL.dot(target_normal, ray)) - math.pi)
