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])
from moveit_msgs.msg import MoveItErrorCodes
if __name__ == '__main__':
try:
rospy.init_node("fk_node")
robot = RobotCommander()
group_name = "manipulator"
group = MoveGroupCommander(group_name)
connection = rospy.ServiceProxy("/compute_fk", GetPositionFK)
rospy.loginfo("waiting for fk server")
connection.wait_for_service()
rospy.loginfo("server found")
request = GetPositionFKRequest()
request.fk_link_names = robot.get_link_names(group_name)
request.header.frame_id = robot.get_root_link()
request.robot_state.joint_state.name = group.get_joints()[0:-1]
# get joint state
joint_states= raw_input("enter joint state j1 j2 j3 j4 j5 j6 j7 \n")
joints= [float(idx) for idx in joint_states.split(' ')]
request.robot_state.joint_state.position = joints
response = connection(request)
if response.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("solution found")
rospy.loginfo(response.pose_stamped[-1])
else:
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Robot Links for arm:"
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print right_arm.get_end_effector_link()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
#scene.remove_world_object("part")
scene.remove_world_object("grasp_marker")
