Spaces:
Sleeping
Sleeping
File size: 8,920 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 |
import numpy as np
from robosuite.controllers.base_controller import Controller
from robosuite.utils.buffers import RingBuffer
class JointVelocityController(Controller):
"""
Controller for controlling the robot arm's joint velocities. This is simply a P controller with desired torques
(pre gravity compensation) taken to be proportional to the velocity error of the robot joints.
NOTE: Control input actions assumed to be taken as absolute joint velocities. A given action to this
controller is assumed to be of the form: (vel_j0, vel_j1, ... , vel_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 list 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 list 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 list 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 list 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 list of float): velocity gain for determining desired torques based upon the joint vel errors.
Can be either be a scalar (same value for all action dims), or a list (specific values for each dim)
policy_freq (int): Frequency at which actions from the robot policy are fed into this controller
velocity_limits (2-list of float or 2-list of list of floats): Limits (m/s) below and above which the magnitude
of a calculated goal joint velocity 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 velocities
to the goal joint velocities 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
"""
def __init__(
self,
sim,
eef_name,
joint_indexes,
actuator_range,
input_max=1,
input_min=-1,
output_max=1,
output_min=-1,
kp=0.25,
policy_freq=20,
velocity_limits=None,
interpolator=None,
**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.joint_dim)
self.input_min = self.nums2array(input_min, self.joint_dim)
self.output_max = self.nums2array(output_max, self.joint_dim)
self.output_min = self.nums2array(output_min, self.joint_dim)
# gains and corresopnding vars
self.kp = self.nums2array(kp, self.joint_dim)
# if kp is a single value, map wrist gains accordingly (scale down x10 for final two joints)
if type(kp) is float or type(kp) is int:
# Scale kpp according to how wide the actuator range is for this robot
low, high = self.actuator_limits
self.kp = kp * (high - low)
self.ki = self.kp * 0.005
self.kd = self.kp * 0.001
self.last_err = np.zeros(self.joint_dim)
self.derr_buf = RingBuffer(dim=self.joint_dim, length=5)
self.summed_err = np.zeros(self.joint_dim)
self.saturated = False
self.last_joint_vel = np.zeros(self.joint_dim)
# limits
self.velocity_limits = np.array(velocity_limits) if velocity_limits is not None else None
# control frequency
self.control_freq = policy_freq
# interpolator
self.interpolator = interpolator
# initialize torques and goal velocity
self.goal_vel = None # Goal velocity desired, pre-compensation
self.current_vel = np.zeros(self.joint_dim) # Current velocity setpoint, pre-compensation
self.torques = None # Torques returned every time run_controller is called
def set_goal(self, velocities):
"""
Sets goal based on input @velocities.
Args:
velocities (Iterable): Desired joint velocities
Raises:
AssertionError: [Invalid action dimension size]
"""
# Update state
self.update()
# Otherwise, check to make sure velocities is size self.joint_dim
assert (
len(velocities) == self.joint_dim
), "Goal action must be equal to the robot's joint dimension space! Expected {}, got {}".format(
self.joint_dim, len(velocities)
)
self.goal_vel = self.scale_action(velocities)
if self.velocity_limits is not None:
self.goal_vel = np.clip(self.goal_vel, self.velocity_limits[0], self.velocity_limits[1])
if self.interpolator is not None:
self.interpolator.set_goal(self.goal_vel)
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_vel is None:
self.set_goal(np.zeros(self.joint_dim))
# Update state
self.update()
# Only linear interpolator is currently supported
if self.interpolator is not None:
if self.interpolator.order == 1:
# Linear case
self.current_vel = self.interpolator.get_interpolated_goal()
else:
# Nonlinear case not currently supported
pass
else:
self.current_vel = np.array(self.goal_vel)
# Compute necessary error terms for PID velocity controller
err = self.current_vel - self.joint_vel
derr = err - self.last_err
self.last_err = err
self.derr_buf.push(derr)
# Only add to I component if we're not saturated (anti-windup)
if not self.saturated:
self.summed_err += err
# Compute command torques via PID velocity controller plus gravity compensation torques
torques = self.kp * err + self.ki * self.summed_err + self.kd * self.derr_buf.average + self.torque_compensation
# Clip torques
self.torques = self.clip_torques(torques)
# Check if we're saturated
self.saturated = False if np.sum(np.abs(self.torques - torques)) == 0 else True
# Always run superclass call for any cleanups at the end
super().run_controller()
# Return final torques
return self.torques
def reset_goal(self):
"""
Resets joint velocity goal to be all zeros
"""
self.goal_vel = np.zeros(self.joint_dim)
# Reset interpolator if required
if self.interpolator is not None:
self.interpolator.set_goal(self.goal_vel)
@property
def name(self):
return "JOINT_VELOCITY"
|