Python PyKDL.addDelta方法代码示例

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [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 addDelta [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 addDelta [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 addDelta [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 addDelta [as 别名]
def derive_features(frame, feature_function, dd=0.01):
  ''' numerically derive the task angle function around the given frame.
      Also computes the inverse: this yields the instantaneous rotation axes
      for each angle.
  '''

  q0 = numpy.mat(feature_function(frame))
  size = q0.size
  print q0.size
  jac_i = numpy.mat(numpy.zeros((q0.size, 6)))

  for i in range(6):
    twist = kdl.Twist()
    twist[i] = 1.0
    frame_moved = kdl.addDelta(frame, twist, dd)
    q_i = numpy.mat( feature_function(frame_moved) )
    jac_i[0:size,i] = ((q_i - q0) / dd).T


  # the columns of jac are six twists to be displayed
  jac = numpy.linalg.pinv(jac_i)

  return (jac_i, jac)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [as 别名]
def compute_increment(goal_features, feature_set, frame, dd=0.1):
  tw = compute_direction(goal_features, feature_set, frame)
  print 'twist: '+ str(tw)
  return kdl.addDelta(frame, tw, dd)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [as 别名]
def pose_oplus(kdl_pose, step):
    t = PyKDL.Twist()
    for i in range(pose_width):
        t[i] = step[i,0]
    next_pose = kdl_pose * PyKDL.addDelta(PyKDL.Frame(), t, 1.0)
    return next_pose

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [as 别名]
def my_adddelta(frame, twist, dt):
    return (
        kdlframe_to_narray(
            PyKDL.addDelta(
                narray_to_kdlframe(frame), narray_to_kdltwist(twist), dt)))

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [as 别名]
  def find_checkerboard_service(self, req):
    poses = []
    min_x = self.ros_image.width
    min_y = self.ros_image.height
    max_x = 0
    max_y = 0
    for i in range(10):
        rospy.sleep(0.5)

        #copy the image over
        with self.mutex:
          image = self.ros_image

        result = self.find_checkerboard_pose(image, req.corners_x, 
                                                 req.corners_y, req.spacing_x, 
                                                 req.spacing_y, self.width_scaling, 
                                                 self.height_scaling)
        if result is None:
            rospy.logerr("Couldn't find a checkerboard")
            continue

        p, corners = result
        poses.append(p)
        for j in range(corners.cols):
            x = cv.GetReal2D(corners, 0, j)
            y = cv.GetReal2D(corners, 1, j)
            min_x = min(min_x, x)
            max_x = max(max_x, x)
            min_y = min(min_y, y)
            max_y = max(max_y, y)

    # convert all poses to twists
    twists = []
    for p in poses:
      twists.append(PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))

    # get average twist 
    twist_res = PyKDL.Twist()
    PyKDL.SetToZero(twist_res)
    for t in twists:
      for i in range(3):
        twist_res.vel[i] += t.vel[i]/len(poses)
        twist_res.rot[i] += t.rot[i]/len(poses)

    # get noise
    noise_rot = 0
    noise_vel = 0
    for t in twists:
      n_vel = (t.vel - twist_res.vel).Norm()
      n_rot = (t.rot - twist_res.rot).Norm()
      if n_vel > noise_vel:
        noise_vel = n_vel
      if n_rot > noise_rot:
        noise_rot = n_rot

    # set service resul
    pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
    pose_msg = PoseStamped()
    pose_msg.header = poses[0].header
    pose_msg.pose = posemath.toMsg(pose_res)
    return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot)

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [as 别名]
from geometry_msgs.msg import Twist

rospy.init_node("moveable_frame")

frame = kdl.Frame()
twist = kdl.Twist()


def callback(msg):
    global twist
    lin = kdl.Vector(msg.linear.x, msg.linear.y, msg.linear.z)
    rot = kdl.Vector(msg.angular.x, msg.angular.y, msg.angular.z)
    twist = kdl.Twist(lin, rot)


frame_name = rospy.get_param("~frame_name", "/frame")
parent_name = rospy.get_param("~parent_name", "/base_link")
f = rospy.get_param("~rate", 30)

rospy.Subscriber("twist", Twist, callback)
tf_pub = tf.TransformBroadcaster()

rate = rospy.Rate(f)

while not rospy.is_shutdown():
    t = kdl.Twist(twist)
    # frame = kdl.addDelta(frame, t, 1.0/f) # global mode
    frame = kdl.addDelta(frame, kdl.Frame(frame.M) * t, 1.0 / f)  # local mode
    tf_pub.sendTransform(frame.p, frame.M.GetQuaternion(), rospy.Time.now(), frame_name, parent_name)
    rate.sleep()

# 需要导入模块: import PyKDL [as 别名]
# 或者: from PyKDL import addDelta [as 别名]
    def execute_cb(self, goal):
        # helper variables
        success = True

        if self.robot_state.joint_impedance_active:
            self._result.error_code = 0
            self._as.set_succeeded(self._result)

        print "MoveCartesianTrajectory ", self._action_name, " points: ", len(goal.trajectory.points)

        init_T_B_W = self.robot_state.fk_solver.calculateFk(self.wrist_name, self.robot_state.getJsPos())
        init_T_B_T = init_T_B_W * self.robot_state.T_W_T[self.wrist_name]

        current_dest_point_idx = 0
        while True:

            if self._as.is_preempt_requested():
                rospy.loginfo("%s: Preempted" % self._action_name)
                self._as.set_preempted()
                return

            time_now = rospy.Time.now()
            time_from_start = (time_now - goal.trajectory.header.stamp).to_sec()
            if time_from_start <= 0:
                rospy.sleep(0.01)
                continue
            while time_from_start > goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec():
                current_dest_point_idx += 1
                if current_dest_point_idx >= len(goal.trajectory.points):
                    break

            if current_dest_point_idx >= len(goal.trajectory.points):
                break

            dest_time = goal.trajectory.points[current_dest_point_idx].time_from_start.to_sec()
            dest_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx].pose)

            if current_dest_point_idx > 0:
                prev_time = goal.trajectory.points[current_dest_point_idx - 1].time_from_start.to_sec()
                prev_T_B_T = pm.fromMsg(goal.trajectory.points[current_dest_point_idx - 1].pose)
            else:
                prev_time = 0.0
                prev_T_B_T = init_T_B_T

            f = (time_from_start - prev_time) / (dest_time - prev_time)
            if f < 0 or f > 1:
                print "error: MoveCartesianTrajectory f: ", str(f)

            diff_T_B_T = PyKDL.diff(prev_T_B_T, dest_T_B_T, 1.0)

            T_B_Ti = PyKDL.addDelta(prev_T_B_T, diff_T_B_T, f)

            T_B_Wi = T_B_Ti * self.robot_state.T_W_T[self.wrist_name].Inverse()
            q_out = self.robot_state.fk_solver.simulateTrajectory(self.wrist_name, self.robot_state.getJsPos(), T_B_Wi)
            if q_out == None:
                self._result.error_code = -3  # PATH_TOLERANCE_VIOLATED
                self._as.set_aborted(self._result)
                return

            for i in range(7):
                joint_name = self.robot_state.fk_solver.ik_joint_state_name[self.wrist_name][i]
                self.robot_state.js.position[self.robot_state.joint_name_idx_map[joint_name]] = q_out[i]

            self.robot_state.arm_cmd[self.wrist_name] = T_B_Ti

            # publish the feedback
            self._feedback.header.stamp = time_now
            self._feedback.desired = pm.toMsg(T_B_Ti)
            self._feedback.actual = pm.toMsg(T_B_Ti)
            self._feedback.error = pm.toMsg(PyKDL.Frame())
            self._as.publish_feedback(self._feedback)

            rospy.sleep(0.01)

        self._result.error_code = 0
        self._as.set_succeeded(self._result)