Controllers¶
Adapted with permission from robosuite.ai
Controllers are used to determine the type of high-level control over a given robot arm. While all arms are directly controlled via their joint torques, the inputted action space for a given environment can vary depending on the type of desired control. Our controller options include EEImpedance, EEPosture, JointImpedance, JointVelocity, and JointTorque.
For research purposes, robotic control is the action-space of robotic learning. The controller libraries exist to unify controllers across different experiments and robot configurations. They also create transparency for ease of debugging. All controllers in this use joint torque as an output control command. Because of the nature of torque control, control commands should be sent to the robot at a high frequency (>100 Hz, preferably 500-1000Hz).
Because dynamics differs between simulation and reality, it is possible to specify different gains for simulated robots and real robots.
Config¶
Controller type are specific to a robot and are set in the project config yaml file with the following:
panda_controller:
selected_type: 'JointTorque' # Choose between 'EEImpedance', 'JointVelocity'
Bullet:
EEImpedance:
kp: [65.0, 65.0, 65.0, 10., 10., 10.]
damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1]
input_max: 1.0
input_min: -1.0
output_max: 1.0
output_min: -1.0
JointVelocity:
kv: 2.0
input_max: 1.0
input_min: -1.0
output_max: 1.0 #1.0
output_min: -1.0 #-1.0
JointImpedance:
kp: 60
damping: 1.0
input_max: 1.0
input_min: -1.0
output_max: 1.0
output_min: -1.0
JointTorque:
input_max: 1.0
input_min: -1.0
output_max: 5.0
output_min: -5.0
EEPosture:
kp: 60 # P Gain for Impedance Control
damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1]
posture_gain: 60
posture: [0,-1.18,0.00,2.18,0.00,0.57,3.3161]
input_max: 1.0
input_min: -1.0
output_max: 1.0
output_min: -1.0
Real:
EEImpedance:
kp: [40, 40, 40, 5.0, 5.0, 3.0] # P Gain for Impedance Control
kv: [10.0, 10.0, 10.0, 1.0, 1.0, 1.7]
damping: 1.0 # Damping factor [0,1]
input_max: 1.0
input_min: -1.0
output_max: 1.0
output_min: -1.0
JointVelocity:
kv: 10.0
input_max: 1.0
input_min: -1.0
output_max: 1.0 #1.0
output_min: -1.0 #-1.0
JointImpedance:
kp: [20.0, 40.0, 20.0, 40.0, 10.0, 12.0, 4.0]
kv: [2.0, 3.0, 2.0, 2.0, 2.0, 2.0, 0.2]
damping: 1.0
input_max: 1.0
input_min: -1.0
output_max: 1.0
output_min: -1.0
JointTorque:
input_max: 1.0
input_min: -1.0
output_max: 5.0
output_min: -5.0
EEPosture:
kp: [40, 40, 40, 5.0, 5.0, 3.0]
kv: [10.0, 10.0, 10.0, 1.0, 1.0, 1.7]
damping: 1.0 # Damping factor [0,1]
posture_gain: [5.0, 5.0, 5.0, 4.0, 3.0, 3.0, 3.0]
input_max: 1.0
input_min: -1.0
output_max: 1.0
output_min: -1.0
interpolator_pos:
type: 'linear'
order: 1
max_dx: 0.2
ramp_ratio: 0.2
interpolator_ori:
type: 'linear'
fraction: 0.2
The selected_type determines which of the following controllers to use.
``EEImpedance`` (ee_imp.py): an end effector impedance controller that takes a desired end effector state in cartesian space. Typically used for interaction and contact-rich applications.Interpolators may be defined for end effector position and orientation in the controller definition. However, only end effector position interpolation is recommended at this time.
*JointImpedance (joint_imp.py): a joint-space impedance controller that takes a desired joint position and velocity as an input in joint space. Typically used for trajectory-following and singularity avoidance.
*JointVelocity (joint_vel.py): a joint-velocity controller that takes a desired joint velocity in joint space. Typically used for visual servoing, teleoperation.
*JointTorque (joint_torque.py): a joint-torque controller that directly feeds joint torques through to the robot. Typically used for end-to-end learning or custom controllers applications.
Interpolators:¶
Controllers are only tuned to provide good performance at small displacements from the current state. Interpolators are used to smooth commands sent to the controllers. This is important as interpolation occurs between consecutive goal commands, rather than between the current state and goal state. This may result in non-markovian behavior, as the next action is dependent on the previous action commanded. We interpolate the goals rather than the current states to produce a smooth trajectory for the robot.
Interpolators provide intermediate desired robot states that the controller than uses to produce control commands. The controllers in perls2 are tuned to a maximum displacement of 0.1 for end effector position and orientation; joint position, velocity and torque.
The available interpolators are: ``linear``: provides a sequence of way points for the controller to reach as a linear function between the current robot state and desired state.None: controller uses no interpolation (not recommended unless
Orientation interpolation.¶
Interpolating orientations requires a different framework compared to positions. Orientations are specified using axis-angel representation. These goals are converted to quaternions and the interpolator uses SLERP to interpolate between goals. Orientation interpolation is recommended for use in EEImpedance and EEPosture controllers.
The config file specifications for orientation interpolation are:
controller: interp_ori: type: linear fraction: 0.2
where fraction refers to the fraction of control steps per action (typically 25 for control frequencies of 500 Hz and Policy frequencies of 20Hz) during which to interpolate.
Robot Model¶
The Model class (robot_model/model.py) captures the current state of the robot that the controller uses to output a control command. They are updated by RobotInterface objects.
Controllers¶
During runtime, the execution of the controllers is as follows. Controllers receive a desired configuration (reference value) and output joint torques that try to minimize the error between the current configuration and the desired one. Policies and solutions provide these desired configurations, elements of some action space, at what we call policy frequency (\(f_{p}\)), e.g., 20Hz or 30Hz. perls2 will execute several iterations composed of a controller execution and a simulation step at simulation frequency, \(f_s\) (\(f_s = N\cdot f_p\)), using the same reference signal until a new action is provided by the policy/solution. In these iterations, while the desired configuration is kept constant, the current state of the robot is changing, and thus, the error.
In the following we summarize the options, variables, and the control laws (equations) that convert desired values from the policy/solution and current robot states into executable joint torques to minimize the difference.
Joint Space Control - Torque¶
Controller Type: JointTorque
Action Dimensions (not including gripper): n (number of joints)
Since our controllers transform the desired values from the policies/solutions into joint torques, if these values are already joint torques, there is a one-to-one mapping between the reference value from the policy/solution and the output value from the joint torque controller at each step: \(\tau = \tau_d\) \begin{equation} \tau = \tau_d \end{equation}
Joint Space Control - Velocity¶
Controller Type: JointVelocity
Action Dimensions (not including gripper): n (number of joints)
To control joint velocities, we create a proportional (P) control law between the desired value provided by the policy/solution (interpreted as desired velocity of each joint) and the current joint velocity of the robot. This control law, parameterized by a proportional constant, \(k_p\), generates joint torques to execute at each simulation step:
\begin{equation} \tau = k_p (\dot{q}_d - \dot{q}) \end{equation}
Joint Space Control - Position with Fixed Impedance¶
Controller Type: JointImpedance
Impedance: fixed
Action Dimensions (not including gripper): n (number of joints)
In joint position control, we create a proportional-derivative (PD) control law between the desired value provided by the policy/solution (interpreted as desired configuration for each joint) and the current joint positions of the robot. The control law that generates the joint torques to execute is parameterized by proportional and derivative gains, \(k_p\) and \(k_v\), and defined as
\begin{equation} \tau = \Lambda \left[k_p \Delta_q - k_d\dot{q}\right] \end{equation}
where \(\Delta_q = q_d - q\) is the difference between current and desired joint configurations, and \(\Lambda\) is the inertia matrix, that we use to scale the error to remove the dynamic effects of the mechanism. The stiffness and damping parameters, \(k_p\) and \(k_d\), are determined in construction and kept fixed.
Operational Space Control - Pose with Fixed Impedance¶
Controller Type: EEImpedance
Impedance: fixed
Action Dimensions (not including gripper): 6
In the OSC_POSE controller, the desired value is the 6D pose (position and orientation) of a controlled frame. We follow the formalism from [Khatib87]. Our control frame is always the eef_site defined in the gripper model, placed at the end of the last link for robots without gripper or between the fingers for robots with gripper. The operational space control framework (OSC) computes the necessary joint torques to minimize the error
between the desired and the current pose of the eef_site with the minimal kinematic energy.
Given a desired pose \(\mathbf{x}_{\mathit{des}}\) and the current end-effector pose, , we first compute the end-effector acceleration that would help minimize the error between both, assumed. PD (proportional-derivative) control schema to improve convergence and stability. For that, we first decompose into a desired position, \(p_d \in \mathbb{R}^3\), and a desired orientation, \(R_d \in \mathbb{SO}(3)\). The end-effector acceleration to minimize the error should increase with the difference between desired end-effector pose and current pose, \(p\) and \(R\) (proportional term), and decrease with the current end-effector velocity, \(v\) and \(\omega\) (derivative term).
We then compute the robot actuation (joint torques) to achieve the desired end-effector space accelerations leveraging the kinematic and dynamic models of the robot with the dynamically-consistent operational space formulation in [Khatib1995a]. First, we compute the wrenches at the end-effector that corresponds to the desired accelerations, \({f}\in\mathbb{R}^{6}\). Then, we map the wrenches in end-effector space \({f}\) to joint torque commands with the end-effector Jacobian at the current joint configuration \(J=J(q)\): \(\tau = J^T{f}\).
Thus, the function that maps end-effector space position and orientation to low-level robot commands is (\(\textrm{ee} = \textrm{\it end-effector space}\)): \begin{equation} \begin{aligned} \tau &= J_p[\Lambda_p[k_p^p (p_d - p) - k_v^p v]] + J_R[\Lambda_R\left[k_p^R(R_d \ominus R) - k_d^R \omega \right]] \end{aligned} \end{equation} where \(\Lambda_p\) and \(\Lambda_R\) are the parts corresponding to position and orientation in \(\Lambda \in \mathbb{R}^{6\times6}\), the inertial matrix in the end-effector frame that decouples the end-effector motions, \(J_p\) and \(J_R\) are the position and orientation parts of the end-effector Jacobian, and \(\ominus\) corresponds to the subtraction in \(\mathbb{SO}(3)\). The difference between current and desired position (\(\Delta_p= p_d - p\)) and between current and desired orientation (\(\Delta_R = R_d \ominus R\)) can be used as alternative policy action space, \(\mathcal{A}\). \(k_p^p\), \(k_p^d\), \(k_p^R\), and \(k_d^R\) are vectors of proportional and derivative gains for position and orientation (parameters \(\kappa\)), respectively, set once at initialization and kept fixed.
Operational Space Control - Pose with nullspace posture control¶
Controller Type: EEPosture
Impedance: Fixed
Action Dimensions (not including gripper): 6, posture given at initialization.
\begin{equation} \begin{aligned} \tau &= J_p[\Lambda_p[k_p^p (p_d - p) - k_v^p v]] + J_R[\Lambda_R\left[k_p^R(R_d \ominus R) - k_d^R \omega \right]] + [\mathcal{N}(k_p * (q_{desired}- q))] \end{aligned} \end{equation}
where $:nbsphinx-math:mathcal{N} is the nullspace of the task given by:
\begin{equation} \begin{aligned} \mathcal{N} &= I - M^{-1}J^{T}\Lambda J \end{aligned} \end{equation}
The control law is the same as EEImpedance but, in this case the posture, or joint position of the robot provides an additional constraint. The torques required to maintain the joint positions are mapped to the nullspace of the taskspace and therefore do not interfere with the robot’s ability to acheive the desired goal.
Configurations¶
The config directory (found within the controller module) provides a set of default configuration files that hold default examples of parameters relevant to individual controllers. Note that when creating your controller config templates of a certain type of controller, the listed parameters in the default example are required and should be specified accordingly.
Note: Each robot has its own default controller configuration which is called by default unless a different controller config is called.
Below, a brief overview and description of each subset of controller parameters are shown:
type: Type of controller to control. Can beEEPosture,EEImpedance,JointImpedance,JointVelocity,JointTorqueinterpolator_pos: If notnull, specified type of interpolation to use between desired actions. Currently onlylinearis supported.interpolator_ori: If notnull, specified type of interpolation to use between desired actions. Currently onlylinearis supported.ramp_ratio: If usinglinearinterpolation, specifies the proportion of allotted timesteps (value from [0, 1]) over which to execute the interpolated commands.{...}_limits: Limits for that specific controller. E.g.: for aJointImpedance, the relevant limits are its joint positions,qpos_limits. Can be either a 2-element list (same min/max limits across entire relevant space), or a list of lists (specific limits for each component){input,output}_{min,max}: Scaling ranges for mapping action space inputs into controller inputs. Settings these limits will automatically clip the action space input to be within theinput_{min,max}before mapping the requested value into the specifiedoutput_{min,max}range. Can be either a scalar (same limits across entire action space), or a list (specific limits for each action component)kp: Where relevant, specifies the proportional gain for the controller. Can be either be a scalar (same value for all controller dimensions), or a list (specific values for each dimension)uncouple_pos_ori: Only relevant forOSC_POSE.truedecouples the desired position and orientation torques when executing the controller