Python PyKDL.dot方法代码示例

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
    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)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
    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)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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])

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
 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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
 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)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
 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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
  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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import dot [as 别名]
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