def __init__(self, chain=None):
DynamixelHelloXL430.__init__(self, 'wrist_yaw', chain)
self.poses = {
'side': deg_to_rad(90.0),
'forward': deg_to_rad(0.0),
'stow': deg_to_rad(180.0)
}
def __init__(self, chain=None):
DynamixelHelloXL430.__init__(self, 'stretch_gripper', chain)
self.status['pos_pct']= 0.0
self.pct_max_open=self.world_rad_to_pct(self.ticks_to_world_rad(self.params['range_t'][1])) #May be a bit greater than 50 given non-linear calibration
self.poses = {'zero': 0,
'open': self.pct_max_open,
'close': -100}
def move_to(self,pct, v_r=None, a_r=None):
"""
pct: commanded absolute position (Pct).
v_r: velocity for trapezoidal motion profile (rad/s).
a_r: acceleration for trapezoidal motion profile (rad/s^2)
"""
x_r=self.pct_to_world_rad(pct)
DynamixelHelloXL430.move_to(self,x_des=x_r, v_des=v_r, a_des=a_r)
def __init__(self, chain=None):
DynamixelHelloXL430.__init__(self, 'wrist_roll', chain)
self.logger = logging.getLogger('robot.wrist_roll')
self.poses = {
'cw_90': deg_to_rad(90.0),
'forward': deg_to_rad(0.0),
'ccw_90': deg_to_rad(-90.0)
}
self.sentry_active = False
def __init__(self, chain=None, verbose=False):
DynamixelHelloXL430.__init__(self, 'wrist_yaw', chain, verbose=verbose)
self.logger = logging.getLogger('robot.wrist_yaw')
self.poses = {
'side': deg_to_rad(90.0),
'forward': deg_to_rad(0.0),
'stow': deg_to_rad(180.0)
}
self.sentry_active = False
def __init__(self, chain=None):
DynamixelHelloXL430.__init__(self, 'wrist_pitch', chain)
self.logger = logging.getLogger('robot.wrist_pitch')
self.poses = {
'up': deg_to_rad(56.0),
'forward': deg_to_rad(0.0),
'down': deg_to_rad(-90.0)
}
self.sentry_active = False
def step_sentry(self, robot):
"""This sentry attempts to prevent the gripper servo from overheating during a prolonged grasp
When the servo is stalled and exerting an effort above a threshold it will command a 'back off'
position (slightly opening the grasp). This reduces the PID steady state error and lowers the
commanded current. The gripper's spring design allows it to retain its grasp despite the backoff.
"""
DynamixelHelloXL430.step_sentry(self, robot)
if self.hw_valid and self.robot_params['robot_sentry']['stretch_gripper_overload'] and not self.is_homing:
if self.status['stall_overload']:
if self.status['effort'] < 0: #Only backoff in open direction
self.logger.debug('Backoff at stall overload')
self.move_by(self.params['stall_backoff'])
def __init__(self):
DynamixelXChain.__init__(self, '/dev/hello-dynamixel-wrist')
pitch = DynamixelHelloXL430('reactor_pitch', self)
self.add_motor(pitch)
roll = DynamixelHelloXL430('reactor_roll', self)
self.add_motor(roll)
gripper = DynamixelHelloXL430('reactor_gripper', self)
self.add_motor(gripper)
self.poses = {
'stow': [deg_to_rad(-90), deg_to_rad(0)],
'tool_ahead_0': [deg_to_rad(0), deg_to_rad(0)],
'tool_ahead_90': [deg_to_rad(0), deg_to_rad(90)],
'tool_ahead_-90': [deg_to_rad(0), deg_to_rad(-90)]
}
def step_sentry(self, robot):
"""This sentry attempts to prevent the wrist yaw servo from overheating
if it is pushing against an object for too long. It works by backing off
the commanded position from the current position, so as to lower the steady
state error of the PID controller.
"""
DynamixelHelloXL430.step_sentry(self, robot)
if self.hw_valid and self.robot_params['robot_sentry'][
'wrist_yaw_overload']:
if self.status['stall_overload']:
if self.status['effort'] > 0:
self.move_by(self.params['stall_backoff'])
self.logger.debug('Backoff at stall overload')
else:
self.move_by(-1 * self.params['stall_backoff'])
self.logger.debug('Backoff at stall overload')
def __init__(self):
DynamixelXChain.__init__(self, '/dev/hello-dynamixel-head')
self.name = 'head'
self.joints = ['head_pan', 'head_tilt']
for j in self.joints:
self.add_motor(DynamixelHelloXL430(j, self))
self.poses = {'ahead': [0, 0], 'back': [deg_to_rad(-180), deg_to_rad(0)],
'tool': [deg_to_rad(-90), deg_to_rad(-45)],
'wheels': [deg_to_rad(0), deg_to_rad(-90)], 'left': [deg_to_rad(90), deg_to_rad(0)],
'up': [deg_to_rad(0), deg_to_rad(30)]}
def __init__(self, chain=None,verbose=False):
DynamixelHelloXL430.__init__(self,'stretch_gripper',chain,verbose=verbose)
self.logger = logging.getLogger('robot.stretch_gripper')
self.status['pos_pct']= 0.0
self.poses = {'zero':0,'open': 50, 'close': -100}
def __init__(self, chain=None):
DynamixelHelloXL430.__init__(self, 'wrist_pitch', chain)
self.poses = {'tool_up': deg_to_rad(45),
'tool_down': deg_to_rad(-45)}
def world_rad_to_pct(self,r):
pct_to_tick = -1 * ((self.params['zero_t'] - self.params['range_t'][0]) / 100.0)
t = DynamixelHelloXL430.world_rad_to_ticks(self,r)
pct = -1*((t / pct_to_tick)+100)
return pct
def pct_to_world_rad(self,pct):
pct_to_tick = -1 * ((self.params['zero_t'] - self.params['range_t'][0]) / 100.0)
t=((-1*pct)-100)*pct_to_tick
r = DynamixelHelloXL430.ticks_to_world_rad(self, t)
return r
def pull_status(self,data=None):
DynamixelHelloXL430.pull_status(self,data)
self.status['pos_pct']=self.world_rad_to_pct(self.status['pos'])
def pretty_print(self):
print('--- StretchGripper ----')
print("Position (%)",self.status['pos_pct'])
DynamixelHelloXL430.pretty_print(self)
def home(self,move_to_zero=True):
DynamixelHelloXL430.home(self,single_stop=True,move_to_zero=move_to_zero,delay_at_stop=3.0)
def home(self):
"""
Home to hardstops
"""
DynamixelHelloXL430.home(self, single_stop=True)
time.sleep(2.0) #extra time to get back