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 callback(self, request=True):
"""
Checks the status of the grasping components.
:param request: request from brain to check the handling
:return: True if all checks are good and 'False' otherwise
"""
# 1. Enable Arms
if not self.enable_arms():
return False
# 2. Untuck Arms
if not self.untuck_arms():
return False
# 3. Gripper Calibration
gripper = Gripper('left')
if not self.calibrate_grippers(gripper):
return False
gripper.open(block=True)
gripper = Gripper('right')
if not self.calibrate_grippers(gripper):
return False
gripper.open(block=True)
rospy.loginfo("All grasping checks were completed successfully.")
return True
def __init__(self, max_vscale=1.0):
self.grasp_queue = Queue()
initialized = False
while not initialized:
try:
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
self.left_arm.set_max_velocity_scaling_factor(max_vscale)
self.go_to_pose('left', L_HOME_STATE)
self.right_arm = moveit_commander.MoveGroupCommander(
'right_arm')
self.right_arm.set_max_velocity_scaling_factor(max_vscale)
self.go_to_pose('right', R_HOME_STATE)
self.left_gripper = Gripper('left')
self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE)
self.left_gripper.open()
self.right_gripper = Gripper('right')
self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE)
self.right_gripper.open()
initialized = True
except rospy.ServiceException as e:
rospy.logerr(e)
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 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 __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):
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, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control {left, right}
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._close_io = DigitalIO('%s_upper_button' % (arm, )) # 'dash' btn
self._open_io = DigitalIO('%s_lower_button' % (arm, )) # 'circle' btn
self._light_io = DigitalIO('%s_lower_cuff' % (arm, )) # cuff squeeze
# outputs
self._gripper = Gripper('%s' % (arm, ))
self._nav = Navigator('%s' % (arm, ))
# connect callback fns to signals
if self._gripper.type() != 'custom':
self._gripper.calibrate()
self._open_io.state_changed.connect(self._open_action)
self._close_io.state_changed.connect(self._close_action)
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
if lights:
self._light_io.state_changed.connect(self._light_action)
rospy.loginfo("%s Cuff Control initialized...",
self._gripper.name.capitalize())
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg', Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg', Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState, self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
def main():
global thetas
configureLeftHand()
planner = PathPlanner("right_arm")
grip = Gripper('right')
grip.calibrate()
raw_input("gripper calibrated")
grip.open()
table_size = np.array([3, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -5 - .112
#put cup
cup_pos = start_pos_xy
end_pos = goal_pos_xy
putCupObstacle(planner, cup_pos, "cup1")
putCupObstacle(planner, end_pos, "cup2")
planner.add_box_obstacle(table_size, "table", table_pose)
#move gripper to behind cup
position = cup_pos + np.array([-0.1, 0, -0.02])
orientation = np.array([0, np.cos(np.pi / 4), 0, np.cos(np.pi / 4)])
executePositions(planner, [position], [orientation])
raw_input("moved behind cup, press enter")
#move to cup and remove cup1 as obstacle since picking up
removeCup(planner, "cup1")
position = cup_pos + np.array([0, 0, -0.05])
executePositions(planner, [position], [orientation])
raw_input("moved to cup, press to close")
grip.close()
rospy.sleep(1)
raw_input("gripped")
moveAndPour(planner, end_pos)
raw_input("poured")
executePositions(planner, [position + np.array([0, 0, 0.02])],
[orientation])
grip.open()
removeCup(planner, "cup2")
def __init__(self, pr_init, pl_init):
self.centerx = 365
self.centery = 140
self.coefx = 0.1 / (526 - 369)
self.coefy = 0.1 / (237 - 90)
self.count = 0
self.right = MoveGroupCommander("right_arm")
self.left = MoveGroupCommander("left_arm")
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
self.gripper.set_holding_force(0.1)
self.pr = pr_init
self.pl = pl_init
self.angle = 0
self.position_list = (0.7, -0.3, -0.06, 0, 0.0, self.angle)
self.busy = False
self.blank_image = np.zeros((400, 640, 3), np.uint8)
cv2.putText(self.blank_image,
"DEMO", (180, 200),
cv2.FONT_HERSHEY_SIMPLEX,
fontScale=4,
color=255,
thickness=10)
self.movenum = 0
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.H = homography()
self.framenumber = 0
self.history = np.arange(0, 20) * -1
self.newPosition = True
self.bowlcamera = None
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 __init__(self):
rospy.loginfo("++++++++++++Ready to move the robot+++++++++++")
#Initialize moveit_commander
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
#Display Trajectory in RVIZ
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.moving_publisher = rospy.Publisher('amimoving',
String,
queue_size=10)
rospy.sleep(3.0)
# print self.robot.get_current_state()
# print ""
self.planning_frame = self.right_arm_group.get_planning_frame()
rospy.loginfo("============ Reference frame: %s" % self.planning_frame)
self.eef_link = self.right_arm_group.get_end_effector_link()
print "============ End effector: %s" % self.eef_link
self.group_names = self.robot.get_group_names()
print "============ Robot Groups:", self.robot.get_group_names()
#set planning and execution parameters
self.right_arm_group.set_goal_position_tolerance(0.01)
self.right_arm_group.set_goal_orientation_tolerance(0.01)
self.right_arm_group.set_planning_time(5.0)
self.right_arm_group.allow_replanning(False)
self.right_arm_group.set_max_velocity_scaling_factor(0.6)
self.right_arm_group.set_max_acceleration_scaling_factor(0.6)
#specify which gripper and limb are taking command
self.right_gripper = Gripper('right', CHECK_VERSION)
self.right_gripper.reboot()
self.right_gripper.calibrate()
# self.limb = baxter_interface.Limb('right')
# self.angles = self.limb.joint_angles()
# self.wave_1 = {'right_s0': -0.459, 'right_s1': -0.202, 'right_e0': 1.807, 'right_e1': 1.714, 'right_w0': -0.906, 'right_w1': -1.545, 'right_w2': -0.276}
# rospy.sleep(3)
# self.limb.move_to_joint_positions(self.wave_1)
self.pose_target = Pose()
self.standoff = 0.1
self.__right_sensor = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range, self.rangecallback)
self.rangestatetemp = Range()
self.box_name = 'table'
self.moving = '0'
self.moving_publisher.publish(self.moving)
def __init__(self, camera_averaging=False):
self._cvbr = CvBridge()
self.rs = RobotEnable()
self.ik = IKHelper()
camera_topic = '/cameras/{0}_camera/image_rect'
if camera_averaging:
camera_topic += '_avg'
self.left_img = None
self._left_camera_sub = rospy.Subscriber(
camera_topic.format('left_hand'), Image,
self._on_left_imagemsg_received)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
camera_topic.format('right_hand'), Image,
self._on_right_imagemsg_received)
self.head_img = None
self._head_camera_sub = rospy.Subscriber(
camera_topic.format('head'), Image,
self._on_head_imagemsg_received)
self.left_itb = None
self._left_itb_sub = rospy.Subscriber('/robot/itb/left_itb/state',
ITBState,
self._on_left_itbmsg_received)
self.right_itb = None
self._right_itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState,
self._on_right_itbmsg_received)
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.head = Head()
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
def __init__(self):
# create a RobotCommander
self.robot = RobotCommander()
# create a PlanningScene Interface object
self.scene = PlanningSceneInterface()
# create left arm move group and gripper
self.left_arm = Arm("left_arm")
self.left_gripper = self.left_arm.gripper = Gripper("left")
# create right arm move group and gripper
self.right_arm = Arm("right_arm")
self.right_gripper = self.right_arm.gripper = Gripper("right")
self.gripper_offset = 0.0
# reset robot
self.reset()
def calibrate_gripper(self):
""" Initialize the gripper and its vacuum sensor. """
self.gripper = Gripper(self.gripper_side)
self.gripper.calibrate()
self.gripper.open()
self.vacuum_sensor = AnalogIO(self.gripper_side +
'_vacuum_sensor_analog')
if rospy.get_param('verbose'):
rospy.loginfo('Calibrated gripper. (type={})'.format(
self.gripper.type()))
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, 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.loginfo("=============Ready to move the robot================")
#Initialize moveit_commander
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm")
#Display Trajectory in RVIZ
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
rospy.sleep(3.0)
self.planning_frame = self.right_arm_group.get_planning_frame()
rospy.loginfo( "============ Reference frame: %s" % self.planning_frame)
self.eef_link = self.right_arm_group.get_end_effector_link()
print "============ End effector: %s" % self.eef_link
self.group_names = self.robot.get_group_names()
print "============ Robot Groups:", self.robot.get_group_names()
#set planning and execution parameters
self.right_arm_group.set_goal_position_tolerance(0.01)
self.right_arm_group.set_goal_orientation_tolerance(0.01)
self.right_arm_group.set_planning_time(5.0)
self.right_arm_group.allow_replanning(False)
self.right_arm_group.set_max_velocity_scaling_factor(0.5)
self.right_arm_group.set_max_acceleration_scaling_factor(0.5)
#specify which gripper and limb are taking command
self.right_gripper = Gripper('right', CHECK_VERSION)
self.right_gripper.reboot()
self.right_gripper.calibrate()
self.pose_target=Pose()
self.startpose=Pose()
self.standoff=0.2
#Where to place the block
self.place_target=Pose()
self.placex_coord= 0.65
self.placey_coord=0
self.__right_sensor=rospy.Subscriber('/robot/range/right_hand_range/state',Range,self.rangecallback)
# self.location_data=rospy.Subscriber('square_location',String,self.compute_cartesian)
self.compute_cartesian()
self.rangestatetemp=Range()
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
def __init__(self):
left_gripper = Gripper('left')
## First initialize `moveit_commander`_ and a `rospy`_ node:
joint_state_topic = ['joint_states:=/robot/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints).
## This interface can be used to plan and execute motions:
group_name = "left_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
# get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
# get a list of all the groups in thPickAndPlaceTutoriale robot:
group_names = robot.get_group_names()
# get the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self.left_gripper = left_gripper
# List of all chess the moves to be executed by the robot
# will be filled in by subscribing to the topic where
# chess_moves_generator.py publishes moves
self.moves = []
def main():
factor=9
print("Right Init node")
rospy.init_node("Right_Ik_service_client", anonymous=True)
rate= rospy.Rate(1000)
right_gripper=Gripper('right')
right_gripper.calibrate()
right_gripper.open()
while not rospy.is_shutdown():
initial_position(round(6000*factor))#10ms
go_position(round(10000*factor),0.25)
for x in range (0,int(round(2000*factor))):
right_gripper.close()
#initial_position(10000)#3ms
go_box(round(250*factor))
for x in range (0,int(round(800*factor))):
right_gripper.open()
print "Objecto dejado"
def __init__(self, side_name):
"""Will configure and initialise Baxter's hand."""
self._side_name = side_name
# This is the reference to Baxter's arm.
self.limb = MoveGroupCommander("{}_arm".format(side_name))
self.limb.set_end_effector_link("{}_gripper".format(side_name))
# Unfortunetly, MoveIt was not able to make the gripper work, neither
# did was able to find a very good pose using Forward Kinematics,
# so instead the Baxter SDK is used here to do these two jobs.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self._limb = Limb(side_name)
# This solver seems to be better for finding solution among obstacles
self.limb.set_planner_id("RRTConnectkConfigDefault")
# Error tollerance should be as low as possible for better accuracy.
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
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 __init__(self, limb):
# init moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# instantiate things
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.arm = moveit_commander.MoveGroupCommander(limb)
# set up gripper things
# self.right_gripper = Gripper('right')
self.left_gripper = Gripper(
'left') # Assuming only using left gripper right now
# create pub to pub trajs for RVIZ
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting project "
def __init__(self, side, baxter=None):
self.gripper = Gripper(side)
self.prepare()
self.side = side
self.opened = True
self.outside_grip = True
self.grippd = False
self.state_pos = 100
self.weight_pub = rospy.Publisher(
"/robot/end_effector/" + side + "_gripper/command",
EndEffectorCommand)
self.baxter = baxter
self.id = 65537
self.touch_links = [
"pedestal", side + "_gripper", side + "_gripper_base",
side + "_hand_camera", side + "_hand_range", side + "_hand",
side + "_wrist"
]
rospy.sleep(1)
self.gripper_weight = 0.8
self.object_weight = 0.3
self.setWeight(0.8)
def __init__(self):
pygame.init()
rospy.init_node('node_mouse', anonymous=True)
right_gripper=Gripper('right')
left_gripper=Gripper('left')
limb_0=baxter_interface.Limb('right')
limb_0.set_joint_positions({'right_w2': 0.00})
def set_j(joint_name):
global delta
limb=baxter_interface.Limb('right')
current_position = limb.joint_angle(joint_name)
send=current_position+delta
if(send>2.80 or send<-2.80):
delta=-delta
time.sleep(0.15)
joint_command = {joint_name: send}
limb.set_joint_positions(joint_command)
#if(current_position-send>0):
# delta=-1
print(current_position)
print(delta)
pygame.display.set_caption('Game')
pygame.mouse.set_visible(True)
vec = Vector3(1, 2, 3)
right_gripper.calibrate()
left_gripper.calibrate()
try:
pub = rospy.Publisher('mouse', Vector3, queue_size=1)
rate = rospy.Rate(10) # 10hz
screen = pygame.display.set_mode((640,480), 0, 32)
pygame.mixer.init()
a=0 ,0 ,0
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEBUTTONUP:
a = pygame.mouse.get_pressed()
print a
if a[0]==1:
left_gripper.open()
else:
left_gripper.close()
if a[2]==1:
right_gripper.open()
else:
right_gripper.close()
if a[1]==1:
set_j('right_w2')
vec.x=a[0]
vec.y=a[1]
vec.z=a[2]
pub.publish(vec)
pygame.mixer.quit()
pygame.quit()
except rospy.ROSInterruptException:
pass
if __name__ == '__main__':
rospy.init_node('griptest')
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
rospy.wait_for_message("/robot/sim/started", Empty)
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
global right_gripper
#rospy.sleep(rospy.Duration(5,0))
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
right_gripper = Gripper('right')
right_limb = Limb('right')
right_gripper.calibrate()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
print "opening gripper"
right_gripper.open()
current_angles = [
right_limb.joint_angle(joint) for joint in right_limb.joint_names()
]
orient_quaternion_components = quaternion_from_euler(
math.pi, 0, math.pi / 2)
orient_down = Quaternion()
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,
},
}
def main():
"""
Main Script
"""
grip_right = Gripper('right')
# Make sure that you've looked at and understand path_planner.py before starting
imagePathFile = '/home/cc/ee106a/fa19/class/ee106a-afn/ros_workspaces/project/src/planning/src/images/our17.jpg'
planner = PathPlanner("right_arm")
# add_block_obstacle(planner, [0.963, -0.15, -0.1], 'table', [10, 10, 0])
# size = np.array([0.963, -0.15, -0.1])
# pose = PoseStamped()
# pose.header.frame_id = 'table'
# pose.pose.position.x = 10
# pose.pose.position.y = 10
# pose.pose.position.z = 0
# planner.add_box_obstacle(size, 'table', pose)
#gripper calibration
grip_right.calibrate()
grip_right.open()
# img_npy = 'distance_coordinates.npy'
# points = np.load(img_npy, "r")
# distBetween = Distance_Between('/home/cc/ee106a/fa19/class/ee106a-afn/ros_workspaces/project/src/planning/src/images/our16.jpg', 0.02426)
#for second test
distBetween = Distance_Between(imagePathFile, 0.02426)
points = distBetween.getTransformInfo()
# return None
# print(points)
# qx = p[2]
# qy = p[3]
# qz = p[4]
# qw = p[5]
#one for table
# size: 3x' ndarray; (x, y, z) size of the box (in the box's body frame)
# name: unique name of the obstacle (used for adding and removing)
# pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame
# pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame
# planner.add_box_obstacle()
# controller = Controller(Kp, Ki, Kd, Kw, Limb('right')
ar_x = arTransformStamp.transform.translation.x
ar_y = arTransformStamp.transform.translation.y
ar_z = arTransformStamp.transform.translation.z
print(points[0][0], points[0][1])
while not rospy.is_shutdown():
try:
for point in points:
#pose 1
pickup(ar_x, ar_y, ar_z)
dropoff(ar_x, ar_y, ar_z, p)
except Exception as e:
print e
traceback.print_exc()
