def __init__(self, limb_name, trajectory, offset, radius):
# crate a limb instance control the baxter arm
self.limb = Limb(limb_name)
# numpy joint angle trajectory of nx7 shape
self.trajectory = trajectory
# index variable to keep track of current row in numpy array
self.trajectory_index = 0
# store the joint names since
# it will be used later while commanding the limb
self.joint_names = self.limb.joint_names()
# define a service called 'move_arm_to_waypoint'
self.move_arm_to_waypoint_service = rospy.Service(
'move_arm_to_waypoint', Trigger, self.handle_move_arm_to_waypoint)
# define another service called 'get_ee_pose'
self.get_ee_position_service = rospy.Service(
'get_ee_position', GetEEPosition,
self.handle_get_ee_position_service)
# flag to set when trajectory is finished
self.trajectory_finished = False
# define 4x4 transformation for marker wrt end-effector
# considering no rotation in 'marker_wrt_ee' transformation matrix
self.marker_wrt_ee = np.eye(4)
self.marker_wrt_ee[2, -1] = offset + radius # in z direction only
def __init__(self, usegripper = True):
rospy.init_node("Baxter")
threading.Thread(target=self.__jointtraj_server).start()
self.trajrgt = Trajectory("right")
self.trajlft = Trajectory("left")
if usegripper:
self.__right_hand = Gripper("right")
self.__left_hand = Gripper("left")
self.__right_hand.calibrate()
self.__left_hand.calibrate()
self.__right_limb = Limb("right")
self.__left_limb = Limb("left")
self.baxter = RobotEnable()
self.__enabled = self.baxter.state().enabled
self.enable_baxter()
self.img = None
for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']:
try:
cam = CameraController(camname)
# cam.resolution = (1280, 800)
cam.resolution = (320, 200)
except:
pass
print("baxter opened")
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
rospy.sleep(0.5)
self.gripper.set_holding_force(0.1)
rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
def __init__(self, mode, arm_mode, step=5):
self.mode = mode
self.arm_mode = arm_mode
self._step = step
self._right_limb = Limb('right')
self._left_limb = Limb('left')
self._subscribe()
def set_joints( target_angles_right, target_angles_left):
right = Limb("right")
left = Limb("left")
reach_right = False
reach_left = False
while not reach_right or not reach_left:
right.set_joint_positions(target_angles_right)
left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
def __init__(self,
starting_poss=None,
push_thresh=10,
mode='one_arm',
arm_mode='first'):
"""
Initialises parameters and moves the arm to a neutral
position.
"""
self.push_thresh = push_thresh
self._right_neutral_pos = starting_poss[0]
self._left_neutral_pos = starting_poss[1]
self._mode = mode
self._arm_mode = arm_mode
rospy.loginfo("Creating interface and calibrating gripper")
self._right_limb = Limb('right')
self._left_limb = Limb('left')
self._right_gripper = Gripper('right', CHECK_VERSION)
self._right_gripper.calibrate()
self._left_gripper = Gripper('left', CHECK_VERSION)
self._left_gripper.calibrate()
self._is_right_fist_closed = False
self._is_left_fist_closed = False
rospy.loginfo("Moving to neutral position")
self.move_to_neutral()
rospy.loginfo("Initialising PoseGenerator")
self._pg = PoseGenerator(self._mode, self._arm_mode)
self._sub_right_gesture = rospy.Subscriber(
"/low_myo/gesture", UInt8, self._right_gesture_callback)
self._sub_left_gesture = rospy.Subscriber("/top_myo/gesture", UInt8,
self._left_gesture_callback)
self._last_data = None
self._pg.calibrate()
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {'fk': {'ros': 'compute_fk'},
'ik': {'ros': 'compute_ik',
'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name),
'trac': 'trac_ik_{}'.format(name)}}
self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK),
'func': self._get_fk_ros},
'kdl': {'func': self._get_fk_pykdl},
'robot': {'func': self._get_fk_robot}},
'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK),
'func': self._get_ik_ros},
'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK),
'func': self._get_ik_robot},
'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK),
'func': self._get_ik_trac},
'kdl': {'func': self._get_ik_pykdl}}}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name)
self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber(
'/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
def mover_setup(self):
limb = Limb("right")
# Right arm planner
self.planner = PathPlanner("right_arm")
# Left arm planner
self.planner_left = PathPlanner("left_arm")
place_holder = {'images': [], 'camera_infos': []}
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
# Controller for right arm
self.c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
self.orien_const = OrientationConstraint()
self.orien_const.link_name = "right_gripper";
self.orien_const.header.frame_id = "base";
self.orien_const.orientation.y = -1.0;
self.orien_const.absolute_x_axis_tolerance = 0.1;
self.orien_const.absolute_y_axis_tolerance = 0.1;
self.orien_const.absolute_z_axis_tolerance = 0.1;
self.orien_const.weight = 1.0;
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = self.box.pose.position.x
box.pose.position.y = self.box.pose.position.y
# box.pose.position.z = self.box.pose.position.z
box.pose.position.z = self.conveyor_z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
self.planner.add_box_obstacle((100, 100, 0.00001), "box", box)
# Controller for left arm
self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0, 10) * -1
self.history_y = np.arange(0, 10) * -1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1 / (526 - 369)
self.coefy = 0.1 / (237 - 90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400, 640, 3), np.uint8)
cv2.imshow('image', self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision,
self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image,
self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range,
self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
def main():
joint_states = {
'right_initial': {
'right_e0': 2.498,
'right_e1': 2.158,
'right_s0': 0.826,
'right_s1': 0.366,
'right_w0': 2.809,
'right_w1': 1.867,
'right_w2': 0.411,
},
'left_initial': {
'left_e0': -1.738,
'left_e1': 1.828,
'left_s0': 0.247,
'left_s1': 0.257,
'left_w0': 0.0721,
'left_w1': -0.818,
'left_w2': 1.826,
},
}
rospy.init_node('demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm_right = Limb("right")
kin_right = baxter_kinematics('right')
arm_left = Limb("left")
kin_left = baxter_kinematics('left')
set_joints(target_angles_right=joint_states['right_initial'])
# right arm movement
ik_move(hdr,
arm_right,
kin_right,
target_dx=0.35,
arm_position='right',
speedx=0.05)
ik_move(hdr,
arm_right,
kin_right,
target_dy=0.1,
arm_position='right',
speedy=0.1)
ik_move(hdr,
arm_right,
kin_right,
target_dx=-0.35,
arm_position='right',
speedx=0.1)
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(_IKSVC_LEFT_URI, SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(250)
def __init__(self):
self.rightg = Gripper('right')
self.rightg.set_holding_force(100)
self.leftg = Gripper('left')
self.right = Limb('right')
self.left = Limb('left')
self.head = Head()
self.angles= list()
rospy.Subscriber("command", String, self.command)
rospy.spin()
class TrajectoryWaypointsRecorder():
def __init__(self, limb_name, file_name):
self.limb = Limb(limb_name)
self.trajectory = []
self.file_name = file_name
self.file_header = self.limb.joint_names()
self.dt = rospy.Duration(secs=2)
self.previous_time = rospy.Time.now()
self.limb_nav = Navigator(limb_name)
self.limb_nav.button2_changed.connect(self.limb_nav_button_pressed)
rospy.on_shutdown(self.save_data)
rospy.loginfo('Press Ctrl + C to exit')
rospy.spin()
def save_data(self):
file_header = ','.join(x for x in self.file_header)
# src: http://wiki.ros.org/Packages#Client_Library_Support
full_path = os.path.join(
rospkg.RosPack().get_path('multiple_kinect_baxter_calibration'),
'files', self.file_name)
np.savetxt(full_path,
self.trajectory,
header=file_header,
delimiter=',',
fmt='%.4f',
comments='')
rospy.loginfo("Trajectory have been successfully saved to \n'%s'\n" %
full_path)
def limb_nav_button_pressed(self, state):
now = rospy.Time.now()
if (now - self.previous_time > self.dt) and state:
self.limb_nav.inner_led = state
self.record()
elif not state:
self.limb_nav.inner_led = state
self.previous_time = now
def record(self):
joint_angles_dict = self.limb.joint_angles()
joint_angles = [
joint_angles_dict[joint_name] for joint_name in self.file_header
]
self.trajectory.append(joint_angles)
rospy.loginfo('%d samples collected.' % len(self.trajectory))
def return_to_init(dict_keys,arm_name):
from baxter_interface import Limb
'''
Joint angles that give us desired initial position:
{'left_w0': -0.029529130166794215, 'left_w1': 0.08436894333369775, 'left_w2': -0.08782040010643993,
'left_e0': 0.10009224640952324, 'left_e1': 0.03604854851530722,
'left_s0': -0.8061069040337849, 'left_s1': -0.13690778531877318}
'''
init_pos=[-0.8061069040337849,-0.13690778531877318,0.10009224640952324,0.03604854851530722,
-0.029529130166794215,0.08436894333369775,-0.08782040010643993]
init_pos_dict=convert_list_to_dict(dict_keys,init_pos)
arm=Limb(arm_name)
t_end=time.time()+4 # Loop timer (in seconds)
while time.time()<t_end:
arm.set_joint_positions(init_pos_dict)
def record():
rospy.init_node('record')
RobotEnable(CHECK_VERSION).enable()
baxter_limb = Limb(limb_name)
while True:
initial_time = rospy.get_time()
forces_vec3 = baxter_limb.endpoint_effort()['force']
forces = np.array([forces_vec3.x, forces_vec3.y, forces_vec3.z])
print "===========\nEndpoint Forces: \nF_x: {x} \nF_y: {y} \nF_z: {z}\n===========\n".format(forces_vec3)
after_time = rospy.get_time()
rospy.sleep(dt - (after_time - initial_time))
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def __init__(self, limb_name, file_name):
self.limb = Limb(limb_name)
self.trajectory = []
self.file_name = file_name
self.file_header = self.limb.joint_names()
self.dt = rospy.Duration(secs=2)
self.previous_time = rospy.Time.now()
self.limb_nav = Navigator(limb_name)
self.limb_nav.button2_changed.connect(self.limb_nav_button_pressed)
rospy.on_shutdown(self.save_data)
rospy.loginfo('Press Ctrl + C to exit')
rospy.spin()
def initBaxterObjects():
global rightGripper
global leftGripper
global rightMover
global leftMover
rightGripper = Gripper('right', versioned = False)
leftGripper = Gripper('left', versioned = False)
rightMover = Limb('right')
leftMover = Limb('left')
# Initialize based on which is over the blocks
global numBlocks
global isOneArmSolution
gridToCartesian.initToBaxter(rightMover)
def stir_salad(self,stir_joint_angles):
self.errorhandle("ladle")
initial_pose = get_current_pose(self.hdr,self.control_arm)
initial_angles = Limb("right").joint_angles()
set_joints(target_angles_right = stir_joint_angles)
upstraight_pose = get_current_pose(self.hdr,self.control_arm)
ready_pose = copy.deepcopy(upstraight_pose)
x,y = self.moveToBowl()
ready_pose.pose.position.x = x + 0.13
ready_pose.pose.position.y = y - 0.15
ik_move_to_pose(self.control_arm,self.kin_right,ready_pose)
stir_pose = copy.deepcopy(ready_pose)
stir_pose.pose.position.z -= 0.24
ik_move_to_pose(self.control_arm,self.kin_right,stir_pose)
initial_stir_pose = get_current_pose(self.hdr,self.control_arm)
initial_y = initial_stir_pose.pose.position.y
initial_x = initial_stir_pose.pose.position.x
current_pose = copy.deepcopy(initial_stir_pose)
for i in range(6000):
current_pose.pose.position.y = initial_y + math.sin(i*0.004)*0.07
current_pose.pose.position.x = initial_x - (0.07-math.cos(i*0.004)*0.07)
ik_pykdl(self.control_arm,self.kin_right,current_pose)
ik_move_to_pose(self.control_arm,self.kin_right,ready_pose)
ik_move_to_pose(self.control_arm,self.kin_right,upstraight_pose)
set_joints(target_angles_right = initial_angles)
def __init__(self, side_name):
"""
Constructor.
@param side_name: which arm side we are refering to 'left' or 'right'.
"""
self.__side_name = side_name
# MoveIt interface to a group of arm joints.
# Either left arm joint group or right arm joint group.
self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name))
# MoveIt limb setting.
self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name))
self.moveit_limb.set_planner_id('RRTConnectKConfigDefault')
self.moveit_limb.set_goal_position_tolerance(0.01)
self.moveit_limb.set_goal_orientation_tolerance(0.01)
# MoveIt does not provide support for Baxter gripper.
# Thus we use Baxter SDK gripper instead.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self.baxter_sdk_limb = Limb(side_name)
def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]]
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(
_IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16)
self._joint_update_pub.publish(250)
def set_joints( target_angles_right = None, target_angles_left = None,timeout= 40000):
right = Limb("right")
left = Limb("left")
if target_angles_right == None:
reach_right = True
else:
reach_right = False
if target_angles_left == None:
reach_left = True
else:
reach_left = False
time = 0
while not reach_right or not reach_left:
if target_angles_right: right.set_joint_positions(target_angles_right)
if target_angles_left: left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
if reach_right == False:
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
if reach_left == False:
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
time+=1
if time > timeout:
print "Time out"
break
class Baxter_Interactive(object):
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def recorder(self):
doneRecording = False
while not doneRecording:
if self._navigator.button0:
self.recorded.append(self._limb.joint_angles())
print 'Waypoint Added'
rospy.sleep(1)
if self._close_button.state:
self.recorded.append('CLOSE')
self._gripper.close()
print 'Gripper Closed'
rospy.sleep(1)
if self._open_button.state:
self.recorded.append('OPEN')
self._gripper.open()
print 'Gripper Opened'
rospy.sleep(1)
if self._navigator.button1:
print 'END RECORDING'
doneRecording = True
rospy.sleep(3)
while doneRecording:
for waypoint in self.recorded:
if waypoint == 'OPEN':
self._gripper.open()
rospy.sleep(1)
elif waypoint == 'CLOSE':
self._gripper.close()
rospy.sleep(1)
else:
self._limb.move_to_joint_positions(waypoint)
def clean_shutdown(self):
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm = Limb("right")
# ik_move(hdr,arm, x = 0.82 , y = -0.2)
# ik_move(hdr,arm, z = -0.136)
# self.gripper.open()
# rospy.sleep(0.5)
# ik_move(hdr,arm, target_dx= -0.2)
print "Demo finished"
rospy.signal_shutdown("Done")
def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[
pose['orientation'].x, pose['orientation'].y,
pose['orientation'].z, pose['orientation'].w
]]
class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
rospy.sleep(0.5)
self.gripper.set_holding_force(0.1)
rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def pour_dressing(self, dressing_angles):
self.errorhandle("dressing")
initial_pose = get_current_pose(self.hdr, self.control_arm)
initial_angles = Limb("right").joint_angles()
# set_joints(target_angles_right = dressing_angles)
ready_pose = get_current_pose(self.hdr, self.control_arm)
#x,y = self.moveToBowl()
x = self.pour_x
y = self.pour_y
ready_pose.pose.position.x = x
ready_pose.pose.position.y = y - 0.15
ik_move_to_pose(self.control_arm, self.kin_right, ready_pose)
def __init__(self, sub_topic, pub_topic):
self.planner = None
self.limb = None
self.executed = False
self.planner = PathPlanner("right_arm")
self.limb = Limb("right")
# self.orien_const = OrientationConstraint()
# self.orien_const.link_name = "right_gripper"
# self.orien_const.header.frame_id = "base"
# self.orien_const.orientation.z = 0
# self.orien_const.orientation.y = 1
# self.orien_const.orientation.w = 0
# self.orien_const.absolute_x_axis_tolerance = 0.1
# self.orien_const.absolute_y_axis_tolerance = 0.1
# self.orien_const.absolute_z_axis_tolerance = 0.1
# self.orien_const.weight = 1.0
size = [1, 2, 2]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = -1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = 0
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "wall", obstacle_pose)
size = [.75, 1, 1]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = 1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = -.4
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "table", obstacle_pose)
self.pub = rospy.Publisher(pub_topic, Bool)
self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback)
def __init__(self, arm):
self.as_goal = {'left': baxterGoal(), 'right': baxterGoal()}
self.as_feed = {'left': baxterFeedback(), 'right': baxterFeedback()}
self.as_res = {'left': baxterResult(), 'right': baxterResult()}
self.action_server_left = actionlib.SimpleActionServer(
"baxter_action_server_left",
baxterAction,
self.execute_left,
auto_start=False)
self.action_server_right = actionlib.SimpleActionServer(
"baxter_action_server_right",
baxterAction,
self.execute_right,
auto_start=False)
self.left_arm = Limb('left')
self.right_arm = Limb('right')
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.left_gripper.calibrate()
self.arm = arm
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0,10)*-1
self.history_y = np.arange(0,10)*-1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400,640,3), np.uint8)
cv2.imshow('image',self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
def create_arms(self):
"""
Create arm interface objects for Baxter.
An arm consists of a Limb and its Gripper.
"""
# create arm objects
if self.sim:
self.left_arm = pybullet_interface.Limb(self.baxter_id, "left",
self.config)
# self.left_arm.gripper = pybullet_interface.Gripper("left")
self.right_arm = pybullet_interface.Limb(self.baxter_id, "right",
self.config)
# self.right_arm.gripper = pybullet_interface.Gripper("right")
else:
self.left_arm = Limb("left")
self.left_arm.gripper = Gripper("left")
self.left_arm.kin = baxter_kinematics("left")
self.right_arm = Limb("right")
self.right_arm.gripper = Gripper("right")
self.right_arm.kin = baxter_kinematics("right")
return
class Baxter_Deli(object):
def __init__(self):
self.rightg = Gripper('right')
self.rightg.set_holding_force(100)
self.leftg = Gripper('left')
self.right = Limb('right')
self.left = Limb('left')
self.head = Head()
self.angles= list()
rospy.Subscriber("command", String, self.command)
rospy.spin()
def command(self, comm):
if comm.data == "left":
self.angles.append(self.left.joint_angles())
elif comm.data == "right":
self.angles.append(self.right.joint_angles())
elif comm.data == "done":
print self.angles
def set_joints( target_angles_right = None, target_angles_left = None,timeout= 40000):
right = Limb("right")
left = Limb("left")
if target_angles_right == None:
reach_right = True
else:
reach_right = False
if target_angles_left == None:
reach_left = True
else:
reach_left = False
time = 0
while not reach_right or not reach_left:
if target_angles_right: right.set_joint_positions(target_angles_right)
if target_angles_left: left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
if reach_right == False:
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
if reach_left == False:
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
time+=1
if time > timeout:
print "Time out"
break
def main():
joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe_vertical': {
'right_e0': 0.2784175126831055,
'right_e1': 1.8561167512207033,
'right_s0': -0.1464951650756836,
'right_s1': -0.5292233712158203,
'right_w0': -0.9572040105468751,
'right_w1': -1.200723460345459,
'right_w2': 0.004985437554931641,
},
}
roscpp_initialize(sys.argv)
rospy.init_node('demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm = Limb("right")
kin = baxter_kinematics('right')
#set_joints(target_angles_right = joint_states['observe_midpoint'],target_angles_left = joint_states['observe_left'],timeout= 100000)
tracker = track()
rospy.on_shutdown(tracker.clean_shutdown)
#pour tomato
set_joints(target_angles_right=joint_states['observe_vertical'])
initial_pose = get_current_pose(hdr, arm)
#warm up the tracker after initial start up
tracker.track(initial_pose, hdr, arm, id=1)
ik_move(hdr, arm, kin, z=0.47)
ik_move(hdr, arm, kin, target_dx=0.3, speedx=0.1)
#ik_move(hdr,arm, kin, target_dz = -0.08, speedz = 0.1)
ik_move(hdr, arm, kin, z=0.35, speedz=0.1)
ik_move(hdr, arm, kin, x=0.75, speedx=0.1)
def main():
"""
Calculate best PID constans using simulated annealing
:return:
"""
rospy.init_node('pid_calibration')
enabler = RobotEnable()
enabler.enable()
limb = Limb('left')
init_params = {'left_s0': {'kp': 80.0, 'ki': 0.29, 'kd': 1.4},
'left_s1': {'kp': 100.0, 'ki': 1.0, 'kd': 1.0},
'left_e0': {'kp': 10.0, 'ki': 2.1, 'kd': 0.12},
'left_e1': {'kp': 16.0, 'ki': 2.1, 'kd': 0.26},
'left_w0': {'kp': 2.0, 'ki': 0.1, 'kd': 0.5},
'left_w1': {'kp': 1.9, 'ki': 1.71, 'kd': 0.15},
'left_w2': {'kp': 1.8, 'ki': 2.31, 'kd': 0.11}}
init_error = calculate_error(init_params)
best = simulated_annealing(init_params, init_error, calculate_error, neighbour, acceptance,
stop_condition, 100.0, temperature_update=lambda x: 0.95*x, callback=callback)
print best
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 0
if (resp.isValid[0]):
#print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
arm = Limb("right")
arm.set_joint_positions(limb_joints)
return 1
#rospy.sleep(0.05)
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
def ik_pykdl(arm, kin, pose, arm_position = 'right'):
position = pose.pose.position
orientation = pose.pose.orientation
pos = [position.x,position.y,position.z]
rot = [orientation.x,orientation.y,orientation.z,orientation.w]
joint_angles = kin.inverse_kinematics(pos,rot)
if joint_angles:
#holding force: 40
#replace cookie with drink
#individually wrapped triangle sandwich will not do
#!/usr/bin/env python
import rospy, baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('deli_Baxter')
rightg = Gripper('right')
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201}
neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate), self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans,r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
(l_trans,l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (
sum(map(lambda x: x[0], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) / self._avg_window)
r_trans = (
sum(map(lambda x: x[0], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) / self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768-50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._left_iksvc, self._left_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._right_iksvc, self._right_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
class vision_server():
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0,10)*-1
self.history_y = np.arange(0,10)*-1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400,640,3), np.uint8)
cv2.imshow('image',self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
print "\nReady to use right hand vision server\n"
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
#print np.dot(self.J,self.jointVelocity)
def _read_distance(self,data):
self.distance = data.range
def _set_threshold(self):
cv2.createTrackbar('Min R(H)','image',0,255,nothing)
cv2.createTrackbar('Max R(H)','image',255,255,nothing)
cv2.createTrackbar('Min G(S)','image',0,255,nothing)
cv2.createTrackbar('Max G(S)','image',255,255,nothing)
cv2.createTrackbar('Min B(V)','image',0,255,nothing)
cv2.createTrackbar('Max B(V)','image',255,255,nothing)
def get_joint_velocity(self):
cmd = {}
velocity_list = np.asarray([1,2,3,4,5,6,7],np.float32)
for idx, name in enumerate(self.control_joint_names):
v = self.control_arm.joint_velocity( self.control_joint_names[idx])
velocity_list[idx] = v
cmd[name] = v
return velocity_list
def list_to_dic(self,ls):
cmd = {}
for idx, name in enumerate(self.control_joint_names):
v = ls.item(idx)
cmd[name] = v
return cmd
def PID(self):
Kp = 0.5
vy = -Kp*self.dx
vx = -Kp*self.dy
return vx,vy
def _on_camera(self, data):
self.busy = True
self.framenumber += 1
index = self.framenumber % 10
cv2.namedWindow('image')
cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8")
np_image = np.asarray(cv_image)
image_after_process, mask = self.image_process(np_image)
self.history_x[index] = self.dx*100
self.history_y[index] = self.dy*100
self.avg_dx = np.average(self.history_x)/100
self.avg_dy = np.average(self.history_y)/100
# if abs(self.dx)<0.01 and abs(self.dy)<0.01:
# self.found = True
# else:
# self.found = False
vx, vy = self.PID()
self.v_end = np.asarray([(1-((abs(vx)+abs(vy))*5))*0.03,vy,vx,0,0,0],np.float32)
#self.v_end = np.asarray([0,0,0,0,0,0.05],np.float32)
self.v_joint = np.dot(self.J_inv,self.v_end)
self.cmd = self.list_to_dic(self.v_joint)
self.busy = False
cv2.imshow('image',image_after_process)
cv2.waitKey(1)
def image_process(self,img):
# min_r = cv2.getTrackbarPos('Min R(H)','image')
# max_r = cv2.getTrackbarPos('Max R(H)','image')
# min_g = cv2.getTrackbarPos('Min G(S)','image')
# max_g = cv2.getTrackbarPos('Max G(S)','image')
# min_b = cv2.getTrackbarPos('Min B(V)','image')
# max_b = cv2.getTrackbarPos('Max B(V)','image')
# min_r = 0
# max_r = 57
# min_g = 87
# max_g = 181
# min_b = 37
# max_b = 70
min_r = 0
max_r = 58
min_g = 86
max_g = 255
min_b = 0
max_b = 255
min_color = (min_r, min_g, min_b)
max_color = (max_r, max_g, max_b)
#img = cv2.flip(img, flipCode = -1)
hsv_img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_img, min_color, max_color)
result = cv2.bitwise_and(img, img, mask = mask)
mask_bool = np.asarray(mask, bool)
label_img = morphology.remove_small_objects(mask_bool, 1000, connectivity = 1)
objects = measure.regionprops(label_img)
if objects != []:
self.found = True
target = objects[0]
box = target.bbox
cv2.rectangle(img,(box[1],box[0]),(box[3],box[2]),(0,255,0),3)
dx_pixel=target.centroid[1]-self.centerx
dy_pixel=target.centroid[0]-self.centery
dx=dx_pixel*self.coefx
dy=dy_pixel*self.coefy
angle = target.orientation
cv2.circle(img,(int(target.centroid[1]),int(target.centroid[0])),10,(0,0,255),-1)
self.dx = dx
self.dy = dy
#print self.dx,self.dy
else:
self.found = False
self.dx = 0
self.dy = 0
return img, mask_bool
def handle_vision_req(self, req):
#resp = VisionResponse(req.requestID, 0, self.avg_dx, self.avg_dy)
self.finish = 0
if self.found == True:
if self.distance > 0.07:
self.control_arm.set_joint_velocities(self.cmd)
#rospy.sleep(0.02)
else:
self.finish = 1
resp = VisionResponse(req.requestID, self.finish, self.dx, self.dy)
return resp
def clean_shutdown(self):
print "Server finished"
cv2.imwrite('box_img.png', self.blank_image)
rospy.signal_shutdown("Done")
sys.exit()
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper_left = Gripper("left")
self.gripper_left.calibrate()
self.gripper_left.set_moving_force(0.01)
rospy.sleep(0.5)
self.gripper_left.set_holding_force(0.01)
rospy.sleep(0.5)
self.gripper_right = Gripper("right")
self.gripper_right.calibrate()
rospy.sleep(1)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin_right = baxter_kinematics('right')
self.kin_left = baxter_kinematics('left')
self.J = self.kin_right.jacobian()
self.J_inv = self.kin_right.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.left_arm = Limb("left")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
self.pour_x = 0
self.pour_y = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
'observe_left':{
'left_w0': 2.761932405432129,
'left_w1': -1.5700293346069336,
'left_w2': -0.8893253607604981,
'left_e0': -0.9805972175354004,
'left_e1': 1.8300390778564455,
'left_s0': 1.4783739826354982,
'left_s1': -0.9503010970092775,
},
'stir':{
'right_e0': -0.179,
'right_e1': 1.403,
'right_s0': 0.381,
'right_s1': -0.655,
'right_w0': 1.3,
'right_w1': 2.04,
'right_w2': 0.612,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_midpoint':{
'right_e0': -0.606,
'right_e1': 0.968,
'right_s0': 0.0,
'right_s1': -0.645,
'right_w0': 0.465,
'right_w1': 1.343,
'right_w2': -0.437,
},
'dressing':{
'right_e0': 0.967,
'right_e1': 1.386,
'right_s0': -0.348,
'right_s1': -0.155,
'right_w0': 0.264,
'right_w1': 1.521,
'right_w2': -2.199,
},
}
import sys
import time
import rospy
from std_msgs.msg import UInt32
from msg_types import *
import baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('run_condition')
voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10)
time.sleep(1)
rightg = Gripper('right')
rightg.set_holding_force(50)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
# neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.6)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.6)
class IK(object):
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.6)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.6)
def neutral(self):
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def ik_calculate(self,limb,pos,rot):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
#rotation -0.5 is perp and -1.0 is parallel for rot[2]
quat = transformations.quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0], #depth
y=pos[1], #lateral
z=pos[2], #height
),
orientation=Quaternion(
quat[0],
quat[1],
quat[2],
quat[3],
),
),
)
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
# Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
resp.result_type)
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
#print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
# (seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
else:
pass
#print pose
#print("INVALID POSE - No Valid Joint Solution Found.")
return 0
class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
rospy.sleep(2)
# self.gripper.set_moving_force(0.1)
# rospy.sleep(0.5)
# self.gripper.set_holding_force(0.1)
# rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
}
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
ikreq.pose_stamp.append(poses['right'])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
if (resp.isValid[0]):
print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
#limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
#print limb_joints
arm = Limb('right')
i = 0
while i < 300:
arm.set_joint_positions(limb_joints)
i += 1
rospy.sleep(0.01)
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
#self.gripper.calibrate()
rospy.sleep(2)
# self.gripper.set_moving_force(0.1)
# rospy.sleep(0.5)
# self.gripper.set_holding_force(0.1)
# rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
}
class IKHelper(object):
"""An abstraction layer for using Baxter's built in IK service."""
############################################################################
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(
_IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16)
self._joint_update_pub.publish(250)
############################################################################
def reset(self):
"""Reset both arms to their neutral positions."""
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
"""Set the endpoint of the left arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait)
def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
"""Set the endpoint of the right arm to the supplied coordinates.
Arguments:
pos -- Position in space in (x, y, z) format.
rot -- Rotation in space in (r, p, y) format. (defaults to pointing
downwards.)
Keyword arguments:
wait -- If True, method will block until in position. (default
False)
"""
self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait)
def get_left(self):
"""Return the current endpoint pose of the left arm."""
return self._left_arm.endpoint_pose()['position']
def get_right(self):
"""Return the current endpoint pose of the left arm."""
return self._right_arm.endpoint_pose()['position']
def get_left_velocity(self):
"""Return the current endpoint velocity of the left arm."""
return self._left_arm.endpoint_velocity()['linear']
def get_right_velocity(self):
"""Return the current endpoint velocity of the right arm."""
return self._right_arm.endpoint_velocity()['linear']
def get_left_force(self):
"""Return the current endpoint force on the left arm."""
return self._left_arm.endpoint_effort()['force']
def get_right_force(self):
"""Return the current endpoint force on the right arm."""
return self._right_arm.endpoint_effort()['force']
############################################################################
def _set_arm(self, iksvc, limb, pos, rot, wait):
resp = self._get_ik(iksvc, pos, rot)
positions = resp[0]
isValid = resp[1]
if not isValid:
print('invalid: {0} {1} {2}'.format(x, y, z))
if not wait:
limb.set_joint_positions(positions)
else:
limb.move_to_joint_positions(positions)
def _get_ik(self, iksvc, pos, rot):
q = quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=pos[0],
y=pos[1],
z=pos[2],
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
iksvc.wait_for_service(5.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
