Contents

  1. Overview
  2. ROS API

Overview

The JinvTeleopController tracks cartesian setpoints using a Jacobian-inverse 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.
command_twist (geometry_msgs/TwistStamped)
  • Sets the desired twist for the controller (don't use together with command_pose).

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)
  • Amount of damping for the pseudo-inverse computation.
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).
k_posture (double, default: 1.0)
  • Gain for posture control.
joints/<joint>/p (double, default: 0.0)
  • Propertional gain for joint <joint>
joints/<joint>/d (double, default: 0.0)
  • Derivative gain for joint <joint>
joints/<joint>/p_clamp (double, default: 0.0)
  • The setpoint for <joint> is always clamped to within p_clamp of the actual joint position.

Example configuration:

r_cartesian_inv:
  type: JinvTeleopController7
  root_name: torso_lift_link
  tip_name: r_gripper_tool_frame
  k_posture: 20.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
  joints:
    r_shoulder_pan_joint:   {p: 2400.0, d: 18.0, ff: 10.0, p_clamp: 0.087}
    r_shoulder_lift_joint:  {p: 1200.0, d: 10.0, ff: 10.0, p_clamp: 0.087}
    r_upper_arm_roll_joint: {p: 1000.0, d: 6.0,  ff: 8.0,  p_clamp: 0.087}
    r_elbow_flex_joint:     {p: 700.0,  d: 4.0,  ff: 5.0,  p_clamp: 0.087}
    r_forearm_roll_joint:   {p: 300.0,  d: 6.0,  ff: 5.0,  p_clamp: 0.087}
    r_wrist_flex_joint:     {p: 300.0,  d: 4.0,  ff: 2.0,  p_clamp: 0.087}
    r_wrist_roll_joint:     {p: 300.0,  d: 4.0,  ff: 2.0,  p_clamp: 0.087}
  vel_saturation_trans: 1.0
  vel_saturation_rot: -1

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

Wiki: teleop_controllers/JinvTeleopController (last edited 2012-04-16 16:58:39 by John Hoare)