class MoveitPlanner:
def __init__(self, manipulator_side = 'left'):
# Enable the robot at start and disable at the end
BaxterRobot().EnableBaxterRobot()
rospy.on_shutdown(BaxterRobot().DisableBaxterRobot)
# Test Open Both Grippers and Close Them
# baxter_manipulator = BaxterManipulatorAndGripper()
# baxter_manipulator.SetupBaxterManipualtor()
# baxter_manipulator.MoveBaxterManipulatorToNeutralPose()
# baxter_manipulator.SetupBaxterGrippers()
# baxter_manipulator.OpenGripper('left')
# baxter_manipulator.OpenGripper('right')
# rospy.sleep(3.0)
# baxter_manipulator.CloseGripper('left')
# baxter_manipulator.CloseGripper('right')
# Referenceing baxter arm
self.__manipulator_name = manipulator_side
self.limb = MoveGroupCommander("{}_arm".format(manipulator_side))
self.limb.set_end_effector_link("{}_gripper".format(manipulator_side))
# Baxter Model configuration with MoveIt not configured to control its gripper,
# thus using Baxter SDK API gripper class
baxter_manipulator = BaxterManipulatorAndGripper()
baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side)
self.limb.set_planner_id("RRTConnectKConfigDefualt")
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
class MoveItArm:
"""
Represents Baxter's Arm in MoveIt! world.
Represents a hand of Baxter through MoveIt! Group. Some reusable code has
been packed together into this class to make the code more readable.
"""
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 __str__(self):
"""
String representation of the arm.
String representation of the arm, either 'left' or 'right' string will
be returned. Useful when the string representation of the arm is
needed, like when accessing the IK solver.
"""
return self._side_name
def is_left(self):
"""Will return True if this is the left arm, false otherwise."""
return True if self._side_name == "left" else False
def is_right(self):
"""Will return True if this is the right arm, false otherwise."""
return True if self._side_name == "right" else False
def open_gripper(self):
"""Will open Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.open(block=True)
def close_gripper(self):
"""Will close Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.close(block=True)
def init_mp(cls):
movegroup_ns = ManipulatorActions.get_ns()
robot_description = ManipulatorActions.get_robdesc_ns()
sc = PlanningSceneInterface(ns=movegroup_ns)
mg = MoveGroupCommander('arm',
ns=movegroup_ns,
robot_description=robot_description)
mg.set_max_acceleration_scaling_factor(0.5)
mg.set_max_velocity_scaling_factor(0.6)
mg.set_end_effector_link('tool_frame')
rospy.sleep(1.0)
sc.remove_world_object()
ManipulatorActions.rc = RobotCommander(ns=movegroup_ns)
ManipulatorActions.mg = mg
ManipulatorActions.sc = sc
ManipulatorActions.add_table_plane()
ManipulatorActions.measure_weight(calibrate=True)
ManipulatorActions.move_up(home=True)
ManipulatorActions.measure_weight(calibrate=True)
return mg, sc
class RobotArm:
"""
A class used to represent the arm of a robot
using both "Moveit" API and Baxter SDK API.
"""
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 __str__(self):
"""
Built-in function to return a printable string
representing the arm used in either print(object)
or str(object).
"""
return str(self.__side_name)
def open_gripper(self):
"""
Command the robot arm end-effector i.e. the gripper to open.
"""
self.gripper.open(block=True)
def close_gripper(self):
"""
Command the robot arm end-effector i.e. the gripper to close.
"""
self.gripper.close(block=True)
def is_left_arm(self):
"""
Return true if the arm is corresponding to the left arm.
"""
return self.__side_name == 'left'
def is_right_arm(self):
"""
Return true if the arm is corresponding to the right arm.
"""
return self.__side_name == 'right'
class pick_place():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.ur5 = MoveGroupCommander("manipulator")
self.gripper = MoveGroupCommander("end_effector")
print("======================================================")
print(self.robot.get_group_names())
print(self.robot.get_link_names())
print(self.robot.get_joint_names())
print(self.robot.get_planning_frame())
print(self.ur5.get_end_effector_link())
print("======================================================")
self.ur5.set_max_velocity_scaling_factor(0.2)
self.ur5.set_max_acceleration_scaling_factor(0.2)
self.ur5.set_end_effector_link("fts_toolside")
self.ur5.set_planning_time(10.0)
#self.ur5.set_planner_id("RRTkConfigDefault")
self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller')
self.gripper_ac.wait_for_server()
self.gripper_ac.initiate()
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def single_exuete(self, position, mode):
offset = 0.01
rospy.loginfo("let do a single exuete")
rospy.sleep(1)
position_copy = deepcopy(position)
position_copy += [0.14]
position_copy[1] = position_copy[1] + offset
pre_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
post_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
grasp_position = define_grasp(position_copy)
self.ur5.set_pose_target(pre_position)
rospy.loginfo("let's go to pre position")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
self.ur5.set_pose_target(grasp_position)
rospy.loginfo("let's do this")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
if mode == "pick":
self.gripper_ac.send_goal(0)
if mode == "place":
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
rospy.loginfo("I got this")
rospy.sleep(1)
self.ur5.set_pose_target(post_position)
rospy.loginfo("move out")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pair_exuete(self, pick_position, place_position):
rospy.loginfo("here we go pair")
if pick_position and place_position:
self.single_exuete(pick_position, "pick")
self.single_exuete(place_position, "place")
#rospy.sleep(1)
rospy.loginfo("let's go and get some rest")
rest_position = define_grasp([0.486, -0.152, 0.342])
self.ur5.set_pose_target(rest_position)
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pickplace_cb(self, msg):
#print(msg)
print(msg.data)
a = list(msg.data)
mean_x = np.mean([a[i] for i in range(0, len(a)-2, 2)])
mean_y = np.mean([a[i] for i in range(1, len(a)-2, 2)])
num_goals = (len(msg.data) -2)/2
rospy.loginfo("there is {} goals".format(num_goals))
for i in range(0, len(a)-2, 2):
pick_x, pick_y = coord_converter(msg.data[i], msg.data[i+1])
leeway_x = int(msg.data[i] - mean_x)
leeway_y = int(msg.data[i+1] - mean_y)
place_x, place_y = coord_converter(msg.data[-2] + leeway_x, msg.data[-1] + leeway_y)
print(pick_x, pick_y)
print(place_x, place_y)
self.pair_exuete([pick_x, pick_y], [place_x, place_y])
#!/usr/bin/env python
import rospy
from moveit_commander import MoveGroupCommander
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Pose
group = MoveGroupCommander("right_arm") # arm for fetch
group.set_end_effector_link('r_gripper_tool_frame')
group.set_pose_reference_frame('/base_footprint')
def pose_callback(pose):
pos = pose.position
qtr = pose.orientation
if qtr.x == 0 and qtr.y == 0 and qtr.z == 0 and qtr.w == 0:
group.set_position_target([pos.x, pos.y, pos.z])
else:
group.set_pose_target(pose)
group.go()
def joint_callback(msg):
goal = map(lambda joint_name: msg.position[msg.name.index(joint_name)],
group.get_active_joints())
group.set_joint_value_target(goal)
group.go()
if __name__ == "__main__":
rospy.init_node('movescratch')
#!/usr/bin/env python
import rospy
from moveit_commander import MoveGroupCommander
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Pose
group = MoveGroupCommander("right_arm") # arm for fetch
group.set_end_effector_link('r_gripper_tool_frame')
group.set_pose_reference_frame('/base_footprint')
def pose_callback(pose):
pos = pose.position
qtr = pose.orientation
if qtr.x == 0 and qtr.y == 0 and qtr.z == 0 and qtr.w == 0:
group.set_position_target([pos.x, pos.y, pos.z])
else: group.set_pose_target(pose)
group.go()
def joint_callback(msg):
goal = map(lambda joint_name: msg.position[msg.name.index(joint_name)],
group.get_active_joints())
group.set_joint_value_target(goal)
group.go()
if __name__ == "__main__":
rospy.init_node('movescratch')
rospy.Subscriber("movescratch/pose_goal", Pose, pose_callback)
rospy.Subscriber("movescratch/joint_goal", JointState, joint_callback)
rospy.spin()
