Spaces:
Sleeping
Sleeping
File size: 13,934 Bytes
96da58e |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 |
from typing import Dict, List, Literal
import numpy as np
from robosuite.controllers.base_controller import Controller
from robosuite.utils.control_utils import *
# Supported impedance modes
IMPEDANCE_MODES = {"fixed", "variable", "variable_kp"}
class JointPositionController(Controller):
"""
Controller for controlling robot arm via impedance control. Allows position control of the robot's joints.
NOTE: Control input actions assumed to be taken relative to the current joint positions. A given action to this
controller is assumed to be of the form: (dpos_j0, dpos_j1, ... , dpos_jn-1) for an n-joint robot
Args:
sim (MjSim): Simulator instance this controller will pull robot state updates from
eef_name (str): Name of controlled robot arm's end effector (from robot XML)
joint_indexes (dict): Each key contains sim reference indexes to relevant robot joint information, namely:
:`'joints'`: list of indexes to relevant robot joints
:`'qpos'`: list of indexes to relevant robot joint positions
:`'qvel'`: list of indexes to relevant robot joint velocities
actuator_range (2-tuple of array of float): 2-Tuple (low, high) representing the robot joint actuator range
input_max (float or Iterable of float): Maximum above which an inputted action will be clipped. Can be either be
a scalar (same value for all action dimensions), or a list (specific values for each dimension). If the
latter, dimension should be the same as the control dimension for this controller
input_min (float or Iterable of float): Minimum below which an inputted action will be clipped. Can be either be
a scalar (same value for all action dimensions), or a list (specific values for each dimension). If the
latter, dimension should be the same as the control dimension for this controller
output_max (float or Iterable of float): Maximum which defines upper end of scaling range when scaling an input
action. Can be either be a scalar (same value for all action dimensions), or a list (specific values for
each dimension). If the latter, dimension should be the same as the control dimension for this controller
output_min (float or Iterable of float): Minimum which defines upper end of scaling range when scaling an input
action. Can be either be a scalar (same value for all action dimensions), or a list (specific values for
each dimension). If the latter, dimension should be the same as the control dimension for this controller
kp (float or Iterable of float): positional gain for determining desired torques based upon the joint pos error.
Can be either be a scalar (same value for all action dims), or a list (specific values for each dim)
damping_ratio (float or Iterable of float): used in conjunction with kp to determine the velocity gain for
determining desired torques based upon the joint pos errors. Can be either be a scalar (same value for all
action dims), or a list (specific values for each dim)
impedance_mode (str): Impedance mode with which to run this controller. Options are {"fixed", "variable",
"variable_kp"}. If "fixed", the controller will have fixed kp and damping_ratio values as specified by the
@kp and @damping_ratio arguments. If "variable", both kp and damping_ratio will now be part of the
controller action space, resulting in a total action space of num_joints * 3. If "variable_kp", only kp
will become variable, with damping_ratio fixed at 1 (critically damped). The resulting action space will
then be num_joints * 2.
kp_limits (2-list of float or 2-list of Iterable of floats): Only applicable if @impedance_mode is set to either
"variable" or "variable_kp". This sets the corresponding min / max ranges of the controller action space
for the varying kp values. Can be either be a 2-list (same min / max for all kp action dims), or a 2-list
of list (specific min / max for each kp dim)
damping_ratio_limits (2-list of float or 2-list of Iterable of floats): Only applicable if @impedance_mode is
set to "variable". This sets the corresponding min / max ranges of the controller action space for the
varying damping_ratio values. Can be either be a 2-list (same min / max for all damping_ratio action dims),
or a 2-list of list (specific min / max for each damping_ratio dim)
policy_freq (int): Frequency at which actions from the robot policy are fed into this controller
qpos_limits (2-list of float or 2-list of Iterable of floats): Limits (rad) below and above which the magnitude
of a calculated goal joint position will be clipped. Can be either be a 2-list (same min/max value for all
joint dims), or a 2-list of list (specific min/max values for each dim)
interpolator (Interpolator): Interpolator object to be used for interpolating from the current joint position to
the goal joint position during each timestep between inputted actions
**kwargs: Does nothing; placeholder to "sink" any additional arguments so that instantiating this controller
via an argument dict that has additional extraneous arguments won't raise an error
Raises:
AssertionError: [Invalid impedance mode]
"""
def __init__(
self,
sim,
eef_name,
joint_indexes,
actuator_range,
input_max=1,
input_min=-1,
output_max=0.05,
output_min=-0.05,
kp=50,
damping_ratio=1,
impedance_mode="fixed",
kp_limits=(0, 300),
damping_ratio_limits=(0, 100),
policy_freq=20,
qpos_limits=None,
interpolator=None,
input_type: Literal["delta", "absolute"] = "delta",
**kwargs, # does nothing; used so no error raised when dict is passed with extra terms used previously
):
super().__init__(
sim,
eef_name,
joint_indexes,
actuator_range,
)
# Control dimension
self.control_dim = len(joint_indexes["joints"])
# input and output max and min (allow for either explicit lists or single numbers)
self.input_max = self.nums2array(input_max, self.control_dim)
self.input_min = self.nums2array(input_min, self.control_dim)
self.output_max = self.nums2array(output_max, self.control_dim)
self.output_min = self.nums2array(output_min, self.control_dim)
# limits
self.position_limits = np.array(qpos_limits) if qpos_limits is not None else qpos_limits
# kp kd
self.kp = self.nums2array(kp, self.control_dim)
self.kd = 2 * np.sqrt(self.kp) * damping_ratio
# kp and kd limits
self.kp_min = self.nums2array(kp_limits[0], self.control_dim)
self.kp_max = self.nums2array(kp_limits[1], self.control_dim)
self.damping_ratio_min = self.nums2array(damping_ratio_limits[0], self.control_dim)
self.damping_ratio_max = self.nums2array(damping_ratio_limits[1], self.control_dim)
# Verify the proposed impedance mode is supported
assert impedance_mode in IMPEDANCE_MODES, (
"Error: Tried to instantiate OSC controller for unsupported "
"impedance mode! Inputted impedance mode: {}, Supported modes: {}".format(impedance_mode, IMPEDANCE_MODES)
)
# Impedance mode
self.impedance_mode = impedance_mode
# Add to control dim based on impedance_mode
if self.impedance_mode == "variable":
self.control_dim *= 3
elif self.impedance_mode == "variable_kp":
self.control_dim *= 2
# control frequency
self.control_freq = policy_freq
# interpolator
self.interpolator = interpolator
self.input_type = input_type
print(f"Input type: {self.input_type}")
assert self.input_type in ["delta", "absolute"], f"Input type must be delta or absolute, got: {self.input_type}"
if self.input_type == "absolute":
assert self.impedance_mode == "fixed", "Absolute input type is only supported for fixed impedance mode."
# initialize
self.goal_qpos = None
def set_goal(self, action, set_qpos=None):
"""
Sets goal based on input @action. If self.impedance_mode is not "fixed", then the input will be parsed into the
delta values to update the goal position / pose and the kp and/or damping_ratio values to be immediately updated
internally before executing the proceeding control loop.
Note that @action expected to be in the following format, based on impedance mode!
:Mode `'fixed'`: [joint pos command]
:Mode `'variable'`: [damping_ratio values, kp values, joint pos command]
:Mode `'variable_kp'`: [kp values, joint pos command]
Args:
action (Iterable): Desired relative joint position goal state
set_qpos (Iterable): If set, overrides @action and sets the desired absolute joint position goal state
Raises:
AssertionError: [Invalid action dimension size]
"""
# Update state
self.update()
if self.input_type == "delta":
# Parse action based on the impedance mode, and update kp / kd as necessary
jnt_dim = len(self.qpos_index)
if self.impedance_mode == "variable":
damping_ratio, kp, delta = action[:jnt_dim], action[jnt_dim : 2 * jnt_dim], action[2 * jnt_dim :]
self.kp = np.clip(kp, self.kp_min, self.kp_max)
self.kd = 2 * np.sqrt(self.kp) * np.clip(damping_ratio, self.damping_ratio_min, self.damping_ratio_max)
elif self.impedance_mode == "variable_kp":
kp, delta = action[:jnt_dim], action[jnt_dim:]
self.kp = np.clip(kp, self.kp_min, self.kp_max)
self.kd = 2 * np.sqrt(self.kp) # critically damped
else: # This is case "fixed"
delta = action
# Check to make sure delta is size self.joint_dim
assert len(delta) == jnt_dim, "Delta qpos must be equal to the robot's joint dimension space!"
if delta is not None:
scaled_delta = self.scale_action(delta)
else:
scaled_delta = None
self.goal_qpos = set_goal_position(
scaled_delta, self.joint_pos, position_limit=self.position_limits, set_pos=set_qpos
)
elif self.input_type == "absolute":
self.goal_qpos = action
if self.interpolator is not None:
self.interpolator.set_goal(self.goal_qpos)
def run_controller(self):
"""
Calculates the torques required to reach the desired setpoint
Returns:
np.array: Command torques
"""
# Make sure goal has been set
if self.goal_qpos is None:
self.set_goal(np.zeros(self.control_dim))
# Update state
self.update()
desired_qpos = None
# Only linear interpolator is currently supported
if self.interpolator is not None:
# Linear case
if self.interpolator.order == 1:
desired_qpos = self.interpolator.get_interpolated_goal()
else:
# Nonlinear case not currently supported
pass
else:
desired_qpos = np.array(self.goal_qpos)
# torques = pos_err * kp + vel_err * kd
position_error = desired_qpos - self.joint_pos
vel_pos_error = -self.joint_vel
desired_torque = np.multiply(np.array(position_error), np.array(self.kp)) + np.multiply(vel_pos_error, self.kd)
# Return desired torques plus gravity compensations
self.torques = np.dot(self.mass_matrix, desired_torque) + self.torque_compensation
# Always run superclass call for any cleanups at the end
super().run_controller()
# print(f"current qpos: {self.joint_pos}")
# print(f"desired qpos: {desired_qpos}")
return self.torques
def reset_goal(self):
"""
Resets joint position goal to be current position
"""
self.goal_qpos = self.joint_pos
# Reset interpolator if required
if self.interpolator is not None:
self.interpolator.set_goal(self.goal_qpos)
@property
def control_limits(self):
"""
Returns the limits over this controller's action space, overrides the superclass property
Returns the following (generalized for both high and low limits), based on the impedance mode:
:Mode `'fixed'`: [joint pos command]
:Mode `'variable'`: [damping_ratio values, kp values, joint pos command]
:Mode `'variable_kp'`: [kp values, joint pos command]
Returns:
2-tuple:
- (np.array) minimum action values
- (np.array) maximum action values
"""
if self.impedance_mode == "variable":
low = np.concatenate([self.damping_ratio_min, self.kp_min, self.input_min])
high = np.concatenate([self.damping_ratio_max, self.kp_max, self.input_max])
elif self.impedance_mode == "variable_kp":
low = np.concatenate([self.kp_min, self.input_min])
high = np.concatenate([self.kp_max, self.input_max])
else: # This is case "fixed"
low, high = self.input_min, self.input_max
return low, high
@property
def name(self):
return "JOINT_POSITION" |