def find_ik_and_execute(self, pose_transformed): x = pose_transformed.pose.position.x y = pose_transformed.pose.position.y z = pose_transformed.pose.position.z ik_candidate = ik_solver(x, y, z, -90) # print "========== Find ",len(ik_candidate)," Plan ==========" if not np.isnan(ik_candidate.all()): for theta_1, theta_2, theta_3, theta_4 in ik_candidate: # while not rospy.is_shutdown(): try: if self.execute_fk(theta_1, theta_2, theta_3, theta_4): # rospy.loginfo("========== Execute Plan ==========") # print [theta_1, theta_2, theta_3, theta_4] break except Exception as e: # rospy.loginfo(e) # print "------------- Failed -------------" # print [theta_1, theta_2, theta_3, theta_4],"\n" continue else: rospy.loginfo("========== Cannot Find Solution ==========") self._result.state = False self._as.set_aborted(self._result)
def pick_cb(self, req): br = tf.TransformBroadcaster() tf_name = req.tf print(tf_name) pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = tf_name.position.x pose_goal.position.y = tf_name.position.y pose_goal.position.z = tf_name.position.z - 0.005 print "Your object's position : ", pose_goal.position degree = -90 for j in range(20): joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y, pose_goal.position.z, degree) if len(joint_value) > 0: for joint in joint_value: joint = list(joint) # determine gripper state joint.append(0) try: self.move_group_arm.go(joint, wait=True) self.move_group_arm.stop() except: rospy.loginfo( str(joint) + " isn't a valid configuration.") continue grip_data = Float64() grip_data.data = 1.5 self.pub_gripper.publish(grip_data) rospy.sleep(2) self.home_4() rospy.sleep(2) grip_data.data = 0.0 self.pub_gripper.publish(grip_data) rospy.loginfo("End process") return pick_srvResponse(True) degree += 1 return pick_srvResponse(False)
def move(self, x, y, z): pose_goal = Pose() pose_goal.position.x = x pose_goal.position.y = y pose_goal.position.z = z # ik_4dof.ik_solver(x, y, z, degree) joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y, pose_goal.position.z, -90) for joint in joint_value: joint = list(joint) # determine gripper state joint.append(0) try: self.move_group.go(joint, wait=True) except: rospy.loginfo(str(joint) + " isn't a valid configuration.")
def processFeedback(self, feedback): p = feedback.pose.position self.tf_listener.waitForTransform("/base_link", "/world", rospy.Time(0), rospy.Duration(1)) pp = PointStamped() pp.point = p pp.header.frame_id = "/world" point_after = self.tf_listener.transformPoint("arm_base_link", pp) x = point_after.point.x #+ random.uniform(0.005, -0.005) y = point_after.point.y #+ random.uniform(0.005, -0.005) # print "x: ",x, " y: ",y z = point_after.point.z ik_candidate = ik_solver(x, y, z, -90) # print ik_candidate if not np.isnan(ik_candidate.all()): for theta_1, theta_2, theta_3, theta_4 in ik_candidate: if abs(theta_2) < 2 and abs(theta_3) < 2 and abs(theta_4) < 2: self.ik = [theta_1, theta_2, theta_3, theta_4] break
def handle_move_arm(req): # initial publisher for gripper command topic which is used for gripper control pub_gripper = rospy.Publisher("/gripper_joint/command", Float64, queue_size=1) check = True # you need to check if moveit server is open or not while (check): check = False try: # Instantiate a MoveGroupCommander object. This object is an interface to a planning group # In our case, we have "arm" and "gripper" group move_group = moveit_commander.MoveGroupCommander("arm") except: print "moveit server isn't open yet" check = True # First initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Instantiate a RobotCommander object. # Provides information such as the robot's kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() print "============ Planning frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() print "============ End effector link: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Available Planning Groups:", group_names # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state", robot.get_current_state() print "" ### Go home home() ############################ Method : Using IK to calculate joint value ############################ # After determining a specific point where arm should move, we input x,y,z,degree to calculate joint value for each wrist. pose_goal = Pose() pose_goal.position.x = req.x pose_goal.position.y = req.y pose_goal.position.z = -0.06 # ik_4dof.ik_solver(x, y, z, degree) joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y, pose_goal.position.z, -90) for joint in joint_value: joint = list(joint) # determine gripper state joint.append(0) try: move_group.go(joint, wait=True) except: rospy.loginfo(str(joint) + " isn't a valid configuration.") # Reference: https://ros-planning.github.io/moveit_tutorials/doc/move_group_python_interface/move_group_python_interface_tutorial.html ### close gripper grip_data = Float64() grip_data.data = 1.3 pub_gripper.publish(grip_data) ### Go home home() ### open gripper rospy.sleep(2) grip_data = Float64() grip_data.data = 0.5 pub_gripper.publish(grip_data) rospy.sleep(2)
def __init__(self): check = True # you need to check if moveit server is open or not while (check): check = False try: # Instantiate a MoveGroupCommander object. This object is an interface to a planning group # In our case, we have "arm" and "gripper" group move_group = moveit_commander.MoveGroupCommander("arm") except: print "moveit server isn't open yet" check = True # First initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # 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() # Create a DisplayTrajectory ROS publisher which is used to display trajectories in Rviz: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() print "============ Planning frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() print "============ End effector link: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Available Planning Groups:", robot.get_group_names( ) # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state", robot.get_current_state() print "" ############################ Method : Joint values (Go home!)############################ # We can get the joint values from the group and adjust some of the values: # Go home!!! joint_goal = move_group.get_current_joint_values() joint_goal[0] = 0 # arm_shoulder_pan_joint joint_goal[1] = -pi * 5 / 13 # arm_shoulder_lift_joint joint_goal[2] = pi * 3 / 4 # arm_elbow_flex_joint joint_goal[3] = pi / 3 # arm_wrist_flex_joint joint_goal[4] = 0 # gripper_joint # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop() ############################ Method : Using IK to calculate joint value ############################ # After determining a specific point where arm should move, we input x,y,z,degree to calculate joint value for each wrist. pose_goal = Pose() pose_goal.position.x = 0.141 pose_goal.position.y = 0.020 pose_goal.position.z = 0.042 # ik_4dof.ik_solver(x, y, z, degree) joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y, pose_goal.position.z, -90) for joint in joint_value: joint = list(joint) # determine gripper state joint.append(0) try: move_group.go(joint, wait=True) except: rospy.loginfo(str(joint) + " isn't a valid configuration.")
def transform(self, req): br = tf.TransformBroadcaster() tag = "tag_" + str(req.tag_id) try: now = rospy.Time.now() self.listener.waitForTransform('pi_camera', tag, now, rospy.Duration(3.0)) (trans, rot) = self.listener.lookupTransform('pi_camera', tag, now) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, \ tf.Exception): rospy.loginfo("Tag not found!") return tagResponse("Tag not found!") pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = trans[2] - 0.22 pose_goal.position.y = -trans[0] pose_goal.position.z = trans[1] + 0.01 x1 = pose_goal.position.x print "Your box's position : ", pose_goal.position # if 0.15 <= pose_goal.position.x <= 0.21: # if -0.07 <= pose_goal.position.y <= 0.065: # self.special() # self._return() # return tagResponse("Process Successfully") # elif -0.15 <= pose_goal.position.y <= 0.165: # return tagResponse("Cannot arrive") for l in range(6): pose_goal.position.x = x1 for i in range(5): degree = -90 for j in range(90): joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y, pose_goal.position.z, degree) if len(joint_value) > 0: for joint in joint_value: joint = list(joint) # determine gripper state joint.append(0) try: self.move_group_arm.go(joint, wait=True) self.move_group_arm.stop() except: rospy.loginfo( str(joint) + " isn't a valid configuration.") continue self._return() return tagResponse("Process Successfully") degree += 1 pose_goal.position.x -= 0.01 pose_goal.position.z += 0.01 return tagResponse("Cannot arrive objective pose!!")