class TRobotRobotiqNB(TMultiArmRobot):
def __init__(self, name='RobotiqNB'):
super(TRobotRobotiqNB,self).__init__(name=name)
self.currarm= 0
'''Initialize (e.g. establish ROS connection).'''
def Init(self):
self._is_initialized= False
res= []
ra= lambda r: res.append(r)
self.robotiq= TRobotiq() #Robotiq controller
self.grippers= [self.robotiq, self.robotiq]
print 'Initializing and activating Robotiq gripper...'
ra(self.robotiq.Init())
if False not in res: self._is_initialized= True
return self._is_initialized
def Cleanup(self):
#NOTE: cleaning-up order is important. consider dependency
super(TRobotRobotiqNB,self).Cleanup()
'''Answer to a query q by {True,False}. e.g. Is('PR2').'''
def Is(self, q):
if q in ('RobotiqNB',): return True
return super(TRobotRobotiqNB,self).Is(q)
@property
def BaseFrame(self):
return 'world'
'''Return range of gripper.
arm: arm id, or None (==currarm). '''
def GripperRange(self, arm=None):
if arm is None: arm= self.Arm
return self.grippers[arm].PosRange()
'''End effector of an arm.'''
def EndEff(self, arm=None):
if arm is None: arm= self.Arm
return self.grippers[arm]
#Dummy FK.
def FK(self, q=None, x_ext=None, arm=None, with_st=False):
x_res= [0,0,0, 0,0,0,1]
return (x_res, True) if with_st else x_res
'''Open a gripper.
arm: arm id, or None (==currarm).
blocking: False: move background, True: wait until motion ends. '''
def OpenGripper(self, arm=None, blocking=False):
self.MoveGripper(pos=0.1, arm=arm, blocking=blocking)
'''Close a gripper.
arm: arm id, or None (==currarm).
blocking: False: move background, True: wait until motion ends. '''
def CloseGripper(self, arm=None, blocking=False):
self.MoveGripper(pos=0.0, arm=arm, blocking=blocking)
'''High level interface to control a gripper.
arm: arm id, or None (==currarm).
pos: target position in meter.
max_effort: maximum effort to control; 0 (weakest), 100 (strongest).
speed: speed of the movement; 0 (minimum), 100 (maximum).
blocking: False: move background, True: wait until motion ends. '''
def MoveGripper(self, pos, max_effort=50.0, speed=50.0, arm=None, blocking=False):
if arm is None: arm= self.Arm
gripper= self.grippers[arm]
if gripper.Is('Robotiq'):
with self.control_locker:
gripper.Move(pos, max_effort, speed, blocking=blocking)
'''Get a gripper position in meter.
arm: arm id, or None (==currarm). '''
def GripperPos(self, arm=None):
if arm is None: arm= self.Arm
gripper= self.grippers[arm]
if gripper.Is('Robotiq'):
with self.sensor_locker:
pos= gripper.Position()
return pos
'''Get fingertip offset in meter.
The fingertip trajectory of Robotiq gripper has a round shape.
This function gives the offset from the opening posture.
pos: Gripper position to get the offset. None: Current position.
arm: arm id, or None (==currarm).'''
def FingertipOffset(self, pos=None, arm=None):
if arm is None: arm= self.Arm
gripper= self.grippers[arm]
if isinstance(gripper, TRobotiq):
if pos is None: pos= self.GripperPos(arm)
return -0.701*pos**3 - 2.229*pos**2 + 0.03*pos + 0.128 - 0.113
class TRobotBaxter(TDualArmRobot):
def __init__(self, name='Baxter', is_sim=False):
super(TRobotBaxter, self).__init__(name=name)
self.is_sim = is_sim
self.joint_names = [[], []]
self.joint_names[RIGHT] = [
'right_' + joint
for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
]
self.joint_names[LEFT] = [
'left_' + joint
for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
]
#Baxter all link names:
#obtained from /gazebo/link_states
#obtained from self.planning_scene_req.planning_scene_diff.allowed_collision_matrix.entry_names
self.links = {}
self.links['base'] = ['base', 'pedestal', 'torso']
self.links['head'] = [
'collision_head_link_1', 'collision_head_link_2', 'display',
'head', 'screen', 'sonar_ring'
]
self.links['l_arm'] = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist', 'left_upper_elbow_visual',
'left_upper_forearm_visual'
]
self.links['l_gripper'] = [
'left_gripper', 'left_gripper_base', 'left_hand',
'left_hand_accelerometer', 'left_hand_camera', 'left_hand_range'
]
self.links['r_arm'] = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist', 'right_upper_elbow_visual',
'right_upper_forearm_visual'
]
self.links['r_gripper'] = [
'right_gripper', 'right_gripper_base', 'right_hand',
'right_hand_accelerometer', 'right_hand_camera', 'right_hand_range'
]
self.links['robot'] = self.links['base'] + self.links[
'head'] + self.links['l_arm'] + self.links[
'l_gripper'] + self.links['r_arm'] + self.links['r_gripper']
'''Initialize (e.g. establish ROS connection).'''
def Init(self):
self._is_initialized = False
res = []
ra = lambda r: res.append(r)
self.default_face = 'config/baymax.jpg'
self.ChangeFace(self.default_face)
self.limbs = [None, None]
self.limbs[RIGHT] = baxter_interface.Limb(LRTostr(RIGHT))
self.limbs[LEFT] = baxter_interface.Limb(LRTostr(LEFT))
#self.joint_names= [[],[]]
#self.joint_names[RIGHT]= self.limbs[RIGHT].joint_names()
#self.joint_names[LEFT]= self.limbs[LEFT].joint_names()
#end_link=*_gripper(default),*_wrist,*_hand
self.kin = [None, None]
self.kin[RIGHT] = TKinematics(base_link=None, end_link='right_gripper')
self.kin[LEFT] = TKinematics(base_link=None, end_link='left_gripper')
self.head = baxter_interface.Head()
ra(
self.AddActC('r_traj',
'/robot/limb/right/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
time_out=3.0))
ra(
self.AddActC('l_traj',
'/robot/limb/left/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
time_out=3.0))
self.robotiq = TRobotiq() #Robotiq controller
self.epgripper = TBaxterEPG('right')
self.grippers = [self.epgripper, self.robotiq]
print 'Enabling the robot...'
baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION).enable()
print 'Calibrating electric parallel gripper...'
ra(self.epgripper.Init())
print 'Initializing and activating Robotiq gripper...'
ra(self.robotiq.Init())
if False not in res: self._is_initialized = True
return self._is_initialized
def Cleanup(self):
#NOTE: cleaning-up order is important. consider dependency
for gripper in self.grippers:
gripper.Cleanup()
super(TRobotBaxter, self).Cleanup()
'''Configure a state validity checker.'''
def ConfigureSVC(self, c):
c.JointNames = copy.deepcopy(self.joint_names)
c.Links = copy.deepcopy(self.links)
c.PaddingLinks = c.Links['l_gripper'] + c.Links['r_gripper']
c.PaddingValues = [0.002] * len(c.PaddingLinks)
c.DefaultBaseFrame = 'torso'
c.HandLinkToGrasp[RIGHT] = 'right_gripper'
c.HandLinkToGrasp[LEFT] = 'left_gripper'
c.IgnoredLinksInGrasp[RIGHT] = ['right_gripper'] + c.Links['r_gripper']
c.IgnoredLinksInGrasp[LEFT] = ['left_gripper'] + c.Links['l_gripper']
'''Answer to a query q by {True,False}. e.g. Is('PR2').'''
def Is(self, q):
if q in ('Baxter', 'Baxter_SIM'): return True
return super(TRobotBaxter, self).Is(q)
@property
def BaseFrame(self):
return 'torso'
'''End link of an arm.'''
def EndLink(self, arm=None):
if arm is None: arm = self.Arm
if arm == RIGHT: return 'right_gripper'
elif arm == LEFT: return 'left_gripper'
'''Names of joints of an arm.'''
def JointNames(self, arm=None):
if arm is None: arm = self.Arm
return self.joint_names[arm]
'''Return limits (lower, upper) of joint angles.
arm: LEFT, RIGHT, or None (==currarm). '''
def JointLimits(self, arm=None):
if arm is None: arm = self.Arm
return self.kin[arm].joint_limits_lower, self.kin[
arm].joint_limits_upper
'''Return limits of joint angular velocity.
arm: LEFT, RIGHT, or None (==currarm). '''
def JointVelLimits(self, arm=None):
#['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
return [0.5, 0.5, 0.8, 0.8, 0.8, 0.8, 0.8]
'''Return range of gripper.
arm: LEFT, RIGHT, or None (==currarm). '''
def GripperRange(self, arm=None):
if arm is None: arm = self.Arm
gripper = self.grippers[arm]
if gripper.Is('BaxterEPG'): return gripper.PosRange()
elif gripper.Is('Robotiq'): return gripper.PosRange()
'''End effector of an arm.'''
def EndEff(self, arm):
if arm is None: arm = self.Arm
return self.grippers[arm]
'''Return joint angles of an arm (list of floats).
arm: LEFT, RIGHT, or None (==currarm). '''
def Q(self, arm=None):
if arm is None: arm = self.Arm
with self.sensor_locker:
angles = self.limbs[arm].joint_angles()
q = [angles[joint] for joint in self.joint_names[arm]] #Serialize
return q
'''Return joint velocities of an arm (list of floats).
arm: LEFT, RIGHT, or None (==currarm). '''
def DQ(self, arm=None):
if arm is None: arm = self.Arm
with self.sensor_locker:
velocities = self.limbs[arm].joint_velocities()
dq = [velocities[joint]
for joint in self.joint_names[arm]] #Serialize
#return dq
raise NotImplemented('TRobotBaxter.DQ(not tested)')
'''Compute a forward kinematics of an arm.
Return self.EndLink(arm) pose on self.BaseFrame.
return: x, res; x: pose (list of floats; None if failure), res: FK status.
arm: LEFT, RIGHT, or None (==currarm).
q: list of joint angles, or None (==self.Q(arm)).
x_ext: a local pose on self.EndLink(arm) frame.
If not None, the returned pose is x_ext on self.BaseFrame.
with_st: whether return FK status. '''
def FK(self, q=None, x_ext=None, arm=None, with_st=False):
if arm is None: arm = self.Arm
if q is None: q = self.Q(arm)
angles = {
joint: q[j]
for j, joint in enumerate(self.joint_names[arm])
} #Deserialize
with self.sensor_locker:
x = self.kin[arm].forward_position_kinematics(joint_values=angles)
x_res = list(x if x_ext is None else Transform(x, x_ext))
return (x_res, True) if with_st else x_res
'''Compute a Jacobian matrix of an arm.
Return J of self.EndLink(arm).
return: J, res; J: Jacobian (numpy.matrix; None if failure), res: status.
arm: LEFT, RIGHT, or None (==currarm).
q: list of joint angles, or None (==self.Q(arm)).
x_ext: a local pose (i.e. offset) on self.EndLink(arm) frame.
If not None, we do not consider an offset.
with_st: whether return the solver status. '''
def J(self, q=None, x_ext=None, arm=None, with_st=False):
if arm is None: arm = self.Arm
if q is None: q = self.Q(arm)
if x_ext is not None:
#Since KDL does not provide Jacobian computation with an offset x_ext,
#and converting J with x_ext is not simple, we raise an Exception.
#TODO: Implement our own FK to solve this issue.
raise Exception(
'TRobotBaxter.J: Jacobian with x_ext is not implemented yet.')
angles = {
joint: q[j]
for j, joint in enumerate(self.joint_names[arm])
} #Deserialize
with self.sensor_locker:
J_res = self.kin[arm].jacobian(joint_values=angles)
return (J_res, True) if with_st else J_res
'''Compute an inverse kinematics of an arm.
Return joint angles for a target self.EndLink(arm) pose on self.BaseFrame.
return: q, res; q: joint angles (list of floats; None if failure), res: IK status.
arm: LEFT, RIGHT, or None (==currarm).
x_trg: target pose.
x_ext: a local pose on self.EndLink(arm) frame.
If not None, the returned q satisfies self.FK(q,x_ext,arm)==x_trg.
start_angles: initial joint angles for IK solver, or None (==self.Q(arm)).
with_st: whether return IK status. '''
def IK(self,
x_trg,
x_ext=None,
start_angles=None,
arm=None,
with_st=False):
if arm is None: arm = self.Arm
if start_angles is None: start_angles = self.Q(arm)
x_trg[3:] /= la.norm(x_trg[3:]) #Normalize the orientation:
xw_trg = x_trg if x_ext is None else TransformRightInv(x_trg, x_ext)
with self.sensor_locker:
res, q = self.kin[arm].inverse_kinematics(xw_trg[:3],
xw_trg[3:],
seed=start_angles,
maxiter=1000,
eps=1.0e-6,
with_st=True)
if q is not None: q = list(q)
if res: return (q, True) if with_st else q
else: return (q, False) if with_st else None
'''Follow a joint angle trajectory.
arm: LEFT, RIGHT, or None (==currarm).
q_traj: joint angle trajectory [q0,...,qD]*N.
t_traj: corresponding times in seconds from start [t1,t2,...,tN].
blocking: False: move background, True: wait until motion ends, 'time': wait until tN. '''
def FollowQTraj(self, q_traj, t_traj, arm=None, blocking=False):
assert (len(q_traj) == len(t_traj))
if arm is None: arm = self.Arm
#Insert current position to beginning.
if t_traj[0] > 1.0e-4:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
#copy q_traj, t_traj to goal
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.goal_time_tolerance = rospy.Time(0.1)
goal.trajectory.joint_names = self.joint_names[arm]
goal.trajectory = ToROSTrajectory(self.JointNames(arm), q_traj, t_traj)
with self.control_locker:
actc = self.actc.r_traj if arm == RIGHT else self.actc.l_traj
actc.send_goal(goal)
BlockAction(actc, blocking=blocking, duration=t_traj[-1])
#actc.wait_for_result(timeout=rospy.Duration(t_traj[-1]+5.0)) WARNING: Maybe it's better to set timeout
'''Open a gripper.
arm: LEFT, RIGHT, or None (==currarm).
blocking: False: move background, True: wait until motion ends. '''
def OpenGripper(self, arm=None, blocking=False):
self.MoveGripper(pos=0.1, arm=arm, blocking=blocking)
'''Close a gripper.
arm: LEFT, RIGHT, or None (==currarm).
blocking: False: move background, True: wait until motion ends. '''
def CloseGripper(self, arm=None, blocking=False):
self.MoveGripper(pos=0.0, arm=arm, blocking=blocking)
'''High level interface to control a gripper.
arm: LEFT, RIGHT, or None (==currarm).
pos: target position in meter.
max_effort: maximum effort to control; 0 (weakest), 100 (strongest).
speed: speed of the movement; 0 (minimum), 100 (maximum).
blocking: False: move background, True: wait until motion ends. '''
def MoveGripper(self,
pos,
max_effort=50.0,
speed=50.0,
arm=None,
blocking=False):
if self.is_sim:
return #WARNING:We do nothing if the robot is on simulator.
if arm is None: arm = self.Arm
gripper = self.grippers[arm]
if gripper.Is('BaxterEPG'):
with self.gripper_locker:
gripper.Move(pos, max_effort, speed, blocking=blocking)
elif gripper.Is('Robotiq'):
with self.gripper_locker:
gripper.Move(pos, max_effort, speed, blocking=blocking)
'''Get a gripper position in meter.
arm: LEFT, RIGHT, or None (==currarm). '''
def GripperPos(self, arm=None):
if self.is_sim:
return 0.0 #WARNING:We do nothing if the robot is on simulator.
if arm is None: arm = self.Arm
gripper = self.grippers[arm]
if gripper.Is('BaxterEPG'):
with self.sensor_locker:
pos = gripper.Position()
return pos
elif gripper.Is('Robotiq'):
with self.sensor_locker:
pos = gripper.Position()
return pos
'''Get a fingertip height offset in meter.
The fingertip trajectory of some grippers has a rounded shape.
This function gives the offset from the highest (longest) point (= closed fingertip position),
and the offset is always negative.
NOTE: In the previous versions (before 2019-12-10), this offset was from the opened fingertip position.
pos: Gripper position to get the offset. None: Current position.
arm: arm id, or None (==currarm).'''
def FingertipOffset(self, pos=None, arm=None):
if arm is None: arm = self.Arm
if pos is None: pos = self.GripperPos(arm)
return self.grippers[arm].FingertipOffset(pos)
'''Control the head around z.
angle: target angle in radian.
speed: movement speed; 0 (minimum), 100 (maximum).
timeout: timeout in seconds. '''
def MoveHeadPan(self, angle, speed=100, timeout=10):
self.head.set_pan(angle, speed, timeout)
'''Nod head (no controllable parameters). '''
def NodHead(self):
self.head.command_nod()
'''Change the face image to file_name.'''
def ChangeFace(self, file_name):
img = cv2.imread(file_name)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot/xdisplay',
sensor_msgs.msg.Image,
latch=True,
queue_size=1)
pub.publish(msg)