class tambourine_text: robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") gripper = moveit_commander.MoveGroupCommander("gripper") run = False ### ### グローバル宣言 ### move_max_velocity_value = 0.2 stop_time = 2.0 # 停止する時間を指定 force_hold_stick = 0.2 # 棒を握る力を指定 ### 縦 tambourine_x_position_vertical = 0.043040 # タンバリンの手前のx座標を指定 tambourine_y_position_vertical = 0.303386 # タンバリンのy座標を指定 tambourine_z_position_vertical = 0.085469 # タンバリンの少し上のz座標を指定 stick_angle_vertical = 1.3 # 棒の角度を指定 tambourine_x_position_vertical1 = 0.043040 # タンバリンの手前のx座標を指定 tambourine_y_position_vertical1 = 0.303386 # タンバリンのy座標を指定 tambourine_z_position_vertical1 = 0.485469 """ 現在縦持ちで仮定しているためコメントアウト ### 横 tambourine_x_position_horizontal = 0.005 # タンバリンの手前のx座標を指定 tambourine_y_position_horizontal = 0.180 # タンバリンのy座標を指定 tambourine_z_position_horizontal = 0.141 # タンバリンの少し上のz座標を指定 stick_angle_horizontal = -3.14 * 9.0 / 10.0 # 棒の角度を指定 """ ### ### 変数宣言ここまで ### def main (self): ##rospy.init_node("crane_x7_pick_and_place_controller") self.run = True self.robot = moveit_commander.RobotCommander() self.arm = moveit_commander.MoveGroupCommander("arm") self.gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(self.robot.get_group_names()) print("Current state:") print(self.robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = self.arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) """ 現在縦持ちで仮定しているためコメントアウト # 横持ち # タンバリンをたたくために位置移動 def preparation_horizontal(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = tambourine_x_position_horizontal target_pose.position.y = tambourine_y_position_horizontal target_pose.position.z = tambourine_z_position_horizontal q = quaternion_from_euler(-3.14, 0, 3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # 角度を変化させタンバリンを叩く def hit_tambourine_horizontal(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = tambourine_x_position_horizontal target_pose.position.y = tambourine_y_position_horizontal target_pose.position.z = tambourine_z_position_horizontal q = quaternion_from_euler(stick_angle_horizontal, 0, 3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 """ # SRDFに定義されている"home"の姿勢にする self.arm.set_named_target("home") self.arm.go() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical1 target_pose.position.y = self.tambourine_y_position_vertical1 target_pose.position.z = self.tambourine_z_position_vertical1 q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical1 target_pose.position.y = self.tambourine_y_position_vertical1 target_pose.position.z = self.tambourine_z_position_vertical1 q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # SRDFに定義されている"home"の姿勢にする self.arm.set_named_target("home") self.arm.go() self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value) self.arm.set_named_target("home") self.arm.go() """ 現在縦持ちで仮定しているためコメントアウト #パターン-----横持ち self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value) self.arm.set_named_target("home") self.arm.go() """ self.run = False # ハンドを開く/ 閉じる def move_gripper(self,pou): self.gripper.set_joint_value_target([pou, pou]) self.gripper.go() # アームを移動する def move_arm(self,pos_x, pos_y, pos_z): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = pos_x target_pose.position.y = pos_y target_pose.position.z = pos_z q = quaternion_from_euler(-3.14/2.0, 3.14, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # 縦持ち # タンバリンをたたくために位置移動 def preparation_vertical(self): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical target_pose.position.y = self.tambourine_y_position_vertical target_pose.position.z = self.tambourine_z_position_vertical q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # 角度を変化させタンバリンを叩く def hit_tambourine_vertical(self): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical target_pose.position.y = self.tambourine_y_position_vertical target_pose.position.z = self.tambourine_z_position_vertical q = quaternion_from_euler(3.14 * 9 / 10, self.stick_angle_vertical, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行
def main(): try: ### Set up the moveit_commander print "============ Setting up the moveit_commander (press ctrl-d to exit) ..." group = moveit_commander.MoveGroupCommander("right_hand") ### Go to the given joint state print "============ Go to initial joint state ..." joint_goal = np.zeros(12).tolist() group.go(joint_goal, wait=True) group.stop() ### Planning of two ur5 arms: go to pose goal print "========== Load joint trajectory from h5 file " file_name = 'glove_test_data' group_name = 'right_glove_test_1' #'right_glove_test_1' f = h5py.File(file_name + '.h5', "r") timestamps = f[group_name + '/time'][:] r_glove_angles = f[group_name + '/r_glove_angle'][:] r_glove_angles = r_glove_angles * pi / 180 # convert degree to radius!!! r_hand_angles = map_glove_to_inspire_hand( r_glove_angles) # linearly map to inspire hand ### Create a plan using the imported trajectory print "========== Create a plan from imported trajectory " joint_plan = moveit_msgs.msg.RobotTrajectory() joint_plan.joint_trajectory.header.frame_id = '/world' joint_plan.joint_trajectory.joint_names = [ 'Link1', 'Link11', 'Link2', 'Link22', 'Link3', 'Link33', 'Link4', 'Link44', 'Link5', 'Link51', 'Link52', 'Link53' ] for i in range(r_hand_angles.shape[0]): traj_point = trajectory_msgs.msg.JointTrajectoryPoint() traj_point.positions = r_hand_angles[i, :].tolist() t = rospy.Time( i * 1.0 / 15.0 ) #rospy.Time(timestamps[i]) # to coordinate with the 15 Hz frame rate of the exported video (though it's not the original timestamps) traj_point.time_from_start.secs = t.secs traj_point.time_from_start.nsecs = t.nsecs joint_plan.joint_trajectory.points.append( copy.deepcopy(traj_point)) ### Go to the start point print "========== Go to the starting position " joint_goal = joint_plan.joint_trajectory.points[0].positions group.go(joint_goal, wait=True) group.stop() ### Execute a plan print "============ Execute a saved path ..." import pdb pdb.set_trace() group.execute(joint_plan, wait=True) except rospy.ROSInterruptException: return except KeyboardInterrupt: return
#! /usr/bin/env python import sys import copy import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node('move_group_python_interface_tutorial', anonymous=True) rospy.init_node('mover4_move_group_python_interface', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("robot") # gripper = moveit_commander.MoveGroupCommander("gripper") rospy.loginfo("release_bottle_on_tall_table execute.py") group.get_current_pose() group.set_named_target("release_bottle_on_tall_table") group.go(wait=True) moveit_commander.roscpp_shutdown()
if __name__ == '__main__': # intialize moveit commander and rospy node moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('pick_and_place', anonymous = True) rospy.wait_for_service("/clear_octomap") clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) gripper = actionlib.SimpleActionClient("iiwa/gripper_controller/gripper_cmd", GripperCommandAction) gripper.wait_for_server() manipulator_name = "manipulator" #give groupname you want to shoe endeffector_name = "gripper" robot = moveit_commander.RobotCommander() manipulator = moveit_commander.MoveGroupCommander(manipulator_name) endeffector = moveit_commander.MoveGroupCommander(endeffector_name) # manipulator.allow_replanning(True) tf_listener = tf.TransformListener() rate = rospy.Rate(10) gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 10.0 #this object is the interface to the world surrounding the robot scene = moveit_commander.PlanningSceneInterface() ### Create Environment create_environment() #Create the environment #Is used to display the trajectoryies in RVIZ display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size = 20)
def __init__(self): super(MoveGroupPythonIntefaceTutorial, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to ## the robot: robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This object is an interface ## to the world surrounding the robot: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to one group of joints. In this case the group is the joints in the Panda ## arm so we set ``group_name = panda_arm``. If you are using a different robot, ## you should change this value to the name of your robot arm planning group. ## This interface can be used to plan and execute motions on the Panda: group_name = "right_hand" group = moveit_commander.MoveGroupCommander(group_name) ## We create a `DisplayTrajectory`_ publisher which is used later to publish ## trajectories for RViz to visualize: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) ## END_SUB_TUTORIAL ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() print "============ Reference frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = group.get_end_effector_link() print "============ End effector: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Robot Groups:", robot.get_group_names() # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state" print robot.get_current_state() print "" ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.group = group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names
# Moving to a pose goal group.execute(plan1, wait=True) rospy.sleep(0.5) if __name__ == "__main__": rospy.init_node("move_cubes") models = rospy.ServiceProxy("/gazebo/get_world_properties", GetWorldProperties) model_coordinates = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) # initializing moveit related things moveit_commander.roscpp_initialize(sys.argv) scene = moveit_commander.PlanningSceneInterface() robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander("Arm") display_trajectory_publisher = rospy.Publisher( "/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=1e3, ) rospy.sleep(2) group.set_goal_orientation_tolerance(0.01) group.set_goal_tolerance(0.01) group.set_goal_joint_tolerance(0.01) group.set_planning_time(1e3) p = geometry_msgs.msg.PoseStamped()
import tf import sys import rospy import moveit_msgs.msg import moveit_commander import geometry_msgs.msg if __name__ == '__main__': moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_to_tf', ) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander('manipulator') # Override default planner group.set_planner_id('RRTConnectkConfigDefault') target_tf = rospy.get_param('~target_tf', '/target') base_tf = rospy.get_param('~base_tf', '/base_link') test_mode = rospy.get_param('~test_mode', False) x = rospy.get_param('~x', 0.1) y = rospy.get_param('~y', 0.4) z = rospy.get_param('~z', 0.8) listener = tf.TransformListener() br = tf.TransformBroadcaster() rate = rospy.Rate(10.0)
from math import pi from tf.transformations import * if __name__ == "__main__": # Initialize moveit_commander and node moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('d3_move_target_pose', anonymous=False) # Get instance from moveit_commander robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() # Get group_commander from MoveGroupCommander group_name = "indy7" move_group = moveit_commander.MoveGroupCommander(group_name) # Move using target_pose pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = 0.0 pose_goal.position.y = -0.6 pose_goal.position.z = 0.8 pose_goal.orientation.x = -0.489606100026 pose_goal.orientation.y = -0.500430269326 pose_goal.orientation.z = 0.49569733542 pose_goal.orientation.w = 0.513945098252
def _init_planning_interface(self): """ Initializes the MoveIn planning interface """ self._robot_comm = moveit_commander.RobotCommander() self._planning_group = moveit_commander.MoveGroupCommander(self._arm) self._planning_group.set_pose_reference_frame(ymc.MOVEIT_PLANNING_REFERENCE_FRAME)
import moveit_msgs.msg import geometry_msgs.msg from math import pi moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface', anonymous=True) ## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to ## the robot: robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This object is an interface ## to the world surrounding the robot: scene = moveit_commander.PlanningSceneInterface() ur5_manipulator = moveit_commander.MoveGroupCommander("ur5_manipulator") ur5_gripper = moveit_commander.MoveGroupCommander("ur5_gripper") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) ur5_manipulator.set_pose_reference_frame('base_link') end_effector_link = ur5_manipulator.get_end_effector_link() #[-0.7068246908767175, 4.987609811687592e-07, -0.7073887590049329, 4.946056750221776e-07] pose_taget = geometry_msgs.msg.Pose() pose_taget.position.x = 0.4 pose_taget.position.y = 0.2 pose_taget.position.z = 0.180 pose_taget.orientation.x = 0.415 pose_taget.orientation.y = -0.436 pose_taget.orientation.z = 0.772
def __init__(self): self.track_flag = False self.default_pose_flag = True self.area_flag = False self.cx = 400.0 self.cy = 400.0 self.bridge = cv_bridge.CvBridge() self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image, self.image_callback) rospy.init_node("moveit_cartesian_path", anonymous=False) self.state_change_time = rospy.Time.now() rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander('manipulator') # Get the name of the end-effector link self.end_effector_link = self.arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = "/base_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # Get the current pose so we can add it as a waypoint start_pose = self.arm.get_current_pose(self.end_effector_link).pose # Initialize the waypoints list self.waypoints= [] # Set the first waypoint to be the starting pose # Append the pose to the waypoints list wpose = deepcopy(start_pose) # Set the next waypoint to the right 0.5 meters wpose.position.x = -0.2 wpose.position.y = -0.2 wpose.position.z = 0.3 self.waypoints.append(deepcopy(wpose)) wpose.position.x = 0.1052 wpose.position.y = -0.4271 wpose.position.z = 0.4005 wpose.orientation.x = 0.4811 wpose.orientation.y = 0.4994 wpose.orientation.z = -0.5121 wpose.orientation.w = 0.5069 self.waypoints.append(deepcopy(wpose)) if np.sqrt((wpose.position.x-start_pose.position.x)**2+(wpose.position.x-start_pose.position.x)**2 \ +(wpose.position.x-start_pose.position.x)**2)<0.1: rospy.loginfo("Warnig: target position overlaps with the initial position!") # self.arm.set_pose_target(wpose) # Set the internal state to the current state self.arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints """moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path( self, waypoints, eef_step, jump_threshold, avoid_collisios= True) Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ plan, fraction = self.arm.compute_cartesian_path(self.waypoints, 0.01, 0.0, True) # plan = self.arm.plan() # If we have a complete plan, execute the trajectory if 1-fraction < 0.2: rospy.loginfo("Path computed successfully. Moving the arm.") num_pts = len(plan.joint_trajectory.points) rospy.loginfo("\n# intermediate waypoints = "+str(num_pts)) self.arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed")
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_ik_demo') # 初始化需要使用move group控制的机械臂中的arm group rarm = moveit_commander.MoveGroupCommander('R_xarm') rgripper = moveit_commander.MoveGroupCommander('R_gripper') # 获取终端link的名称 rend_effector_link = rarm.get_end_effector_link() # 设置目标位置所使用的参考坐标系 reference_frame = 'bottom_link' rarm.set_pose_reference_frame(reference_frame) # 当运动规划失败后,允许重新规划 rarm.allow_replanning(True) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 rarm.set_goal_position_tolerance(0.001) rarm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 rarm.set_max_acceleration_scaling_factor(0.5) rarm.set_max_velocity_scaling_factor(0.5) # 控制机械臂先回到初始化位置 #arm.set_named_target('home') #arm.go() #rospy.sleep(1) # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述, # 姿态使用四元数描述,基于base_link坐标系 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = -0.0687 target_pose.pose.position.y = 0 target_pose.pose.position.z = 0.77 target_pose.pose.orientation.x = 1.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 0.0 # 设置机器臂当前的状态作为运动初始状态 rarm.set_start_state_to_current_state() # 设置机械臂终端运动的目标位姿 rarm.set_pose_target(target_pose, rend_effector_link) # 规划运动路径 traj = rarm.plan() # 按照规划的运动路径控制机械臂运动 rarm.execute(traj) rospy.sleep(1) rgripper.set_joint_value_target([0.67]) rgripper.go() rospy.sleep(1) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = -0.0776 target_pose.pose.position.y = 0 target_pose.pose.position.z = 0.85 target_pose.pose.orientation.x = 1.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 0.0 # 设置机器臂当前的状态作为运动初始状态 rarm.set_start_state_to_current_state() # 设置机械臂终端运动的目标位姿 rarm.set_pose_target(target_pose, rend_effector_link) # 规划运动路径 traj = rarm.plan() # 按照规划的运动路径控制机械臂运动 rarm.execute(traj) rospy.sleep(1) rgripper.set_joint_value_target([0.1]) rgripper.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def main(): moveit_commander.roscpp_initialize(sys.argv) manipulator_group = moveit_commander.MoveGroupCommander("manipulator") manipulator_group.set_pose_reference_frame('base_link') # Set the allowable error of position (unit: meter) and attitude (unit: radians) manipulator_group.set_goal_position_tolerance(0.05) manipulator_group.set_goal_orientation_tolerance(0.05) # Get the name of the terminal link end_effector_link = manipulator_group.get_end_effector_link() group_variable_values = manipulator_group.get_current_joint_values() group_variable_values[0] = 2.6878 group_variable_values[1] = -2.1817 group_variable_values[2] = -0.2094 group_variable_values[3] = -2.4958 group_variable_values[4] = 1.5533 group_variable_values[5] = 1.4312 manipulator_group.set_joint_value_target(group_variable_values) plan = manipulator_group.plan() manipulator_group.go(wait=True) init_pose = [] init_pose.append(0.278694) init_pose.append(-0.262539) init_pose.append(0.401485) # Get the current pose data as the starting pose of the robot arm movement start_pose = manipulator_group.get_current_pose(end_effector_link).pose wpose = deepcopy(start_pose) waypoints=[] wpose.position.x += float(rows[12][0]) - 0.02289 - init_pose[0] - 0.0233 wpose.position.y += float(rows[12][1]) + 0.011618 - init_pose[1] - 0.045 wpose.position.z += float(rows[12][2]) + 0.165994 - init_pose[2] + 0.01 waypoints.append(deepcopy(wpose)) (plan, fraction) = manipulator_group.compute_cartesian_path (waypoints, 0.01,0.0,True) manipulator_group.execute(plan) # waypoints=[] # wpose.orientation.w = -0.331305 # wpose.orientation.x = -0.498117 # wpose.orientation.y = -0.493528 # wpose.orientation.z = 0.631306 # waypoints.append(deepcopy(wpose)) # (plan, fraction) = manipulator_group.compute_cartesian_path (waypoints, 0.01,0.0,True) # manipulator_group.execute(plan) joint_target = manipulator_group.get_current_joint_values() joint_target[0] = 2.68781 joint_target[1] = -2.0245 joint_target[2] = -0.6806 joint_target[3] = -2.28638 joint_target[4] = 1.518 joint_target[5] = 1.4311 manipulator_group.set_joint_value_target(joint_target) plan = manipulator_group.plan() manipulator_group.go(wait=True) waypoints=[] wpose.position.x += 0.02256 wpose.position.y -= 0.0086 wpose.position.z += 0.0131 waypoints.append(deepcopy(wpose)) (plan, fraction) = manipulator_group.compute_cartesian_path (waypoints, 0.01,0.0,True) manipulator_group.execute(plan) #-----SCAN----- sub = rospy.wait_for_message("fiducial_transforms", FiducialTransformArray) tag_id = save_id(sub) print(tag_id) waypoints=[] wpose.position.x -= 0.0088 wpose.position.y += 0.0023 wpose.position.z -= 0.0296 waypoints.append(deepcopy(wpose)) group_variable_values = manipulator_group.get_current_joint_values() group_variable_values[0] = 2.6878 group_variable_values[1] = -2.14675 group_variable_values[2] = -0.5235 group_variable_values[3] = -2.426 group_variable_values[4] = 1.5009 group_variable_values[5] = 1.44862 manipulator_group.set_joint_value_target(group_variable_values) plan = manipulator_group.plan() manipulator_group.go(wait=True) # #------SCAN------ sub = rospy.wait_for_message("fiducial_transforms", FiducialTransformArray) tag_id = save_id(sub) print(tag_id) #home joint_target = manipulator_group.get_current_joint_values() joint_target[0] = 0 joint_target[1] = -2.0944 joint_target[2] = 1.74533 joint_target[3] = 0.34906 joint_target[4] = 1.5708 joint_target[5] = -1.5708 manipulator_group.set_joint_value_target(joint_target) plan = manipulator_group.plan() manipulator_group.go(wait=True) rospy.sleep(1) moveit_commander.roscpp_shutdown()
def main (self): ##rospy.init_node("crane_x7_pick_and_place_controller") self.run = True self.robot = moveit_commander.RobotCommander() self.arm = moveit_commander.MoveGroupCommander("arm") self.gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(self.robot.get_group_names()) print("Current state:") print(self.robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = self.arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) """ 現在縦持ちで仮定しているためコメントアウト # 横持ち # タンバリンをたたくために位置移動 def preparation_horizontal(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = tambourine_x_position_horizontal target_pose.position.y = tambourine_y_position_horizontal target_pose.position.z = tambourine_z_position_horizontal q = quaternion_from_euler(-3.14, 0, 3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # 角度を変化させタンバリンを叩く def hit_tambourine_horizontal(): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = tambourine_x_position_horizontal target_pose.position.y = tambourine_y_position_horizontal target_pose.position.z = tambourine_z_position_horizontal q = quaternion_from_euler(stick_angle_horizontal, 0, 3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 """ # SRDFに定義されている"home"の姿勢にする self.arm.set_named_target("home") self.arm.go() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical1 target_pose.position.y = self.tambourine_y_position_vertical1 target_pose.position.z = self.tambourine_z_position_vertical1 q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() self.preparation_vertical() self.hit_tambourine_vertical() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = self.tambourine_x_position_vertical1 target_pose.position.y = self.tambourine_y_position_vertical1 target_pose.position.z = self.tambourine_z_position_vertical1 q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] self.arm.set_pose_target(target_pose) # 目標ポーズ設定 self.arm.go() # 実行 # SRDFに定義されている"home"の姿勢にする self.arm.set_named_target("home") self.arm.go() self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value) self.arm.set_named_target("home") self.arm.go() """ 現在縦持ちで仮定しているためコメントアウト #パターン-----横持ち self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.preparation_horizontal() self.hit_tambourine_horizontal() self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value) self.arm.set_named_target("home") self.arm.go() """ self.run = False
def main(): arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(1.0) gripper = moveit_commander.MoveGroupCommander("gripper") PRESET_DEFAULT = 0 PRESET_FREE = 3 pid_gain_no = PRESET_DEFAULT # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() # 現在のアーム姿勢を、目標姿勢にセットする joy_wrapper.set_target_arm(arm.get_current_pose()) joy_wrapper.set_target_gripper(gripper.get_current_joint_values()) while joy_wrapper.do_shutdown() == False: # グリッパーの角度を変更する if joy_wrapper.get_and_reset_grip_update_flag(): gripper.set_joint_value_target(joy_wrapper.get_target_gripper()) gripper.go() # アームの姿勢を変更する if joy_wrapper.get_and_reset_pose_update_flag(): arm.set_pose_target(joy_wrapper.get_target_arm()) if arm.go() == False: # 現在のアーム姿勢を、目標姿勢にセットする joy_wrapper.set_target_arm(arm.get_current_pose()) # アームの姿勢をpose_group (home or vertical)に変更する if joy_wrapper.get_and_reset_name_update_flag(): arm.set_named_target(joy_wrapper.get_target_name()) arm.go() # 現在のアーム姿勢を、目標姿勢にセットする joy_wrapper.set_target_arm(arm.get_current_pose()) # アームのPIDゲインをプリセットする if joy_wrapper.get_and_reset_preset_update_flag(): # PIDゲインを0:default, 1:Free に切り替える if pid_gain_no == PRESET_DEFAULT: pid_gain_no = PRESET_FREE else: pid_gain_no = PRESET_DEFAULT # PIDゲインをdefaultに戻すと、目標姿勢に向かって急に動き出す # 安全のため、現在のアームの姿勢を目標姿勢に変更する arm.set_pose_target(arm.get_current_pose()) arm.go() preset_pid_gain(pid_gain_no) # 現在のアーム姿勢を、目標姿勢にセットする joy_wrapper.set_target_arm(arm.get_current_pose()) # ティーチング teaching_flag = joy_wrapper.get_and_reset_teaching_flag() if teaching_flag == joy_wrapper.TEACHING_SAVE: # 現在のアーム姿勢、グリッパー角度を保存する # アームの角度が制御範囲内にない場合、例外が発生する try: arm_joint_values = arm.get_current_joint_values() gripper_joint_values = gripper.get_current_joint_values() arm.set_joint_value_target(arm_joint_values) gripper.set_joint_value_target(gripper_joint_values) joy_wrapper.save_joint_values(arm_joint_values, gripper_joint_values) except moveit_commander.exception.MoveItCommanderException: print("Error setting joint target. Is the target within bounds?") elif teaching_flag == joy_wrapper.TEACHING_LOAD: # 保存したアーム、グリッパー角度を取り出す joint_values = joy_wrapper.load_joint_values() if joint_values: arm.set_joint_value_target(joint_values[0]) arm.go() gripper.set_joint_value_target(joint_values[1]) gripper.go() # 現在のアーム姿勢を、目標姿勢にセットする joy_wrapper.set_target_arm(arm.get_current_pose()) joy_wrapper.set_target_gripper(gripper.get_current_joint_values()) elif teaching_flag == joy_wrapper.TEACHING_DELETE: # 保存したアーム姿勢、グリッパー角度を削除する joy_wrapper.delete_joint_values() rospy.loginfo("Shutdown...") # SRDFに定義されている"vertical"の姿勢にする arm.set_named_target("vertical") arm.go()
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) #print("Group names:") #print(robot.get_group_names()) #print("Current state:") # print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose #print("Arm initial pose:") #print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0 target_pose.position.y = -0.3 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴みに行く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0 target_pose.position.y = -0.3 target_pose.position.z = 0.07 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを閉じる gripper.set_joint_value_target([0.2, 0.2]) gripper.go() # SRDFに定義されている"landing"の姿勢にする arm.set_named_target("landing") arm.go() # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # SRDFに定義されている"vertical"の姿勢にする #アルコールに当たることがあるので一度アームを垂直にする。 arm.set_named_target("vertical") arm.go() #アームを開いて押す準備をする gripper.set_joint_value_target([0.7, 0.7]) gripper.go() #アームをアルコールの上に持ってくる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.1 target_pose.position.z = 0.33 q = quaternion_from_euler(-3.14, 0.0, -0.7) #手首をz軸に80度ほど回転させた姿勢にしてボトルのノズルを押しやすくする。 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 gripper.set_joint_value_target([0.7, 0.7]) gripper.go() #アルコールボトルを押す target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.1 target_pose.position.z = 0.21 q = quaternion_from_euler(-3.14, 0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 rospy.sleep(1.0) #アルコールボトルからアームを引く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.1 target_pose.position.z = 0.33 q = quaternion_from_euler(-3.14, 0.0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 #home(元々の)の姿勢に戻す。 arm.set_named_target("home") arm.go() print("done")
# initialize moveit_commander and rospy print "============ Starting movement setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('bin_picking_move_fanuc', anonymous=True) # This RobotCommander object is an interface to the robot as a whole. robot = moveit_commander.RobotCommander() # This PlanningSceneInterface object is an interface to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() # MoveGroupCommander object. This object is an interface to one group of joints. # In this case the group is the joints in the manipulator. This interface can be used # to plan and execute motions on the manipulator. group = moveit_commander.MoveGroupCommander("manipulator") rate = rospy.Rate(10) # 10hz # Publisher of pointStamped of the grasping point grasping_point_pub = rospy.Publisher('/graspingPoint', PointStamped, queue_size=10) # Publisher of pointStamped of the grasping point io_pub = rospy.Publisher('/io_client_messages', ios, queue_size=10) normal = Vector3() approx_point = Vector3() eef_position_laser = Vector3() roll = np.pi pitch = Float32()
import moveit_msgs.msg import geometry_msgs.msg import tf_conversions import geometry_msgs.msg from tf import TransformListener print("============ Starting tutorial setup") moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) tf_listener = TransformListener() robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() arm = moveit_commander.MoveGroupCommander("arm") gripper = moveit_commander.MoveGroupCommander("gripper") rospy.sleep(1) arm.set_goal_tolerance(0.003) arm.set_max_velocity_scaling_factor(1.0) print("current pose: ", arm.get_current_pose()) print("current rpy: ", arm.get_current_rpy()) print("joints: ", arm.get_current_joint_values()) print("============ Generating plan 1") pose_target = geometry_msgs.msg.PoseStamped()
def __init__(self): rospy.init_node("moveit_cartesian_path", anonymous=False) rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = self.arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = "/base_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # Get the current pose so we can add it as a waypoint start_pose = self.arm.get_current_pose(end_effector_link).pose #print "start pose", start_pose orientation_list = [ start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z, start_pose.orientation.w ] #print orientation_list (roll, pitch, yaw) = euler_from_quaternion(orientation_list) print roll, pitch, yaw #roll += 0.03 quat = quaternion_from_euler(roll, pitch, yaw) #print quat # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose # Append the pose to the waypoints list waypoints.append(start_pose) wpose = deepcopy(start_pose) wpose.position.x = 0.724878225068 wpose.position.y = -0.0902229262006 wpose.position.z = -0.0132514994323 wpose.orientation.x = -0.425978280494 wpose.orientation.y = 0.553721152459 wpose.orientation.z = 0.44131806175 wpose.orientation.w = 0.563181816326 waypoints.append(deepcopy(wpose)) fraction = 0.0 maxtries = 100 attempts = 0 #print waypoints # Set the internal state to the current state self.arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = self.arm.compute_cartesian_path( waypoints, 0.01, 0.0, True) # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") self.arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
import numpy as np import moveit_commander import geometry_msgs.msg from pr2_controllers_msgs.msg import Pr2GripperCommand sys.path.append(os.path.join(os.path.dirname(__file__), '..')) from constants import TEST_GRIPPER_POS as GRIPPER_POS TOPIC_NAME = '/r_gripper_controller/command' pub_gripper = rospy.Publisher(TOPIC_NAME, Pr2GripperCommand, queue_size=10) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('pr2_move_right_arm_test') robot = moveit_commander.RobotCommander() right_arm = moveit_commander.MoveGroupCommander('right_arm') right_gripper = moveit_commander.MoveGroupCommander('right_gripper') group_names = robot.get_group_names() print('Available Planning Groups:', robot.get_group_names()) print('Current state:', robot.get_current_state()) def right_pose_pos(x, y, z): pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = x pose_goal.position.y = y pose_goal.position.z = z pose_goal.orientation.x = GRIPPER_POS[0] pose_goal.orientation.y = GRIPPER_POS[1] pose_goal.orientation.z = GRIPPER_POS[2]
def move_pick_cube(): print("============ Starting setup ============") moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_pick_cube', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("Arm") end_effector_link = group.get_end_effector_link() # trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) print("============ Reference frame: %s" % group.get_planning_frame()) print("============ End effector frame: %s" % group.get_end_effector_link()) print("============ Robot Groups:") print(robot.get_group_names()) print("============ Printing robot state") print(robot.get_current_state()) print("============") print("============put cubes and bucket into moveit workspace") # If we're coming from another script we might want to remove the objects for i in xrange(0, 6): if "cube{}".format(i) in scene.get_known_object_names(): scene.remove_world_object("cube{}".format(i)) if "bucket" in scene.get_known_object_names(): scene.remove_world_object("bucket") ##put cubes and bucket into moveit workspace ##load param into a list cube_bucket_orient = rospy.get_param("cube_bucket_orientRPY") bucket_pose = rospy.get_param("bucket_XYZ") cube_num = rospy.get_param("cube_num") table_height = 0.75 #table height=0.8 orient_quat = geometry_msgs.msg.Quaternion( *tf_conversions.transformations.quaternion_from_euler( cube_bucket_orient[0], cube_bucket_orient[1], cube_bucket_orient[2])) cube_pose_list = list() for i in xrange(0, cube_num): if rospy.has_param("cube{}_XYZ".format(i)): cube_pose_list.append(rospy.get_param("cube{}_XYZ".format(i))) cube_pose_list[i][2] = table_height ##prepare the cubes info cube_info = geometry_msgs.msg.PoseStamped() cube_info.header.frame_id = "robot_base" for i in xrange(0, cube_num): cube_info.pose.position.x = cube_pose_list[i][0] cube_info.pose.position.y = cube_pose_list[i][1] cube_info.pose.position.z = table_height #table height cube_info.pose.orientation = orient_quat print("cube{} x:{}, y:{}".format(i, cube_pose_list[i][0], cube_pose_list[i][1])) scene.add_box("cube{}".format(i), cube_info, (0.05, 0.05, 0.05)) ##found in cubic.urdf #prepare the bucket info bucket_info = geometry_msgs.msg.PoseStamped() bucket_info.header.frame_id = "robot_base" bucket_info.pose.position.x = bucket_pose[0] bucket_info.pose.position.y = bucket_pose[1] bucket_info.pose.position.z = table_height bucket_info.pose.orientation = orient_quat scene.add_box("bucket", bucket_info, (0.2, 0.2, 0.2)) ##cannot found in bucket.dae print("============moveit workspace loading finished") ## setup the planner parameters group.set_goal_orientation_tolerance(0.01) group.set_goal_tolerance(0.01) group.set_goal_joint_tolerance(0.01) group.set_num_planning_attempts(100) pub = rospy.Publisher("/jaco/joint_control", JointState, queue_size=1) #################make hand towards table print("hand towards table") hand_pick_quat = geometry_msgs.msg.Quaternion(0.347709304721, -0.646715871525, 0.298597553716, 0.609669026475) pose_goal = group.get_current_pose(end_effector_link).pose ini_pose = pose_goal pose_goal.orientation = hand_pick_quat group.set_start_state_to_current_state() group.set_pose_target(pose_goal, end_effector_link) plan1_1 = group.plan() group.execute(plan1_1) base_x = 0.5788 base_y = -0.0154 dis_threshold_far = 0.6 dis_threshold_near = 0.26 for i_cube_check in xrange(0, cube_num): dis = np.sqrt( np.square(cube_pose_list[i_cube_check][0] - base_x) + np.square(cube_pose_list[i_cube_check][1] - base_y)) if dis > dis_threshold_far: print( 'cube{} is at a dangerous position(too far) in a distance:{} to the robot base, the task of picking cube{} might fail' .format(i_cube_check, dis, i_cube_check)) elif dis < dis_threshold_near: print( 'cube{} is at a dangerous position(too near) in a distance:{} to the robot base, the task of picking cube{} might fail' .format(i_cube_check, dis, i_cube_check)) else: print( 'cube{} is at a reachable position in a distance:{} to the robot base' .format(i_cube_check, dis)) for i_cube in xrange(0, cube_num): ######################### middle point print("cube{} go to middle point".format(i_cube)) pose_goal.position.x = 0.30 pose_goal.position.y = 0.0 group.set_start_state_to_current_state() group.set_pose_target(pose_goal, end_effector_link) plan1_2 = group.plan() group.execute(plan1_2) rospy.sleep(1.) ########################### go to the upper of cube print("cube{} go to the upper of cube".format(i_cube)) waypoints12 = list() pose_goal12 = group.get_current_pose(end_effector_link).pose ini_pose12 = pose_goal12 waypoints12.append(copy.deepcopy(pose_goal12)) pose_goal12.position.x = cube_pose_list[i_cube][0] pose_goal12.position.y = cube_pose_list[i_cube][1] pose_goal12.position.z = table_height + 0.2 waypoints12.append(copy.deepcopy(pose_goal12)) # plan and execute fraction = 0.0 attempts_max = 100 attempts = 0 while fraction < 0.6 and attempts < attempts_max: (plan12, fraction) = group.compute_cartesian_path(waypoints12, 0.01, 0.0) attempts += 1 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") ## Moving to a pose goal if fraction > 0.4: #fraction == 1.0: rospy.loginfo("Path planning " + str(fraction) + " success after " + str(attempts_max) + " attempts.") rospy.loginfo("Path computed successfully. Moving the arm.") group.execute(plan12) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(attempts_max) + " attempts.") group.set_start_state_to_current_state() group.set_pose_target(pose_goal12, end_effector_link) plan12 = group.plan() group.execute(plan12) rospy.sleep(1.) ########################go down to the cube print("cube{} go down to the cube".format(i_cube)) waypoints2 = list() pose_goal2 = group.get_current_pose(end_effector_link).pose ini_pose2 = pose_goal2 waypoints2.append(copy.deepcopy(pose_goal2)) pose_goal2.position.x = cube_pose_list[i_cube][0] pose_goal2.position.y = cube_pose_list[i_cube][1] pose_goal2.position.z = table_height + 0.17 waypoints2.append(copy.deepcopy(pose_goal2)) fraction = 0.0 attempts_max = 100 attempts = 0 while fraction < 0.8 and attempts < attempts_max: (plan2, fraction) = group.compute_cartesian_path(waypoints2, 0.01, 0.0) attempts += 1 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") ## Moving to a pose goal if fraction > 0.8: #fraction == 1.0: rospy.loginfo("Path planning " + str(fraction) + " success after " + str(attempts_max) + " attempts.") rospy.loginfo("Path computed successfully. Moving the arm.") group.execute(plan2) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(attempts_max) + " attempts.") continue #the action of going down to the cube needs to be done perfectly, if not, quit rospy.sleep(1.) ############################################################## ################################################close the hand print("cube{} close hand".format(i_cube)) currentJointState = rospy.wait_for_message("/joint_states", JointState) print('Received!') currentJointState.header.stamp = rospy.get_rostime() tmp = 0.7 # tmp_tuple=tuple([tmp] + list(currentJointState.position[1:])) currentJointState.position = tuple( list(currentJointState.position[:6]) + [tmp] + [tmp] + [tmp]) rate = rospy.Rate(10) # 10hz for i in range(3): pub.publish(currentJointState) print('Published!') rate.sleep() print('end!') rospy.sleep(5.) ############################################# #############################################hand up print("cube{} hand up from cube".format(i_cube)) waypoints3 = list() pose_goal3 = group.get_current_pose(end_effector_link).pose ini_pose3 = copy.deepcopy(pose_goal3) #print(pose_goal3) waypoints3.append(copy.deepcopy(pose_goal3)) pose_goal3.position.z = ini_pose3.position.z + 0.2 #0.4 waypoints3.append(copy.deepcopy(pose_goal3)) fraction = 0.0 attempts_max = 100 attempts = 0 while fraction < 1.0 and attempts < attempts_max: (plan3, fraction) = group.compute_cartesian_path(waypoints3, 0.01, 0.0) attempts += 1 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") ## Moving to a pose goal if fraction > 0.4: #fraction == 1.0: rospy.loginfo("Path planning " + str(fraction) + " success after " + str(attempts_max) + " attempts.") rospy.loginfo("Path computed successfully. Moving the arm.") group.execute(plan3) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(attempts_max) + " attempts.") pose_goal3.position.z = ini_pose3.position.z + 0.2 # 0.3#have to use the x,y of current pose instead of cube's pose when going up group.set_start_state_to_current_state() group.set_pose_target(pose_goal3, end_effector_link) plan3 = group.plan() group.execute(plan3) rospy.sleep(1.) rospy.sleep(1.) #################################################################### ###########################################go to the upper of bucket print("cube{} go to the upper of bucket".format(i_cube)) waypoints34 = list() pose_goal34 = group.get_current_pose(end_effector_link).pose ini_pose34 = pose_goal34 waypoints34.append(copy.deepcopy(pose_goal34)) pose_goal34.position.x = bucket_pose[0] pose_goal34.position.y = bucket_pose[1] pose_goal34.position.z = table_height + 0.6 pose_goal34.orientation = hand_pick_quat waypoints34.append(copy.deepcopy(pose_goal34)) (plan34, fraction) = group.compute_cartesian_path(waypoints34, 0.01, 0.0) ## Moving to a pose goal group.execute(plan34, wait=True) rospy.sleep(1.) ############################################go to bucket waypoints4 = list() pose_goal4 = group.get_current_pose(end_effector_link).pose ini_pose4 = pose_goal4 waypoints4.append(copy.deepcopy(pose_goal4)) pose_goal4.position.x = bucket_pose[0] pose_goal4.position.y = bucket_pose[1] pose_goal4.position.z = table_height + 0.5 pose_goal4.orientation = hand_pick_quat #print(pose_goal4) waypoints4.append(copy.deepcopy(pose_goal4)) (plan4, fraction) = group.compute_cartesian_path(waypoints4, 0.01, 0.0) ## Moving to a pose goal group.execute(plan4, wait=True) rospy.sleep(1.) #################################open the hand print("cube{} open hand".format(cube_num)) currentJointState = rospy.wait_for_message("/joint_states", JointState) print('Received!') currentJointState.header.stamp = rospy.get_rostime() tmp = 0.005 # tmp_tuple=tuple([tmp] + list(currentJointState.position[1:])) currentJointState.position = tuple( list(currentJointState.position[:6]) + [tmp] + [tmp] + [tmp]) rate = rospy.Rate(10) # 10hz for i in range(3): pub.publish(currentJointState) print('Published!') rate.sleep() print('end!') rospy.sleep(2.) ############################################# #############################################hand up print("cube{} hand up from bucket".format(i_cube)) waypoints5 = list() pose_goal5 = group.get_current_pose(end_effector_link).pose ini_pose5 = pose_goal5 waypoints5.append(pose_goal5) pose_goal5.position.z = table_height + 0.5 # have to use the x,y of current pose instead of bucket's pose when going up pose_goal5.orientation = hand_pick_quat waypoints5.append(pose_goal5) pose_goal5.position.z = table_height + 0.6 waypoints5.append(pose_goal5) (plan5, fraction) = group.compute_cartesian_path(waypoints5, 0.01, 0.0) ## Moving to a pose goal group.execute(plan5, wait=True) rospy.sleep(1.) print("go back") waypoints6 = list() pose_goal6 = group.get_current_pose(end_effector_link).pose waypoints6.append(pose_goal6) pose_goal6.position = ini_pose.position waypoints6.append(pose_goal6) (plan6, fraction) = group.compute_cartesian_path(waypoints6, 0.01, 0.0) ## Moving to a pose goal group.execute(plan6, wait=True) rospy.sleep(1.) # END_TUTORIAL print("============ STOPPING") R = rospy.Rate(10) while not rospy.is_shutdown(): R.sleep()
import baxter_interface import math import tf print "============ Starting tutorial setup" # Se debe redirigir topic de joint_states para baxter sys.argv.append('joint_states:=/robot/joint_states') moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('movimiento_prueba_1',anonymous=True) robot = moveit_commander.RobotCommander() #scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("left_arm") # Publicador de trayectorias para que rviz las visualize. display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size = 10) # PLANEANDO UNA POSE GOAL rot = tf.transformations.quaternion_from_euler(math.pi, 0, -math.pi/2) print rot print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose()
def default_pose_execute(self): self.waypoints= [] self.pointx = [] self.pointy = [] self.phase = 1 self.object_cnt = 0 self.track_flag = False self.default_pose_flag = True self.cx = 320.0 self.cy = 240.0 self.points=[] self.state_change_time = rospy.Time.now() rospy.loginfo("Starting node moveit_cartesian_path") rospy.on_shutdown(self.cleanup) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the move group for the ur5_arm self.arm = moveit_commander.MoveGroupCommander('manipulator') # Get the name of the end-effector link self.end_effector_link = self.arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = "/base_link" # Set the ur5_arm reference frame accordingly self.arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.arm.set_planning_time(0.1) self.arm.set_max_acceleration_scaling_factor(.4) self.arm.set_max_velocity_scaling_factor(.65) self.shoulder_lift_vel_name = '/robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity' self.shoulder_pan_vel_name = '/robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity' self.elbow_vel_name = '/robot_description_planning/joint_limits/elbow_joint/max_velocity' # Get the current pose so we can add it as a waypoint start_pose = self.arm.get_current_pose(self.end_effector_link).pose # Initialize the waypoints list self.waypoints= [] self.pointx = [] self.pointy = [] # Set the first waypoint to be the starting pose # Append the pose to the waypoints list # Default pose in Cartesian frame wpose = deepcopy(start_pose) # Set the next waypoint to the right 0.5 meters wpose.position.x = -0.1123 wpose.position.y = -0.4111 wpose.position.z = 0.2168 wpose.orientation.x = -0.7068 wpose.orientation.y = 0.0090 wpose.orientation.z = 0.7073 wpose.orientation.w = 0.0086 self.waypoints.append(deepcopy(wpose)) # self.arm.set_pose_target(wpose) # Specify default (idle) joint states self.default_joint_states = self.arm.get_current_joint_values() Q1 = [1.5713225603103638, -2.0962370077716272, -1.3499930540667933, -1.265561882649557, 1.57, 1.596241985951559] self.default_joint_states = Q1 # Transition state 1 Q3 = [2.70245099067688, -1.6770857016192835, -1.9628542105304163, -1.0727198759662073, 1.5699867010116577, 2.727403402328491] # Drop states 1 Q4 = [2.702594757080078, -1.9592016378985804, -2.150653664265768, -0.6029952208148401, 1.569974660873413, 2.7274272441864014] # Transition states 2 (on top of the dropping box) Q5 = [3.0016443729400635, -1.4666817823993128, -2.172218624745504, -1.0752008597003382, 1.5701428651809692, 3.0269551277160645] # Drop states 2 Q6 = [3.001344680786133, -1.8149612585650843, -2.391836706792013, -0.507054630910055, 1.5700825452804565, 3.026643753051758] # Transition states 3 Q7 = [3.3429105281829834, -1.3315523306476038, -2.277149502431051, -1.106044117604391, 1.5708140134811401, 3.3683338165283203] # Drop states 3 Q8 = [3.3425633907318115, -1.72536546388735, -2.524234120045797, -0.4651830832110804, 1.5708019733428955, 3.3679027557373047] self.arm.set_joint_value_target(self.default_joint_states) # Set the internal state to the current state self.arm.set_start_state_to_current_state() plan = self.arm.plan() sleep(1) self.arm.execute(plan) self.transition1_states = Q3 # Specify end states (drop object) self.drop1_joint_states = Q4 # self.end_joint_states[1] = -1.3705 self.transition2_states = Q5 self.drop2_joint_states = Q6 self.transition3_states = Q7 self.drop3_joint_states = Q8
def test(): ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('test', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() rospy.sleep(2) ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. rightarm_group = moveit_commander.MoveGroupCommander("right_arm") pose_copy = rightarm_group.get_current_pose().pose rightarm_pose_target = geometry_msgs.msg.Pose() rightarm_pose_target.orientation.w = 0 rightarm_pose_target.orientation.x = 1 rightarm_pose_target.orientation.y = 0 rightarm_pose_target.orientation.z = 0 rightarm_pose_target.position.x = 0.06 rightarm_pose_target.position.y = 0.21 rightarm_pose_target.position.z = 0.75 rightarm_group.set_joint_value_target(rightarm_pose_target, "Link23", True) first_plan = rightarm_group.plan() print "-----------------------------------size of first_plan" print len(first_plan.joint_trajectory.points) if first_plan.joint_trajectory.points: print "success" # rightarm_group.execute(first_plan) listener = tf.TransformListener() find = False while not find: try: (trans, rot) = listener.lookupTransform('/base_link', '/Link24', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue print trans find = True x = trans[0] y = trans[1] z = 1 + trans[2] leftarm_group = moveit_commander.MoveGroupCommander("left_arm") #to get the trans in link21 and value in link24 link21x = 0.068898 link21y = 0.067365 link21z = 0.895772 link24x = 0.068870 link24y = 0.063403 link24z = 0.706849 size0 = max(x, link21x, link24x) - min( x, link21x, link24x) + 0.04 #[0.04, 0.04, 0.05] is the size of link size1 = max(y, link21y, link24y) - min(y, link21y, link24y) + 0.04 size2 = max(z, link21z, link24z) - min(z, link21z, link24z) + 0.05 size = (size0, size1, size2) #set the collision to avoid the leftarm plan to touch the rightarm when they are move Simultaneously box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.frame_id = leftarm_group.get_planning_frame() box_pose.pose.orientation.w = 1 box_pose.pose.position.x = x box_pose.pose.position.y = y box_pose.pose.position.z = z scene.add_box("box", box_pose, size) leftarm_pose_target = geometry_msgs.msg.Pose() leftarm_pose_target.orientation.w = 0 leftarm_pose_target.orientation.x = 1 leftarm_pose_target.orientation.y = 0 leftarm_pose_target.orientation.z = 0 leftarm_pose_target.position.x = -0.06 leftarm_pose_target.position.y = 0.21 leftarm_pose_target.position.z = 0.75 leftarm_group.set_joint_value_target(leftarm_pose_target, "Link13", True) second_plan = leftarm_group.plan() print "-----------------------------------size of second_plan" print len(second_plan.joint_trajectory.points) scene.remove_world_object("box") if second_plan.joint_trajectory.points: print "second success" leftarm_group.execute(second_plan) else: rightarm_group.set_joint_value_target(pose_copy, "Link23", True) recovery_plan = rightarm_group.plan() rightarm_group.execute(recovery_plan)
def plan_motion(self, x_start, w_start, x_mid, w_mid, x_final, w_final, planning_time, left_or_right_eef): # liangyuwei: This function computes a cartesian path with regard to the specified starting point, via point and the final point. ### Set initial pose print "== Set initial pose ==" if (left_or_right_eef): # 1 or left eff group = moveit_commander.MoveGroupCommander("left_arm") left_pose_target = group.get_current_pose('left_ee_link') left_pose_target.pose.position.x = x_start[0] left_pose_target.pose.position.y = x_start[1] left_pose_target.pose.position.z = x_start[2] left_pose_target.pose.orientation.x = w_start[0] left_pose_target.pose.orientation.y = w_start[1] left_pose_target.pose.orientation.z = w_start[2] left_pose_target.pose.orientation.w = w_start[3] group.set_pose_target(left_pose_target, 'left_ee_link') else: group = moveit_commander.MoveGroupCommander("right_arm") right_pose_target = group.get_current_pose('right_ee_link') right_pose_target.pose.position.x = x_start[0] right_pose_target.pose.position.y = x_start[1] right_pose_target.pose.position.z = x_start[2] right_pose_target.pose.orientation.x = w_start[0] right_pose_target.pose.orientation.y = w_start[1] right_pose_target.pose.orientation.z = w_start[2] right_pose_target.pose.orientation.w = w_start[3] group.set_pose_target(right_pose_target, 'right_ee_link') # set planning time group.set_planning_time(planning_time) # plan plan = group.go(wait=True) # stop group.stop() # clear targets group.clear_pose_targets() ### Set via point print "== Set via point ==" waypoints = [] wpose = group.get_current_pose().pose # first mid point wpose.position.x = x_mid[0] # scale * 0.2 wpose.position.y = x_mid[1] # scale * 0.0 # move to the middle wpose.position.z = x_mid[2] # scale * 0.315 # minimum height wpose.orientation.x = w_mid[0] wpose.orientation.y = w_mid[1] wpose.orientation.z = w_mid[2] wpose.orientation.w = w_mid[3] waypoints.append(copy.deepcopy(wpose)) ### Set the final point print "== Set final pose ==" wpose.position.x = x_final[0] wpose.position.y = x_final[1] wpose.position.z = x_final[2] wpose.orientation.x = w_final[0] wpose.orientation.y = w_final[1] wpose.orientation.z = w_final[2] wpose.orientation.w = w_final[3] waypoints.append(copy.deepcopy(wpose)) ### Compute a cartesian path print "== Compute a cartesian path ==" (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.001, #0.01, # eef_step # set to 0.001 for collecting the data 0.0, # jump_threshold avoid_collisions=False) ### Execute the plan print "== Execute the plan ==" self.execute_plan(plan) return plan
def move_group_python_interface_tutorial(): ## BEGIN_TUTORIAL ## ## Setup ## ^^^^^ ## CALL_SUB_TUTORIAL imports ## ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. group = moveit_commander.MoveGroupCommander("manipulator") ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(10) print "============ Starting tutorial " ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ Reference frame: %s" % group.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing robot state" print robot.get_current_state() print "============" ## Planning to a Pose goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target) ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan1 = group.plan() print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) ## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the ## group.plan() method does this automatically so this is not that useful ## here (it just displays the same trajectory again). print "============ Visualizing plan1" display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory) print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) ## Moving to a pose goal ## ^^^^^^^^^^^^^^^^^^^^^ ## ## Moving to a pose goal is similar to the step above ## except we now use the go() function. Note that ## the pose goal we had set earlier is still active ## and so the robot will try to move to that goal. We will ## not use that function in this tutorial since it is ## a blocking function and requires a controller to be active ## and report success on execution of a trajectory. # Uncomment below line when working with a real robot # group.go(wait=True) ## Planning to a joint-space goal ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## Let's set a joint space goal and move towards it. ## First, we will clear the pose target we had just set. group.clear_pose_targets() ## Then, we will get the current set of joint values for the group group_variable_values = group.get_current_joint_values() print "============ Joint values: ", group_variable_values ## Now, let's modify one of the joints, plan to the new joint ## space goal and visualize the plan group_variable_values[0] = 1.0 group.set_joint_value_target(group_variable_values) plan2 = group.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(5) ## Cartesian Paths ## ^^^^^^^^^^^^^^^ ## You can plan a cartesian path directly by specifying a list of waypoints ## for the end-effector to go through. waypoints = [] # start with the current pose waypoints.append(group.get_current_pose().pose) # first orient gripper and move forward (+x) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x + 0.1 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z waypoints.append(copy.deepcopy(wpose)) # second move down wpose.position.z -= 0.10 waypoints.append(copy.deepcopy(wpose)) # third move to the side wpose.position.y += 0.05 waypoints.append(copy.deepcopy(wpose)) ## We want the cartesian path to be interpolated at a resolution of 1 cm ## which is why we will specify 0.01 as the eef_step in cartesian ## translation. We will specify the jump threshold as 0.0, effectively ## disabling it. (plan3, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(5) ## Adding/Removing Objects and Attaching/Detaching Objects ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## First, we will define the collision object message collision_object = moveit_msgs.msg.CollisionObject() ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown() ## END_TUTORIAL print "============ STOPPING"
def main(): rospy.init_node("terminal_control") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() x_pos = 0.2 y_pos = 0.0 z_pos = 0.3 x_ang = -3.14 y_ang = 0.0 z_ang = -3.14 / 2.0 grip_ang = 0.7 while (True): print("Arm current pose:") print(arm.get_current_pose().pose) input_mode = raw_input( "Position (p), Orientation (o) or both (b)? Toggle Gripper (t). Otherwise, quit (q) " ) if (input_mode is not "p" and input_mode is not "o" and input_mode is not "b" and input_mode is not "q" and input_mode is not "t"): print("Try again") continue elif input_mode is "q": print("Exiting command mode, shutting down...") break if input_mode is "p" or input_mode is "b": while (True): #keep trying until get 3 valid points try: x_pos = float( raw_input( "Cartesian Coordinate (x) in metres from base: ")) y_pos = float( raw_input( "Cartesian Coordinate (y) in metres from base: ")) z_pos = float( raw_input( "Cartesian Coordinate (z) in metres from base: ")) break except: print("Enter valid float values, please") pass if input_mode is "o" or input_mode is "b": while (True): #keep trying until get 3 valid points try: x_ang = float(raw_input("Angle about x-axis in radians: ")) y_ang = float(raw_input("Angle about y-axis in radians: ")) z_ang = float(raw_input("Angle about z-axis in radians: ")) break except: print("Enter valid float values, please") pass if (input_mode is not "t"): # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = x_pos #0.2 target_pose.position.y = y_pos #0.0 target_pose.position.z = z_pos #0.3 q = quaternion_from_euler( x_ang, y_ang, z_ang) #(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 elif input_mode is "t": # ハンドを開く grip_ang = 0.7 if grip_ang < 0.7 else 0.3 gripper.set_joint_value_target([grip_ang, grip_ang]) #([0.7, 0.7]) gripper.go() print("done") print("shutting down the arm...")
def __init__(self): # The current status of the joints. self.JointState = JointTrajectoryControllerState() # The servo power's status of the robot. self.ServoPowerState = Bool() # The fault power's status of the robot. self.PowerFaultState = Bool() # The reference coordinate in the calculations of the elfin_basic_api node self.RefCoordinate = String() # The end coordinate in the calculations of the elfin_basic_api node self.EndCoordinate = String() #The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling. self.DynamicArgs = Config() # get the reference coordinate name of elfin_basic_api from the response of this service. self.call_ref_coordinate = rospy.ServiceProxy('elfin_basic_api/get_reference_link', SetBool) self.call_ref_coordinate_req = SetBoolRequest() # get the current position of elfin_ros_control from the response of this service. self.call_current_position = rospy.ServiceProxy('elfin_ros_control/elfin/get_current_position', SetBool) self.call_current_position_req = SetBoolRequest() # call service recognize_position of elfin_ros_control. self.call_recognize_position = rospy.ServiceProxy('elfin_ros_control/elfin/recognize_position', SetBool) self.call_recognize_position_req = SetBoolRequest() self.call_recognize_position_req.data = True # get the end coordinate name of elfin_basic_api from the response of this service. self.call_end_coordinate = rospy.ServiceProxy('elfin_basic_api/get_end_link', SetBool) self.call_end_coordinate_req = SetBoolRequest() # for publishing joint goals to elfin_basic_api self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal', JointState, queue_size=1) self.JointsGoal = JointState() # for publishing cart goals to elfin_basic_api self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal', PoseStamped, queue_size=1) self.CartPos = PoseStamped() # for pub cart path self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal', PoseArray, queue_size=1) self.CartPath = PoseArray() self.CartPath.header.stamp=rospy.get_rostime() self.CartPath.header.frame_id='elfin_base_link' # for pub one specific joint action to elfin_teleop_joint_cmd_no_limit self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit', Int64 , queue_size=1) self.JointCmd = Int64() # for pub multi specific joint action to elfin_teleop_joint_cmd_no_limit self.JointsCmdPub = rospy.Publisher('changyuan_joints_cmd', JointsFloat64, queue_size=1) self.JointsCmd = JointsFloat64() # action client, send goal to move_group self.action_client = SimpleActionClient('elfin_module_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.action_goal = FollowJointTrajectoryGoal() #self.goal_list = JointTrajectoryPoint() self.goal_list = [] self.joints_ = [] self.ps_ = [] self.listener = tf.TransformListener() self.robot=moveit_commander.RobotCommander() self.scene=moveit_commander.PlanningSceneInterface() self.group=moveit_commander.MoveGroupCommander('elfin_arm') self.ref_link_name=self.group.get_planning_frame() self.end_link_name=self.group.get_end_effector_link() self.ref_link_lock=threading.Lock() self.end_link_lock=threading.Lock() self.call_teleop_stop=rospy.ServiceProxy('elfin_basic_api/stop_teleop', SetBool) self.call_teleop_stop_req=SetBoolRequest() self.call_teleop_joint=rospy.ServiceProxy('elfin_basic_api/joint_teleop',SetInt16) self.call_teleop_joint_req=SetInt16Request() self.call_teleop_joints=rospy.ServiceProxy('elfin_basic_api/joints_teleops',SetFloat64s) self.call_teleop_joints_req=SetFloat64sRequest() self.call_teleop_cart=rospy.ServiceProxy('elfin_basic_api/cart_teleop', SetInt16) self.call_teleop_cart_req=SetInt16Request() self.call_move_homing=rospy.ServiceProxy('elfin_basic_api/home_teleop', SetBool) self.call_move_homing_req=SetBoolRequest() self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool) self.call_reset_req=SetBoolRequest() self.call_reset_req.data=True self.call_power_on = rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool) self.call_power_on_req=SetBoolRequest() self.call_power_on_req.data=True self.call_power_off = rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool) self.call_power_off_req = SetBoolRequest() self.call_power_off_req.data=True self.call_velocity_setting = rospy.ServiceProxy('elfin_basic_api/set_velocity_scale', SetFloat64) self.call_velocity_req = SetFloat64Request() self._velocity_scale = 0.78 self.set_velocity_scale(self._velocity_scale) pass
print "============ Starting setup" Force_Measurement = 0 P = [[1.8288, -0.0447, 1.237]] Q = [0.718181636243, -0.0836401543762, 0.687115714468, 0.0713544453462] Robot_Pos = [] Robot_Joint = [] moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('collision_checker', 'move_group_python_interface_tutorial', anonymous=True) ## MoveIt! Initialization robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("move_group") group.set_goal_position_tolerance(0.04) group.allow_replanning(True) group.set_planner_id( "RRTConnectkConfigDefault" ) #RRTConnectkConfigDefault/SBLkConfigDefault/KPIECEkConfigDefault/BKPIECEkConfigDefault/LBKPIECEkConfigDefault/ group.set_num_planning_attempts(5) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) rospy.sleep(2) print "============ Printing robot Pose" print group.get_current_pose().pose tic = timeit.default_timer() dt = 0
def __init__(self): ### MoveIt! moveit_commander.roscpp_initialize(sys.argv) br = tf.TransformBroadcaster() #rospy.init_node('move_group_planner', # anonymous=True) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.plan_scene = moveit_commander.PlanningScene() self.group_1st = moveit_commander.MoveGroupCommander("panda_left") self.group_2nd = moveit_commander.MoveGroupCommander("panda_right") self.group_3rd = moveit_commander.MoveGroupCommander("panda_top") self.group_list = [self.group_1st, self.group_2nd, self.group_3rd] self.group_chain = moveit_commander.MoveGroupCommander( "panda_closed_chain") self.group_chairup = moveit_commander.MoveGroupCommander( "panda_chair_up") self.hand_left = moveit_commander.MoveGroupCommander("hand_left") self.hand_right = moveit_commander.MoveGroupCommander("hand_right") self.hand_top = moveit_commander.MoveGroupCommander("hand_top") self.hand_list = [self.hand_left, self.hand_right, self.hand_top] self.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: self.planning_frame = self.group_1st.get_planning_frame() print("============ Reference frame: %s" % self.planning_frame) # We can also print the name of the end-effector link for this group: self.eef_link = self.group_1st.get_end_effector_link() print("============ End effector: %s" % self.eef_link) # We can get the name of the reference frame for this robot: self.planning_frame = self.group_3rd.get_planning_frame() print("============ Reference frame: %s" % self.planning_frame) # We can also print the name of the end-effector link for this group: self.eef_link = self.group_3rd.get_end_effector_link() print("============ End effector: %s" % self.eef_link) # We can get a list of all the groups in the robot: self.group_names = self.robot.get_group_names() print("============ Robot Groups:", self.robot.get_group_names()) rospy.sleep(1) self.stefan = SceneObject() # self.scene.remove_attached_object(self.group_1st.get_end_effector_link()) # self.scene.remove_attached_object(self.group_3rd.get_end_effector_link()) # self.scene.remove_world_object() ### Franka Collision self.set_collision_behavior = rospy.ServiceProxy( 'franka_control/set_force_torque_collision_behavior', franka_control.srv.SetForceTorqueCollisionBehavior) #self.set_collision_behavior.wait_for_service() self.active_controllers = [] self.listener = tf.TransformListener() self.tr = TransformerROS() box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.frame_id = "base" box_pose.pose.orientation.w = 1.0 box_pose.pose.position.x = 0.8 box_pose.pose.position.z = 0.675 box_pose2 = geometry_msgs.msg.PoseStamped() box_pose2.header.frame_id = "base" box_pose2.pose.orientation.w = 1.0 box_pose2.pose.position.x = 0.8 box_pose2.pose.position.y = -0.25 box_pose2.pose.position.z = 1.0 self.scene.add_box("box", box_pose, size=(1.2, 0.5, 0.15))