JTTeleopController

Overview

The JTTeleopController tracks cartesian setpoints using a Jacobian-transpose control law with redundancy resolution.

ROS API

Subscribed Topics

command_pose (geometry_msgs/PoseStamped)
  • The pose to track. The controller will transform the pose into its root frame and then attempt to control the arm to the pose.
command_posture (std_msgs/Float64MultiArray)
  • The desired posture of the arm, given as a vector of joint positions. Give a value of 9999 for joints that shouldn't be included as part of the posture.
gains (std_msgs/Float64MultiArray)
  • Sets the cartesian gains for the arm. The first six values should be the proportional cartesian gains (translational x, y, z, then rotational x, y, z). The (optional) next six values are the derivative cartesian gains.

Parameters

root_name (string, default: Required)
  • The name of the root link of the chain of joints
tip_name (string, default: Required)
  • The name of the tip link of the chain of joints
cart_gains/trans/p (double, default: Required)
  • Proportional gain for position
cart_gains/trans/d (double, default: Required)
  • Derivative gain for position
cart_gains/rot/p (double, default: Required)
  • Proportional gain for cartesian orientation
cart_gains/rot/d (double, default: Required)
  • Derivative gain for cartesian orientation
jacobian_inverse_damping (double, default: 0.0)
  • The redundancy resolution depends on computing the inverse of the Jacobian. This parameter allows tuning of the damping of the pseudoinverse computation.
joint_vel_filter (double, default: 1.0)
  • Low pass filter on the incoming joint velocities. A value of 1.0 means no filtering (only use the latest value).
pose_command_filter (double in (0,1], default: 1.0)
  • Low-pass filter on the incoming poses. A value of 1 means don't filter (only use the most recent command).
vel_saturation_trans (double, default: 0.0)
  • Maximum translational velocity. Commanded joint efforts are limited if the velocity exceeds this value. A value of 0.0 means no saturation.
vel_saturation_rot (double, default: 0.0)
  • Maximum rotational velocity. Commanded joint efforts are limited if the velocity exceeds this value. A value of 0.0 means no saturation.
k_posture (double, default: 1.0)
  • Gain for posture control.
saturation/<joint> (double, default: 0.0)
  • Maximum joint effort for the joint named <joint>. If the desired effort exceeds this amount, the controller will scale back the force at the end-effector (thereby scaling the effort for all joints).
joint_feedforward/<joint> (double, default: 0.0)
  • Feedforward gain for the joint, used for converting desired joint accelerations into effort. This gain is used in controlling the redundancy.

Example configuration:

r_arm_cartesian:
  type: JTTeleopController
  root_name: torso_lift_link
  tip_name: r_gripper_tool_frame
  k_posture: 25.0
  jacobian_inverse_damping: 0.01
  pose_command_filter: 0.01
  cart_gains:
    trans:
      p: 800.0
      d: 15.0
    rot:
      p: 80.0
      d: 1.2
  joint_feedforward:
    r_shoulder_pan_joint: 3.33
    r_shoulder_lift_joint: 1.16
    r_upper_arm_roll_joint: 0.1
    r_elbow_flex_joint: 0.25
    r_forearm_roll_joint: 0.133
    r_wrist_flex_joint: 0.0727
    r_wrist_roll_joint: 0.0727
  joint_max_effort:
    r_shoulder_pan_joint: 11.88
    r_shoulder_lift_joint: 11.64
    r_upper_arm_roll_joint: 6.143
    r_elbow_flex_joint: 6.804
    r_forearm_roll_joint: 8.376
    r_wrist_flex_joint: 5.568
    r_wrist_roll_joint: 5.568
  vel_saturation_trans: 2.0
  vel_saturation_rot: 4.0

The JTTeleopController also publishes information on various topics under "state".

Wiki: teleop_controllers/JTTeleopController (last edited 2010-06-02 23:41:03 by StuartGlaser)