Python PyKDL.diff方法代码示例

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

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

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

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

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

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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import diff [as 别名]
    def determine_points_of_interest(self, center_frame, z_max, convex_hull):
        """
        Determine candidates for place poses
        :param center_frame: kdl.Frame, center of the Entity to place on top of
        :param z_max: float, height of the entity to place on, w.r.t. the entity
        :param convex_hull: [kdl.Vector], convex hull of the entity
        :return: [FrameStamped] of candidates for placing
        """

        points = []

        if len(convex_hull) == 0:
            rospy.logerr('determine_points_of_interest: Empty convex hull')
            return []

        # Convert convex hull to map frame
        ch = offsetConvexHull(convex_hull, center_frame)

        # Loop over hulls
        self.marker_array.markers = []

        for i in xrange(len(ch)):
            j = (i + 1) % len(ch)

            dx = ch[j].x() - ch[i].x()
            dy = ch[j].y() - ch[i].y()

            length = kdl.diff(ch[j], ch[i]).Norm()

            d = self._edge_distance
            while d < (length - self._edge_distance):
                # Point on edge
                xs = ch[i].x() + d / length * dx
                ys = ch[i].y() + d / length * dy

                # Shift point inwards and fill message
                fs = kdl_frame_stamped_from_XYZRPY(x=xs - dy / length * self._edge_distance,
                                                   y=ys + dx / length * self._edge_distance,
                                                   z=center_frame.p.z() + z_max,
                                                   frame_id="/map")

                # It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might
                # prevent the robot from hitting the cabinet edges
                # print "Length: {}, edge score: {}".format(length, min(d, length-d))
                setattr(fs, 'edge_score', min(d, length-d))

                points += [fs]

                self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z()))

                # ToDo: check if still within hull???
                d += self._spacing

        self.marker_pub.publish(self.marker_array)

        return points

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

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import diff [as 别名]
    def checkGoal(self, q):
        # interpolate trajectory (in the cartesian space)
        self.openrave.robot_rave.SetDOFValues(q, self.dof_indices)
        link_E = self.openrave.robot_rave.GetLink("right_HandPalmLink")
        T_World_E = conv.OpenraveToKDL(link_E.GetTransform())
        T_B_E = self.openrave.T_World_Br.Inverse() * T_World_E

        for T_B_Eg in self.goals_T_B_E:
            diff = PyKDL.diff(T_B_Eg, T_B_E)
            if diff.vel.Norm() <= 0.02 and diff.rot.Norm() <= 10.0/180.0*math.pi:
                return True

        return False

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import diff [as 别名]
 def setParams(self, param):
     '''
     0-15: homogeneous goal matrix
     16: slow down distance
     '''
     gpos = matrix([param[0:4], param[4:8], param[8:12], param[12:16]])
     self.gp = gpos[0:3, 3].T.A[0]
     self.dToAttractor = length(self.p - self.gp)
     self.slowDownDistance = param[16]
     for i in range(3):
         for j in range(3):
             self.gframe.M[i, j] = gpos[i, j]
     self.tw = kdl.diff(self.frame, self.gframe)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import diff [as 别名]
    def within_tolerance(self, tip_pose, target_pose, scale=1.0):
        # compute cartesian command error
        tip_frame = fromTf(tip_pose)
        target_frame = fromTf(target_pose)

        twist_err = kdl.diff(tip_frame, target_frame)
        linear_err = twist_err.vel.Norm()
        angular_err = twist_err.rot.Norm()

        #print("linear: %g, angular %g" % (linear_err, angular_err))

        # decide if planning is needed
        return linear_err < scale*self.linear_err_threshold and angular_err < scale*self.angular_err_threshold

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import diff [as 别名]
 def getMovementTime2(self, T_B_Wd1, T_B_Wd2, max_v_l = 0.1, max_v_r = 0.2):
     twist = PyKDL.diff(T_B_Wd1, T_B_Wd2, 1.0)
     v_l = twist.vel.Norm()
     v_r = twist.rot.Norm()
     print "v_l: %s   v_r: %s"%(v_l, v_r)
     f_v_l = v_l/max_v_l
     f_v_r = v_r/max_v_r
     if f_v_l > f_v_r:
         duration = f_v_l
     else:
         duration = f_v_r
     if duration < 0.5:
         duration = 0.5
     return duration

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import diff [as 别名]
    def getMovementTime(self, T_B_Wd, max_v_l = 0.1, max_v_r = 0.2):
        self.updateTransformations()
        twist = PyKDL.diff(self.T_B_W, T_B_Wd, 1.0)
        v_l = twist.vel.Norm()
        v_r = twist.rot.Norm()
#        print "v_l: %s   v_r: %s"%(v_l, v_r)
        f_v_l = v_l/max_v_l
        f_v_r = v_r/max_v_r
        if f_v_l > f_v_r:
            duration = f_v_l
        else:
            duration = f_v_r
        if duration < 0.2:
            duration = 0.2
        return duration

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