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)