Python PyKDL类代码示例

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 getCartImpWristTraj(self, js, goal_T_B_W):
        init_js = copy.deepcopy(js)
        init_T_B_W = self.fk_solver.calculateFk("right_arm_7_link", init_js)
        T_B_Wd = goal_T_B_W
        T_B_W_diff = PyKDL.diff(init_T_B_W, T_B_Wd, 1.0)
        steps = int(max(T_B_W_diff.vel.Norm() / 0.05, T_B_W_diff.rot.Norm() / (20.0 / 180.0 * math.pi)))
        if steps < 2:
            steps = 2

        self.updateJointLimits(init_js)
        self.updateMimicJoints(init_js)
        q_list = []
        for f in np.linspace(0.0, 1.0, steps):
            T_B_Wi = PyKDL.addDelta(init_T_B_W, T_B_W_diff, f)
            q_out = self.fk_solver.simulateTrajectory("right_arm_7_link", init_js, T_B_Wi)
            if q_out == None:
                #                print "error: getCartImpWristTraj: ", str(f)
                return None
            q_list.append(q_out)

            for i in range(7):
                joint_name = self.fk_solver.ik_joint_state_name["right_arm_7_link"][i]
                init_js[joint_name] = q_out[i]

        return q_list

    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 calculateWrenchesForTransportTask(self, ext_wrenches_W, transport_T_B_O):
     ext_wrenches_O = []
     for i in range(len(transport_T_B_O)-1):
         diff_B_O = PyKDL.diff(transport_T_B_O[i], transport_T_B_O[i+1])
         # simulate object motion and calculate expected wrenches
         for t in np.linspace(0.0, 1.0, 5):
             T_B_Osim = PyKDL.addDelta(transport_T_B_O[i], diff_B_O, t)
             T_Osim_B = T_B_Osim.Inverse()
             for ewr in ext_wrenches_W:
                 ext_wrenches_O.append(PyKDL.Frame(T_Osim_B.M) * ewr)
     return ext_wrenches_O

 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 calc_R(px, py, pz):
     R_7_M = PyKDL.Frame(rot_mx, PyKDL.Vector(px, py, pz))
     ret = []
     for idx in range(0,len(T_7bp_7i)):
         diff = PyKDL.diff( T_7bp_7i[idx] * R_7_M, R_7_M * T_Mbp_Mi[idx] )
         ret.append( diff.vel.Norm() * weights_pos[idx] )
     return ret

 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 _update(self):
        # Leave if ROS is not running or command is not valid
        if rospy.is_shutdown() or self._last_goal is None:
            return
        # Calculate the goal pose
        goal = self._get_goal()

        # End-effector's pose
        ee_pose = self._arm_interface.get_ee_pose_as_frame()
        # End-effector's velocity
        ee_vel = self._arm_interface.get_ee_vel_as_kdl_twist()
        # Calculate pose error
        error = PyKDL.diff(goal, ee_pose)
        # End-effector wrench to achieve target
        wrench = np.matrix(np.zeros(6)).T
        for i in range(len(wrench)):
            wrench[i] = -(self._Kp[i] * error[i] + self._Kd[i] * ee_vel[i])

        # Compute jacobian transpose
        JT = self._arm_interface.jacobian_transpose()
        # Calculate the torques for the joints
        tau = JT * wrench
        # Store current pose target
        self._last_goal = goal

        self.publish_goal()
        self.publish_joint_efforts(tau)

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 handle_cart_cmd(self, msg):
        side = self.side
        try:
            # Get the current position of the hydra
            hydra_frame = fromTf(self.listener.lookupTransform(
                '/hydra_base',
                '/hydra_'+self.SIDE_STR[side]+'_grab',
                rospy.Time(0)))

            # Get the current position of the end-effector
            tip_frame = fromTf(self.listener.lookupTransform(
                '/world',
                self.tip_link,
                rospy.Time(0)))

            # Capture the current position if we're starting to move
            if not self.deadman_engaged:
                self.deadman_engaged = True
                self.cmd_origin = hydra_frame
                self.tip_origin = tip_frame
            else:
                self.deadman_max = max(self.deadman_max, msg.axes[self.DEADMAN[side]])
                # Update commanded TF frame
                cmd_twist = kdl.diff(self.cmd_origin, hydra_frame)
                cmd_twist.vel = self.scale*self.deadman_max*cmd_twist.vel
                self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
            rospy.logwarn(str(ex))

    def handle_cart_cmd(self, scaling):
        """"""

        try:
            # Get the current position of the hydra
            input_frame = fromTf(self.listener.lookupTransform(
                self.input_ref_frame_id,
                self.input_frame_id,
                rospy.Time(0)))

            # Get the current position of the end-effector
            tip_frame = fromTf(self.listener.lookupTransform(
                self.input_ref_frame_id,
                self.tip_link,
                rospy.Time(0)))

            # Capture the current position if we're starting to move
            if not self.deadman_engaged:
                self.deadman_engaged = True
                self.cmd_origin = input_frame
                self.tip_origin = tip_frame
            else:
                self.cart_scaling = scaling
                # Update commanded TF frame
                cmd_twist = kdl.diff(self.cmd_origin, input_frame)
                cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel
                #rospy.logwarn(cmd_twist)
                self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
            rospy.logwarn(str(ex))

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 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 _standing_still_for_x_seconds(self, timeout):
        """Check whether the robot is standing still for X seconds
        :param timeout how many seconds must the robot be standing still before returning True
        :type timeout float
        :returns bool indicating whether the robot has been standing still for longer than timeout seconds"""
        current_frame = self._robot.base.get_location().frame
        now = rospy.Time.now()

        if not self._last_pose_stamped:
            self._last_pose_stamped = current_frame
            self._last_pose_stamped_time = now
        else:
            current_yaw = current_frame.M.GetRPY()[2]  # Get the Yaw
            last_yaw = self._last_pose_stamped.M.GetRPY()[2]  # Get the Yaw

            # Compare the pose with the last pose and update if difference is larger than x
            if kdl.diff(current_frame.p, self._last_pose_stamped.p).Norm() > 0.05 or abs(current_yaw - last_yaw) > 0.3:
                # Update the last pose
                self._last_pose_stamped = current_frame
                self._last_pose_stamped_time = rospy.Time.now()
            else:

                print "Robot dit not move for x seconds: %f"%(now - self._last_pose_stamped_time).to_sec()

                # Check whether we passed the timeout
                if (now - self._last_pose_stamped_time).to_sec() > timeout:
                    return True
        return False