def gotoInit(self): move_group = MoveGroupInterface("arm", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Plans the joints in joint_names to angles in pose init = self.trajActionGoal.trajectory.points[0].positions move_group.moveToJointPosition(joint_names, init, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Moved to Init") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.")
class GripperClient: def __init__(self): self.move_group = MoveGroupInterface("gripper", "base_link") self.joint_names = ['l_gripper_finger_joint', 'r_gripper_finger_joint'] def open_gripper(self): value = [0.1, 0.1] self.__execu(value) # Plans the joints in joint_names to angles in pose def close_gripper(self): value = [0.0, 0.0] self.__execu(value) # Plans the joints in joint_names to angles in pose def __execu(self, value): self.move_group.moveToJointPosition(self.joint_names, value, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: print 'gripper' else: # If you get to this point please search for: rospy.logerr( "Arm goal in state: %s", self.move_group.get_move_action().get_joint_values()) else: rospy.logerr("MoveIt! failure no result returned.")
def set_init_pose(self): #set init pose move_group = MoveGroupInterface( "manipulator", "base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] # pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007] # move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす # move_group.get_move_action().wait_for_result() #wait result # result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル self.state = { 'default': 0, 'hold': 1, 'way_standby': 2, 'plan_standby': 3 } # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.now_state = self.state['default'] #現在フレームの状態
class TuckThread(Thread): def __init__(self, untuck=False): Thread.__init__(self) self.client = None self.untuck = untuck self.start() def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] if not self.untuck: pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] else: pose = [0.05, 0.29, -0.60, -0.51, 1.45, 0.52, 0.88, -1.97] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return def stop(self): if self.client: self.client.get_move_action().cancel_goal() # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("failed") sys.exit(0)
def main(args): rospy.init_node('simple_move') move_group = MoveGroupInterface('manipulator', "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ 'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] # This is the wrist link not the gripper itself gripper_frame = "wrist_3_link" poses, pose_name = get_pose(args.reset) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = "base_link" while not rospy.is_shutdown(): for pose in poses: # Finish building the Pose_stamped message # If the message stampe is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: log_output = pose_name + " Done!" rospy.loginfo(log_output) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
def set_init_pose(self): #set init pose move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす move_group.get_move_action().wait_for_result() #wait result result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
class Arm: def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6) def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw): orientation = tf.transformations.quaternion_from_euler( Roll, Pitch, Yaw) self.poseStamped = PoseStamped() self.poseStamped.header.frame_id = 'base_link' self.poseStamped.header.stamp = rospy.Time.now() self.poseStamped.pose.position.x = X self.poseStamped.pose.position.y = Y self.poseStamped.pose.position.z = Z self.poseStamped.pose.orientation.x = orientation[0] self.poseStamped.pose.orientation.y = orientation[1] self.poseStamped.pose.orientation.z = orientation[2] self.poseStamped.pose.orientation.w = orientation[3] self.moveGroup.moveToPose(self.poseStamped, 'gripper_link', 0.005, max_velocity_scaling_factor=0.2) self.result = self.moveGroup.get_move_action().get_result() def Tuck(self): joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] self.moveGroup.moveToJointPosition(joints, pose, 0.01, max_velocity_scaling_factor=0.2) while not rospy.is_shutdown(): self.result = self.moveGroup.get_move_action().get_result() if self.result.error_code.val == MoveItErrorCodes.SUCCESS: break def AddDice(self, name, X, Y, Z): self.planning_scene.addCube(name, 0.1, X, Y, Z) def RemoveDice(self, name): self.planning_scene.removeCollisionObject(name)
def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator","base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals()
class TuckThread(Thread): def __init__(self): Thread.__init__(self) self.client = None self.start() def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return def stop(self): if self.client: self.client.get_move_action().cancel_goal() # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("failed") sys.exit(0)
class RobotArm: def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.removeCollisionObject("table1") def MoveToPose(self,X,Y,Z,x,y,z,w): self.poseStamped = PoseStamped() self.poseStamped.header.frame_id = 'base_link' self.poseStamped.header.stamp = rospy.Time.now() self.poseStamped.pose.position.x = X self.poseStamped.pose.position.y = Y self.poseStamped.pose.position.z = Z self.poseStamped.pose.orientation.x = x self.poseStamped.pose.orientation.y = y self.poseStamped.pose.orientation.z = z self.poseStamped.pose.orientation.w = w self.moveGroup.moveToPose(self.poseStamped, 'gripper_link') self.result = self.moveGroup.get_move_action().get_result() def OriginReturn(self): #Move to the origin joints = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] self.moveGroup.moveToJointPosition(joints, pose, 0.02) while 1: self.result = self.moveGroup.get_move_action().get_result() if self.result.error_code.val == MoveItErrorCodes.SUCCESS: break
def arm_move(self): move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) #planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_left_ground", 1, 1.5, self.translation[2][0], self.translation[2][1], self.translation[2][2]) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Lists of joint angles in the same order as in joint_names disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]] for pose in disco_poses: if rospy.is_shutdown(): break # Plans the joints in joint_names to angles in pose move_group.moveToJointPosition(joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Disco!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
class ArmController(object): def __init__(self): # Create move group interface for a fetch robot self._move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground self._planning_scene = PlanningSceneInterface("base_link") self._planning_scene.removeCollisionObject("my_front_ground") self._planning_scene.removeCollisionObject("my_back_ground") self._planning_scene.removeCollisionObject("my_right_ground") self._planning_scene.removeCollisionObject("my_left_ground") self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) def SetPose(self, target_pose): # Plans the joints in joint_names to angles in pose self._move_group.moveToJointPosition(target_pose.keys(), target_pose.values(), wait=False) # Since we passed in wait=False above we need to wait here self._move_group.get_move_action().wait_for_result() result = self._move_group.get_move_action().get_result() return_result = False if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("MoveIt pose reached.") return_result = True else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self._move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops self._move_group.get_move_action().cancel_all_goals() return return_result def SetPoseWithRetry(self, target_pose, max_time=3): for _ in range(max_time): if self.SetPose(target_pose): return True return False
def main(args): rospy.init_node('simple_move') move_group = MoveGroupInterface('manipulator', "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ 'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] poses, pose_name = get_pose(args.case, args.reset) for pose in poses: if rospy.is_shutdown(): break move_group.moveToJointPosition(joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: log_output = pose_name + " Done!" rospy.loginfo(log_output) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
class PoseController(object): def __init__(self): # Create move group interface for a fetch robot self.move_group_ = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground self.planning_scene_ = PlanningSceneInterface("base_link") self.planning_scene_.removeCollisionObject("my_front_ground") self.planning_scene_.removeCollisionObject("my_back_ground") self.planning_scene_.removeCollisionObject("my_right_ground") self.planning_scene_.removeCollisionObject("my_left_ground") self.planning_scene_.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene_.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene_.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene_.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.angle = np.zeros(FILTER_LENGTH) self.last_trigger_time = rospy.Time.now() rospy.Subscriber(AUDIO_TOPIC, SoundSourceAngle, self.SetPoseWithRetry) rospy.spin() def SetPoseWithRetry(self, msg, max_time=3): if msg.is_valid: direction = 0 (direction, self.angle) = TemporalMedian(msg.angle, self.angle) soulder_pan = round((direction / 180.0) * np.pi, 2) target_pose = { 'torso_lift_joint': 0.05287206172943115, 'shoulder_pan_joint': soulder_pan, 'shoulder_lift_joint': 0.0, 'upperarm_roll_joint': -np.pi, 'elbow_flex_joint': 0.0, 'forearm_roll_joint': np.pi, 'wrist_flex_joint': -1.5, 'wrist_roll_joint': 0.0, } if rospy.Time.now() - self.last_trigger_time > rospy.Duration(2): rospy.loginfo("Shoulder pan") rospy.loginfo(soulder_pan) for _ in range(max_time): if self.SetPose(target_pose): return True return False def SetPose(self, target_pose): # Plans the joints in joint_names to angles in pose self.move_group_.moveToJointPosition(target_pose.keys(), target_pose.values(), wait=False) # Since we passed in wait=False above we need to wait here self.move_group_.get_move_action().wait_for_result() result = self.move_group_.get_move_action().get_result() return_result = False if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Done!") return_result = True else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group_.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops self.move_group_.get_move_action().cancel_all_goals() self.last_trigger_time = rospy.Time.now() return return_result
class FetchPose: def __init__(self): self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso") self.joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # planning_scene = PlanningSceneInterface("base_link") # planning_scene.removeCollisionObject("my_front_ground") # planning_scene.removeCollisionObject("my_back_ground") # planning_scene.removeCollisionObject("my_right_ground") # planning_scene.removeCollisionObject("my_left_ground") # planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) # planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) # planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) # planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.tuck_the_arm_joints_value = [ 0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0 ] self.stow_joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] # self.high_pose_value = [0.38380688428878784, -0.12386894226074219, -0.31408262252807617, 2.1759514808654785, 0.0061359405517578125, 0.9556700587272644, -1.921694278717041, -1.6908303499221802] self.high_pose_value = [ 0.4946120488643647, -0.19136428833007812, -0.4793691635131836, 0.009587380103766918, 0.1629854440689087, 0.2983585298061371, 1.8430781364440918, -1.6992675065994263 ] self.trans = Transformer() def excute(self, joints_value): if rospy.is_shutdown(): return False # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, joints_value, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("pose Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") def tuck_the_arm(self): self.excute(self.tuck_the_arm_joints_value) def stow(self): self.excute(self.stow_joints_value) def hige_pose(self): self.excute(self.high_pose_value) def control_torso(self, offset=0.1): joints_values = self.pose_group.get_current_joint_values() joints_values[0] += offset self.excute(joints_values) def grasp_by_move_group(self, point_xyz): wrist_roll_link_gripper_offset = 0.20 q1 = self.trans.quaternion_from_direction((1, 0, 0), (0, 0, -1)) q2 = self.trans.quaternion_from_axis(90, (1, 0, 0)) quaternion = quaternion_multiply(q1, q2) pose = Pose( Point(point_xyz[0], point_xyz[1], point_xyz[2] + wrist_roll_link_gripper_offset), # offset Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = pose gripper_frame = 'wrist_roll_link' self.move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: print("reach success!") return 'Success' else: rospy.logerr("reach fail") return 'Fail' else: rospy.logerr("MoveIt! failure no result returned.") return 'Fail'
class GraspingClient(object): def __init__(self): self.arm = MoveGroupInterface('arm_with_torso', 'map') self.move_group = MoveGroupInterface('arm', 'base_link') def pick(self, target_pose): #print self.arm._fixed_frame, self.arm.planner_id #Taking coordinate values of the object object_x, object_y, object_z = round( target_pose.pose.position.x, 2), round(target_pose.pose.position.y, 2), round(target_pose.pose.position.z, 2) print object_x, object_y, object_z #Initializing offset values for translation offset_x, offset_y, offset_z = 0, 0, 0.05 #Initializing offset values for rotation roll, pitch, yaw = 0, 0, 0 #Applying rotation to the arm for i in range(10, 1, -1): #Adding translation along x-axis final_x = object_x + offset_x #Adding translation along y-axis final_y = object_y + offset_y #Adding translation along z-axis final_z = object_z + offset_z + i * 0.1 print final_x, final_y, final_z, i * 0.1 final_quaternion = quaternion_from_euler(radians(roll), radians(pitch), radians(yaw), 'sxyz') pose_goal = PoseStamped() pose_goal.header.frame_id = 'map' pose_goal.pose.position.x = final_x pose_goal.pose.position.y = final_y pose_goal.pose.position.z = final_z pose_goal.pose.orientation.x = final_quaternion[0] pose_goal.pose.orientation.y = final_quaternion[1] pose_goal.pose.orientation.z = final_quaternion[2] pose_goal.pose.orientation.w = final_quaternion[3] pose_goal.header.stamp = rospy.Time.now() self.arm.moveToPose(pose_goal, 'wrist_roll_link') result = self.arm.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo('Moved arm') rospy.sleep(10) else: rospy.logerr('Cannot move arm') else: rospy.logerr('No result') def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo('Tucking done')
def main(): rospy.init_node('move_group_python_path_follow', anonymous=True) planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) moveit_commander.roscpp_initialize(sys.argv) move_group = MoveGroupInterface("arm", "base_link") robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("arm") print "============ Robot Groups:" print robot.get_group_names() print "============ Printing robot state" print robot.get_current_state() print "============" gripper_frame = 'wrist_roll_link' gripper_poses = list() radius = 0.2 for theta in np.arange(-pi, pi, 0.5 * pi): gripper_poses.append( Pose( Point(0.75, radius * np.sin(theta), 0.6 + radius * np.cos(theta)), Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)))) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = gripper_poses[0] move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) trajectory = RobotTrajectory() (plan, fraction) = group.compute_cartesian_path(gripper_poses, 0.01, 0.0) group.execute(plan) move_group.get_move_action().cancel_all_goals()
[0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [0.385, -1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0], [0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0], [0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0], [0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]] for pose in disco_poses: if rospy.is_shutdown(): break # Plans the joints in joint_names to angles in pose move_group.moveToJointPosition(joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Disco!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals
class TuckThread(Thread): def __init__(self): Thread.__init__(self) self.client = None self.start() def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() else: rospy.loginfo("moveit already started") rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") keepout_pose = Pose() keepout_pose.position.z = 0.375 keepout_pose.orientation.w = 1.0 ground_pose = Pose() ground_pose.position.z = -0.03 ground_pose.orientation.w = 1.0 rospack = rospkg.RosPack() mesh_dir = os.path.join(rospack.get_path('fetch_teleop'), 'mesh') scene.addMesh('keepout', keepout_pose, os.path.join(mesh_dir, 'keepout.stl')) scene.addMesh('ground', ground_pose, os.path.join(mesh_dir, 'ground.stl')) joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition( joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") scene.removeCollisionObject("ground") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return def stop(self): if self.client: self.client.get_move_action().cancel_goal() # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("failed") sys.exit(0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = ['l_gripper_finger_joint', 'r_gripper_finger_joint'] # Lists of joint angles in the same order as in joint_names disco_poses = [[0.01, 0.01]] for pose in disco_poses: if rospy.is_shutdown(): break # Plans the joints in joint_names to angles in pose move_group.moveToJointPosition(joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: pass else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_joint_values()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals
planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] Q = [0, 0, 0, 0.0, -np.pi / 2, 0.0, np.pi / 2, 0.0] move_group.moveToJointPosition(joint_names, Q, wait=False) move_group.get_move_action().wait_for_result() # position: # x: 0.640036865808 # y: -0.0391988119345 # z: 1.11652027628 # orientation: # x: 0.00118531019309 # y: 0.00278754644384 # z: -0.0268161732665 # w: 0.999635792414 L = np.array([[-0.086875, 0, 0.37743], [0.119525, 0, 0.34858], [0.117, 0, 0.06], [0.219, 0, 0], [0.133, 0, 0], [0.197, 0, 0], [0.1245, 0, 0], [0.1385, 0, 0], [0.16645, 0, 0]])
from geometry_msgs.msg import Pose, Point, Quaternion pose = geometry_msgs.msg.PoseStamped() pose.header.frame_id = '/base_link' pose.pose = Pose(Point(0.042, 0.384, 0.826), Quaternion(0, 0, 0, 1)) world_manager.world_manager_client.add_mesh( 'spam', '/home/vlad/Downloads/spam_12oz.dae', pose) ### Using MoveIt to move arm -- this works by itsels, without using CURPP. The arm moves to this location. import rospy from moveit_msgs.msg import MoveItErrorCodes from moveit_python import MoveGroupInterface, PlanningSceneInterface from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion rospy.init_node("hi") move_group_arm = MoveGroupInterface("arm_with_torso", "base_link") # This is the wrist link not the gripper itself gripper_frame = 'wrist_roll_link' # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'spam' gripper_pose_stamped.pose = graspit_grasps.grasps[2].pose # Move gripper frame to the pose specified move_group_arm.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group_arm.get_move_action().get_result()
move_group_gripper = MoveGroupInterface("gripper_custom", "gripper_link") gripper_frame = 'wrist_roll_link' Lwrist2gripper = 0.12 X_pg = [X[0] - Lwrist2gripper - 0.20, X[1], X[2]] X_g = [X[0] - Lwrist2gripper, X[1], X[2]] X_up = [X_g[0], X_g[1], X_g[2] + 0.2] print(X_pg) print(X_g) # Open gripper move_group_gripper.moveToJointPosition( ['l_gripper_finger_joint', 'l_gripper_finger_joint'], [0.040, 0.040], wait=False) move_group_gripper.get_move_action().wait_for_result() result = move_group_gripper.get_move_action().get_result() gripper_pose_pregrasp = create_pose_stamped_msg(X_pg, Q, 'base_link') gripper_pose_grasp = create_pose_stamped_msg(X_g, Q, 'base_link') gripper_pose_up = create_pose_stamped_msg(X_up, Q, 'base_link') # # Create pre grasp pose msg # gripper_pose_pregrasp = PoseStamped() # gripper_pose_pregrasp.header.frame_id = "base_link" # # do position and orientation seperately # gripper_pose_pregrasp.pose.position = object_pose.g_pos.position # gripper_pose_pregrasp.pose.orientation = object_pose.g_pos.orientation # gripper_pose_pregrasp.pose.position.x = g_pos_x - Lwrist2gripper - 0.20
try: t = listener.getLatestCommonTime(frame, "/map") trans, rot = listener.lookupTransform(frame, "/map", t) pos.append(trans) ori.append(rot) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): break if len(pos) == 8: pos = np.array(pos) acc = MotionPolicy(oldStates, pos, obstacle, goal) oldStates = pos acc = np.multiply(acc, ((rospy.get_time() - time)**2) / 2.0) time = rospy.get_time() poses = np.add(pos, acc) pose = poses[-2] rot = ori[-2] gripper_pose = Pose(Point(pose[0], pose[1], pose[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = "/map" gripper_pose_stamped.header.stamp = t gripper_pose_stamped.pose = gripper_pose move_group.moveToPose(gripper_pose_stamped, frames[-2][1:]) rate.sleep() move_group.get_move_action().cancel_all_goals()
class FetchClient(object): def __init__(self): self.bfp = True self.cx = 320 self.cy = 240 self.ax = 57.5 self.ay = 45 self.fx = self.cx / math.tan(self.ax * math.pi / 180.0 / 2) self.fy = self.cy / math.tan(self.ay * math.pi / 180.0 / 2) self.robot = robot_interface.Robot_Interface() self.br = tf.TransformBroadcaster() self.move_group = MoveGroupInterface("arm", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm") self.cam = camera.RGBD() while True: try: cam_info = self.cam.read_info_data() if cam_info is not None: break except: rospy.logerr('info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info) def broadcast_position(self, position, name): pass def calc_3D_position(self, u, v): dep_data = self.robot.get_img_data()[1] z = self.robot.get_depth((u, v), dep_data) if z == 0: return None, None, None x = (u - self.cx) * z / self.fx y = (v - self.cy) * z / self.fy return x, y, z def broadcast_from_pix(self, point, name): while self.bfp: # dep = self.robot.get_depth(point, dep_data) # while dep == 0: # print("depth info error") # dep = self.robot.get_depth(point, dep_data) # # td_points = self.pcm.projectPixelTo3dRay(point) # norm_pose = np.array(td_points) # norm_pose = norm_pose / norm_pose[2] # norm_pose = norm_pose * (dep) norm_pose = self.calc_3D_position(point[0], point[1]) a = tf.transformations.quaternion_from_euler(ai=-2.355, aj=-3.14, ak=0.0) b = tf.transformations.quaternion_from_euler(ai=0.0, aj=0.0, ak=1.57) base_rot = tf.transformations.quaternion_multiply(a, b) self.br.sendTransform(norm_pose, base_rot, rospy.Time.now(), name, 'head_camera_rgb_optical_frame') def get_position_of_baselink(self, name): listener = tf.TransformListener() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: pose, rot = listener.lookupTransform('base_link', name, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue rate.sleep() if pose: # rot = tf.transformations.quaternion_from_euler(0*DEG_TO_RAD, 90*DEG_TO_RAD, 0*DEG_TO_RAD) return pose def create_tf_of_base_link(self, name, position, rot): quat = tf.transformations.quaternion_from_euler(ai=rot[0], aj=rot[1], ak=rot[2]) while True: self.br.sendTransform(position, quat, rospy.Time.now(), name, 'base_link') def show_image_with_point(self, im): plt.imshow(im) pylab.show() def reach(self, point_xyz): pose = Pose(Point(point_xyz[0], point_xyz[1], point_xyz[2]), Quaternion(0, 0.707106781187, 0, 0.707106781187)) # pose = Pose(Point(0.57503179279, 0.170263536187, 0.97855338914), Quaternion(0, 0.707106781187, 0, 0.707106781187)) print(pose) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = pose gripper_frame = 'wrist_roll_link' # gripper_frame = 'l_gripper_finger_link' self.move_group.moveToPose(gripper_pose_stamped, gripper_frame, plan_only=True) result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode print("plan0 success!") height = result.planned_trajectory.joint_trajectory.points[0] gripper_pose_stamped.pose.position.z += height self.move_group.moveToPose(gripper_pose_stamped, gripper_frame, plan_only=True) result1 = self.move_group.get_move_action().get_result() if result1: if result1.error_code.val == MoveItErrorCodes.SUCCESS: print("plan1 success!") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] joint_pose = result1.planned_trajectory.joint_trajectory[ 0].position[6:13] print joint_pose self.move_group.moveToJointPosition(joint_names, joint_pose, wait=False) result2 = self.move_group.get_move_action().get_result() if result2: # Checking the MoveItErrorCode if result2.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("reach!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("reach fail") else: rospy.logerr("MoveIt! failure no result returned.") else: rospy.logerr("plan1 fail") else: rospy.logerr("MoveIt! failure no result returned.") else: rospy.logerr("MoveIt! failure no result returned.")
def move(): rospy.init_node("move") # #target pose # robot = moveit_commander.RobotCommander() print "group name ",robot.get_group_names() #print "current state ",robot.get_current_state() group = moveit_commander.MoveGroupCommander("manipulator") group_initial_pose = group.get_current_pose().pose print "group initaial pose ",group_initial_pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = 0.0 pose_target.orientation.y = 0.0 pose_target.orientation.z = 0.0 pose_target.orientation.w = 1.0 pose_target.position.x = 0.5 pose_target.position.y = 0.5 pose_target.position.z = 0.9 group.set_pose_target(pose_target) group.go() # #joint pose # move_group = MoveGroupInterface("manipulator","base_link") #planning_scene = PlanningSceneInterface("base_link") #setting up to initial robot state # TF joint names joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] # Lists of joint angles in the same order as in joint_names pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() # #cartesian path # #moveing along with catesian paths group = moveit_commander.MoveGroupCommander("manipulator") waypoints = [] # start with the current pose waypoints.append(group.get_current_pose().pose) # first orient gripper and move wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z - 0.05 waypoints.append(copy.deepcopy(wpose)) # second move wpose.position.x += 0.03 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = group.compute_cartesian_path(waypoints,0.01,0.0) #waypoints to follow, eef_setup, jump_threshold group.execute(plan)
class Node: def __init__(self, image_topic, camera_info_topic, camera_frame, published_point_num_topic, published_point_base_topic, torso_movement_topic, head_movement_topic, num_published_points, max_spine_height, min_spine_height, spine_offset): ''' Description: establishes the variabes included in the class Input: self <Object>, image_topic <Object>, camera_info_topic <String>, camera_frame <String>, published_point_num_topic <String>, published_point_base_topic <String>, torso_movement_topic <String>, head_movement_topic <String>, num_published_points <Int>, max_spine_height <Int>, min_spine_height <Int>, spine_offset <Int> Return: None ''' self.camera_frame = camera_frame self.published_point_base_topic = published_point_base_topic self.bridge = CvBridge() self.tf = TransformListener() rospy.Subscriber(image_topic, Image, self.image_callback) rospy.Subscriber(camera_info_topic, CameraInfo, self.camera_info_callback) self.robot_pose = None self.img = None self.pc = None self.camera_info = None self.num_published_points = num_published_points self.published_points = [[0, 0, 0] for i in range(num_published_points)] for i in range(num_published_points): rospy.Subscriber(self.published_point_base_topic + str(i), PointStamped, self.point_published_callback, i) self.points_registered = sets.Set() # base movement self.base_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction) self.base_client.wait_for_server() # torso movement self.max_spine_height = max_spine_height self.min_spine_height = min_spine_height self.spine_offset = spine_offset # head movement self.point_head_client = actionlib.SimpleActionClient( head_movement_topic, PointHeadAction) self.point_head_client.wait_for_server() # keyboard listener self.keypress_sub = rospy.Subscriber('/key_monitor', String, self.key_callback) rospy.loginfo("move group") self.move_group = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("move group end") def robot_pose_callback(self, data): ''' Description: reset the ROS node pose to the stored value in data Input: self <Object>, ar_tag_frame <String>, point topic <Object> Return: None ''' self.robot_pose = data def image_callback(self, data): ''' Description: reset the ROS node img data to the stored value in data Input: self <Object>, data <Object> Return: None ''' try: self.img = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) def move_base_to(self, x, y, theta): ''' Description: sends a position goal to the ROS node to move the robot base to Input: self <Object>, x <Int>, y <Int>, theta <Float> Return: None ''' goal = MoveBaseGoal() goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.header.frame_id = "map" goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y quat = transformations.quaternion_from_euler(0, 0, theta) orientation = Quaternion() orientation.x = quat[0] orientation.y = quat[1] orientation.z = quat[2] orientation.w = quat[3] goal.target_pose.pose.orientation = orientation # send goal self.base_client.send_goal(goal) def move_torso(self, pose): ''' Description: calls the ROS node to adjust the robot torso from its current position to the input Input: self <Object>, pose <Array> Return: perform movement ''' joint_names = [ 'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] poses = [ pose, 1.3192424769714355, 1.4000714648620605, -0.20049656002880095, 1.5290160491638183, -0.0004613047506046297, 1.660243449769287, -0.00012475593825578626 ] self.move_group.moveToJointPosition(joint_names, poses, wait=False) # plan self.move_group.get_move_action().wait_for_result() return self.move_group.get_move_action().get_result() def look_at(self, frame_id, x, y, z): ''' Description: sends the ROS node a goal position to move the head to face Input: self <Object>, frame_id <Int>, x <Int>, y <Int>, z <Int> Return: None ''' goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = frame_id goal.target.point.x = x goal.target.point.y = y goal.target.point.z = z goal.pointing_frame = "pointing_frame" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 # send goal self.point_head_client.send_goal(goal) def camera_info_callback(self, data): ''' Description: resets the current camera info to the stored data Input: self <Object>, data <Object> Return: None ''' self.camera_info = data def key_callback(self, keypress): ''' Description: sends a signal to cancel the current navigation goal Input: self <Object>, keypress <Object> Return: None ''' if keypress.data == "k": self.base_client.cancel_goal() def point_published_callback(self, data, point_id): ''' Description: registers a point in the ROS node Input: self <Object>, data <Object>, point_id <Int> Return: None ''' self.points_registered.add(point_id) self.published_points[point_id][0] = data.point.x self.published_points[point_id][1] = data.point.y self.published_points[point_id][2] = data.point.z def sample_position(self, x_center, y_center, sample_max_radius, sample_min_radius): ''' Description: generates a random position within a radius as a goal point for the robot Input: self <Object>, x_center <Int>, y_center <Int>, sample_max_radius <Int>, sample_min_radius <Int> Return: generated position <Array> ''' min_x = x_center - sample_max_radius max_x = x_center - sample_min_radius * math.sin(.1745) sampled_theta = random.random() * (2 * math.pi) sampled_r = random.random() * (sample_max_radius - sample_min_radius) + sample_min_radius sampled_x = sampled_r * math.cos(sampled_theta) + x_center sampled_y = sampled_r * math.sin(sampled_theta) + y_center sampled_z = random.random() * ( self.max_spine_height - self.min_spine_height) + self.min_spine_height x_diff = sampled_x - x_center y_diff = sampled_y - y_center theta = math.atan2(y_diff, x_diff) + math.pi if theta > (2 * math.pi): theta = theta - (2 * math.pi) if theta < 0: theta = theta + (2 * math.pi) position = [sampled_x, sampled_y, theta, sampled_z] return position def get_img(self): return self.img def get_pc(self): return self.pc def rviz_point_tl_tf(self): return self.rviz_point_tl_tf def rviz_point_br_tf(self): return self.rviz_point_br_tf
class Robot(object): """ fetch max x = 0.96 (wrist roll joint) """ def __init__(self): rospy.loginfo("Waiting for Fetch ...") self.tuck_the_arm_joints_value = [ 0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0 ] self.stow_joints_value = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso") self.joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] def move_to_pose(self, postion, quaternion, frame='base_link'): target_frame = 'wrist_roll_link' pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose( Point(postion[0], postion[1], postion[2]), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Move success!") return True else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) return False else: rospy.logerr("MoveIt! failure no result returned.") return False def move_to_joints(self, joints_value): # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, joints_value, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("move to joint Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") def tuck_the_arm(self): self.move_to_joints(self.tuck_the_arm_joints_value) def stow(self): self.move_to_joints(self.stow_joints_value) def stop(self): """This stops all arm movement goals It should be called when a program is exiting so movement stops """ self.move_group.get_move_action().cancel_all_goals()
class GripperController(object): def __init__(self): self.client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_action...") self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.goal = GripperCommandGoal() self.client.wait_for_server() def close(self): '''Close the gripper''' self.goal.command.position = 0 self.goal.command.max_effort = 100 self.client.send_goal(self.goal) self.client.wait_for_result() def release(self): '''Release the gripper''' self.goal.command.position = 1 self.goal.command.max_effort = 100 self.client.send_goal(self.goal) self.client.wait_for_result() def tuck(self): '''Tuck the arm of fetch''' position = (0.0555, -0.138, 0.571) quaternion = (0.45848, -0.50438, 0.5078, 0.5268) self.move_to_pose(position, quaternion) def move_to_pose(self, postion, quaternion, frame='base_link'): target_frame = 'wrist_roll_link' pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose( Point(postion[0], postion[1], postion[2]), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Move success!") else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # self.move_group.get_move_action().cancel_all_goals() def move_to_some_pose(self, postion, direction, frame='base_link'): target_frame = 'wrist_roll_link' quatrenion = Transformer().quaternion_from_direction((1, 0, 0), direction) pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose(Point(postion[0], postion[1], postion[2]), quatrenion) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("See!") else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") self.move_group.get_move_action().cancel_all_goals() def wave(self): '''Just for fun''' gripper_poses = [ ((0.347, 0.045, 1.022), (-0.174, -0.301, 0.273, 0.635)), ((0.347, 0.245, 1.022), (-0.274, -0.701, 0.173, 0.635)) ] for i in range(5): for pose in gripper_poses: self.move_to_pose(pose[0], pose[1]) self.stop() def stop(self): """This stops all arm movement goals It should be called when a program is exiting so movement stops """ self.move_group.get_move_action().cancel_all_goals()
class FetchClient(object): def __init__(self): self.bfp = True self.robot = robot_interface.Robot_Interface() self.url = 'http://172.31.76.30:80/ThinkingQ/' self.joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.tf_listener = TransformListener() trfm = Transformer() self.r2b = trfm.transform_matrix_of_frames( 'head_camera_rgb_optical_frame', 'base_link') self.model = load_model("./model/0.988.h5") self.br = TransformBroadcaster() self.move_group = MoveGroupInterface("arm", "base_link") # self.pose_group = moveit_commander.MoveGroupCommander("arm") self.cam = camera.RGBD() self.position_cloud = None while True: try: cam_info = self.cam.read_info_data() if cam_info is not None: break except: rospy.logerr('camera info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info) def transform_matrix_of_frames(self, source_frame, target_frame): if self.tf_listener.frameExists(source_frame) and self.tf_listener.frameExists(target_frame): t = self.tf_listener.getLatestCommonTime( source_frame, target_frame) # Compute the transform from source to target position, quaternion = self.tf_listener.lookupTransform( target_frame, source_frame, t) transfrom_matrix = transformations.quaternion_matrix(quaternion) transfrom_matrix[0:3, 3] = position return transfrom_matrix else: return None def get_corner(self): print "get_corner" html = "" while html=="": image_time = time.time() img = self.cam.read_color_data() while img is None: img = self.cam.read_color_data() cv2.imwrite("image/fetch.jpg", img) files = {'IMAGE': (str(image_time) + '.jpg', open('image/fetch.jpg', 'rb'), 'image/png', {})} response = requests.request("POST", url=self.url, files=files, timeout=15000) html = response.text print html exit(0) a = html.split("\n")[1:-1] # print a pl = np.array([int(i) for i in a]) # print pl re = [] i = 0 while i < len(a): point = [] # print "pl i", pl[i] point.append(pl[i]) point.append(pl[i+1]) re.append(point) i = i+2 # print re return re def convert_camera_position_to_base_position(self, array): print 'r2b', self.r2b ret = np.dot(self.r2b[0:3, 0:3], array) # Rotate ret = ret + self.r2b[0:3, 3] return ret def update_position_cloud(self): pc = self.robot.camera.read_point_cloud() arr = pypcd.numpy_pc2.pointcloud2_to_array(pc, split_rgb=True) self.position_cloud = pypcd.numpy_pc2.get_xyz_points(arr, remove_nans=False) def get_position_by_pix(self, x, y): pc = self.robot.camera.read_point_cloud() arr = pypcd.numpy_pc2.pointcloud2_to_array(pc, split_rgb=True) po = pypcd.numpy_pc2.get_xyz_points(arr, remove_nans=False) return po[x, y] def broadcast_position(self, position, name): # while self.bfp: # print "111" a = transformations.quaternion_from_euler( ai=-2.355, aj=-3.14, ak=0.0) b = transformations.quaternion_from_euler( ai=0.0, aj=0.0, ak=1.57) base_rot = transformations.quaternion_multiply(a, b) self.br.sendTransform(position, base_rot, rospy.Time.now(), name, 'head_camera_rgb_optical_frame') def get_position_of_baselink(self, name): listener = TransformListener() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: fetch_client.broadcast_position(cam_position, name) pose, rot = listener.lookupTransform('base_link', name, rospy.Time(0)) except Exception as e: print e continue rate.sleep() if pose: return pose def show_image(self, im): plt.imshow(im) pylab.show() def move_to_position(self, position): position[2] -= 0.24 x = np.round(np.array([position]), 4) * 100 print self.model.predict(x) joints = np.round(self.model.predict(x)[0], 4) / 100 print joints joint_3 = 0 - joints[1] - joints[2] + 1.5 disco_poses = [ [joints[0], joints[1], 0.0, joints[2], 0.0, joint_3, 1.5+joints[0]],# 1 close gripper ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops # self.move_group.get_move_action().cancel_all_goals() def move_to_position_up(self, position): position[2] -= 0.24 x = np.round(np.array([position]), 4) * 100 print self.model.predict(x) joints = np.round(self.model.predict(x)[0], 4) / 100 print joints joint_3 = 0 - joints[1] - joints[2] + 1.5 disco_poses = [ [joints[0], joints[1], 0.0, joints[2], 0.0, joint_3 , 0-joints[1]-joints[2]], #up ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.") def move_to_position_with_up(self, position): position[2] -= 0.24 x = np.round(np.array([position]), 4) * 100 print self.model.predict(x) joints = np.round(self.model.predict(x)[0], 4) / 100 print joints joint_3 = 0 - joints[1] - joints[2] + 1.5 disco_poses = [ [joints[0], joints[1], 0.0, joints[2] - 0.6, 0.0, joint_3 + 0.6, 1.5+joints[0]], #up [joints[0], joints[1], 0.0, joints[2], 0.0, joint_3, 1.5+joints[0]],# 1 close gripper ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.") def stow(self): disco_poses = [ [-0.0, -0.7, 0.0, 0.5, 0.0, 1.5, 1.3], [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]#stow ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") if disco_poses.index(pose) == 1: self.robot.open_gripper() # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.")
def move_corner(self, x, y): position = self.cal_position.get_base_position_from_pix(x, y) position[0] = position[0] - 0.20 move_group = MoveGroupInterface("arm_with_torso", "base_link") planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # This is the wrist link not the gripper itself gripper_frame = 'wrist_roll_link' pose = Pose(Point(position[0], position[1], position[2]), Quaternion(0, 0, 0, 1)) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' # Finish building the Pose_stamped message # If the message stamp is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose # Move gripper frame to the pose specified move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Hello there!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") time.sleep(1) joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] move_group.moveToJointPosition(joint_names, joints_value, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("pose Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.")