def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('get_camera_pose_image_auto')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.5) # 规划时间限制为2秒
arm.allow_replanning(False) # 当运动规划失败后，是否允许重新规划
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
# 开始图像接收
image_receiver = ImageReceiver()
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.listener = tf.TransformListener()
self.save_cnt = 0
print "\n\n[INFO] Pose image saver started."
def __init__(self, pose):
self.check_collision = False
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 将场景中的颜色设置发布
self.sendColors()
rospy.sleep(5)
joint_positions = pose
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
if arm.go():
pass
else:
self.check_collision = True
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class MoveitPlanner:
def __init__(self, manipulator_side = 'left'):
# Enable the robot at start and disable at the end
BaxterRobot().EnableBaxterRobot()
rospy.on_shutdown(BaxterRobot().DisableBaxterRobot)
# Test Open Both Grippers and Close Them
# baxter_manipulator = BaxterManipulatorAndGripper()
# baxter_manipulator.SetupBaxterManipualtor()
# baxter_manipulator.MoveBaxterManipulatorToNeutralPose()
# baxter_manipulator.SetupBaxterGrippers()
# baxter_manipulator.OpenGripper('left')
# baxter_manipulator.OpenGripper('right')
# rospy.sleep(3.0)
# baxter_manipulator.CloseGripper('left')
# baxter_manipulator.CloseGripper('right')
# Referenceing baxter arm
self.__manipulator_name = manipulator_side
self.limb = MoveGroupCommander("{}_arm".format(manipulator_side))
self.limb.set_end_effector_link("{}_gripper".format(manipulator_side))
# Baxter Model configuration with MoveIt not configured to control its gripper,
# thus using Baxter SDK API gripper class
baxter_manipulator = BaxterManipulatorAndGripper()
baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side)
self.limb.set_planner_id("RRTConnectKConfigDefualt")
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('kinect_ur3_move', anonymous=True)
self.listener = tf.TransformListener()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.initialPose = PoseStamped()
self.Pose_goal = PoseStamped()
self.start_goal = PoseStamped()
self.current_goal = PoseStamped()
self.next_pose_goal = PoseStamped()
self.waypoints = []
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.allow_replanning(True)
self.robot_arm.set_goal_position_tolerance(0.01)
self.robot_arm.set_goal_orientation_tolerance(0.1)
def printPose(self):
self.start_goal = self.robot_arm.get_current_pose("ee_link")
print("Start pose:%s",self.start_goal)
def init():
# Wake up Baxter
baxter_interface.RobotEnable().enable()
rospy.sleep(0.25)
print "Baxter is enabled"
#Display initialization screen
display_publsisher("/home/ubuntuz/ws_ros/src/bar_bot2/images/Init.png",
0.0)
print "Intitializing clients for services"
global ik_service_left
ik_service_left = rospy.ServiceProxy(
"ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK)
global ik_service_right
ik_service_right = rospy.ServiceProxy(
"ExternalTools/right/PositionKinematicsNode/IKService",
SolvePositionIK)
# Taken from the MoveIt Tutorials
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
# Activate Left Arm to be used with MoveIt
global left_group
left_group = MoveGroupCommander("left_arm")
left_group.set_goal_position_tolerance(0.01)
left_group.set_goal_orientation_tolerance(0.01)
# Activate and calibrate left gripper
global left_gripper
left_gripper = baxter_interface.Gripper('left')
left_gripper.calibrate()
print left_gripper.parameters()
#Subscribe to get end effector state
global endpoint_subs
endpoint_subs = rospy.Subscriber("/robot/limb/left/endpoint_state",
EndpointState, end_point_callback)
rospy.sleep(1)
#Subscribe to external camera object tracker node
global object_tracker_subs
object_tracker_subs = rospy.Subscriber("/object_tracker", Pose,
tracker_callback)
rospy.sleep(1)
#Subscribe to offset tracker node
global offset_tracker_subs
offset_tracker_subs = rospy.Subscriber("/arm_offset_tracker", Point,
offset_callback)
rospy.sleep(1)
#Subscribe to right arm navigator
global navigator_sub
navigator_sub = rospy.Subscriber("/robot/navigators/right_navigator/state",
NavigatorState, navigator_callback)
rospy.sleep(1)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.pose_goal = Pose()
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to up ======")
rospy.sleep(0.5)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def move_TF(self):
self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538]
self.pose_goal.position.x = self.ee_TF_list[0]
self.pose_goal.position.y = self.ee_TF_list[1]
self.pose_goal.position.z = self.ee_TF_list[2]
self.pose_goal.orientation.x = self.ee_TF_list[3]
self.pose_goal.orientation.y = self.ee_TF_list[4]
self.pose_goal.orientation.z = self.ee_TF_list[5]
self.pose_goal.orientation.w = self.ee_TF_list[6]
self.robot_arm.set_pose_target(self.pose_goal)
self.robot_arm.go(True)
def init():
#Wake up Baxter
baxter_interface.RobotEnable().enable()
rospy.sleep(0.25)
print "Baxter is enabled"
print "Intitializing clients for services"
global ik_service_left
ik_service_left = rospy.ServiceProxy(
"ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK)
global ik_service_right
ik_service_right = rospy.ServiceProxy(
"ExternalTools/right/PositionKinematicsNode/IKService",
SolvePositionIK)
global obj_loc_service
obj_loc_service = rospy.ServiceProxy("object_location_service",
ObjLocation)
global stopflag
stopflag = False
#Taken from the MoveIt Tutorials
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
global scene
scene = moveit_commander.PlanningSceneInterface()
#Activate Left Arm to be used with MoveIt
global left_group
left_group = MoveGroupCommander("left_arm")
left_group.set_goal_position_tolerance(0.01)
left_group.set_goal_orientation_tolerance(0.01)
global right_group
right_group = MoveGroupCommander("right_arm")
pose_right = Pose()
pose_right.position = Point(0.793, -0.586, 0.329)
pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0)
request_pose(pose_right, "right", right_group)
global left_gripper
left_gripper = baxter_interface.Gripper('left')
left_gripper.calibrate()
print left_gripper.parameters()
global end_effector_subs
end_effector_subs = rospy.Subscriber(
"/robot/end_effector/left_gripper/state", EndEffectorState,
end_effector_callback)
rospy.sleep(1)
global pubpic
pubpic = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=1)
rospy.sleep(1)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('start_pos')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.08) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.scene = scene
self.scene_pub = scene_pub
self.colors = dict()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m
self.approach_distance = 0.06
self.back_distance = 0.05
# sub and pub point cloud
self.point_cloud = None
self.update_cloud_flag = False
rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.callback_pointcloud)
thread.start_new_thread(self.publish_pointcloud, ())
class MoveItArm:
"""
Represents Baxter's Arm in MoveIt! world.
Represents a hand of Baxter through MoveIt! Group. Some reusable code has
been packed together into this class to make the code more readable.
"""
def __init__(self, side_name):
"""Will configure and initialise Baxter's hand."""
self._side_name = side_name
# This is the reference to Baxter's arm.
self.limb = MoveGroupCommander("{}_arm".format(side_name))
self.limb.set_end_effector_link("{}_gripper".format(side_name))
# Unfortunetly, MoveIt was not able to make the gripper work, neither
# did was able to find a very good pose using Forward Kinematics,
# so instead the Baxter SDK is used here to do these two jobs.
self.gripper = Gripper(side_name, CHECK_VERSION)
self.gripper.calibrate()
self._limb = Limb(side_name)
# This solver seems to be better for finding solution among obstacles
self.limb.set_planner_id("RRTConnectkConfigDefault")
# Error tollerance should be as low as possible for better accuracy.
self.limb.set_goal_position_tolerance(0.01)
self.limb.set_goal_orientation_tolerance(0.01)
def __str__(self):
"""
String representation of the arm.
String representation of the arm, either 'left' or 'right' string will
be returned. Useful when the string representation of the arm is
needed, like when accessing the IK solver.
"""
return self._side_name
def is_left(self):
"""Will return True if this is the left arm, false otherwise."""
return True if self._side_name == "left" else False
def is_right(self):
"""Will return True if this is the right arm, false otherwise."""
return True if self._side_name == "right" else False
def open_gripper(self):
"""Will open Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.open(block=True)
def close_gripper(self):
"""Will close Baxter's gripper on his active hand."""
# Block ensures that the function does not return until the operation
# is completed.
self.gripper.close(block=True)
def init():
#Wake up Baxter
baxter_interface.RobotEnable().enable()
rospy.sleep(0.25)
print "Baxter is enabled"
print "Intitializing clients for services"
global ik_service_left
ik_service_left = rospy.ServiceProxy(
"ExternalTools/left/PositionKinematicsNode/IKService",
SolvePositionIK)
global ik_service_right
ik_service_right = rospy.ServiceProxy(
"ExternalTools/right/PositionKinematicsNode/IKService",
SolvePositionIK)
global obj_loc_service
obj_loc_service = rospy.ServiceProxy(
"object_location_service", ObjLocation)
global stopflag
stopflag = False
#Taken from the MoveIt Tutorials
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
global scene
scene = moveit_commander.PlanningSceneInterface()
#Activate Left Arm to be used with MoveIt
global left_group
left_group = MoveGroupCommander("left_arm")
left_group.set_goal_position_tolerance(0.01)
left_group.set_goal_orientation_tolerance(0.01)
global right_group
right_group = MoveGroupCommander("right_arm")
pose_right = Pose()
pose_right.position = Point(0.793, -0.586, 0.329)
pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0)
request_pose(pose_right, "right", right_group)
global left_gripper
left_gripper = baxter_interface.Gripper('left')
left_gripper.calibrate()
print left_gripper.parameters()
global end_effector_subs
end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback)
rospy.sleep(1)
global pubpic
pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
rospy.sleep(1)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_speed_demo')
# Initialize the move group for the right arm
arm = MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
# Start the arm in the "home" pose stored in the SRDF file
arm.set_named_target('home')
arm.go()
# Get back the planned trajectory
arm.set_joint_value_target(joint_positions)
traj = arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
arm.execute(new_traj)
rospy.sleep(1)
arm.set_named_target('home')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.Pose_goal = Pose()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
# robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
# self.robot_arm.set_named_target("up") #go to goal state.
# self.robot_arm.go()
# print("====== move plan go to home 1 ======")
# rospy.sleep(2)
# print("====== move plan go to up ======")
# self.robot_arm.set_named_target("zeros") #go to goal state.
# self.robot_arm.go(wait=True)
# print("====== move plan go to zeros ======")
# rospy.sleep(1)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state.
self.robot_arm.go(True)
print("====== move plan go to Pose_goal ======")
#rospy.sleep(2)
robot_state = self.robot_arm.get_current_pose();
robot_angle = self.robot_arm.get_current_joint_values();
print(robot_state)
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
self.Pose_goal = data
self.move_code()
def listener(self):
rospy.Subscriber("chatter", Pose, self.callback)
def arm_pose():
arm = MoveGroupCommander('arm')
arm.allow_replanning(True)
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.03)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_footprint'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
curr_pose = arm.get_current_pose(end_effector_link).pose.position
return curr_pose.x, curr_pose.y, curr_pose.z
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('rest')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('wave')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('rest')
# Get back the planned trajectory
traj = right_arm.plan()
# Set the trajectory speed
new_traj = create_tracking_trajectory(traj, 1.0)
rospy.loginfo(new_traj)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(1) # 规划时间限制为2秒
arm.set_num_planning_attempts(2) # 规划两次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print arm.get_current_pose(eef_link).pose
sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects',
Float64MultiArray, self.callback)
self.juggle_rects = Float64MultiArray()
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('pick_6')
# arm.set_named_target('home')
# arm.go()
self.box_name = ''
self.scene = scene
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to zero ======")
rospy.sleep(0.5)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home2 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.4)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.1) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.082 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.075=0.085m
self.approach_distance = 0.05
self.back_distance = 0.05
def talker_by13():
#init
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo')
#cartesian = rospy.get_param('~cartesian', True)
arm = MoveGroupCommander('manipulator')
arm.set_pose_reference_frame('base_link')
arm.allow_replanning(True)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# arm.set_max_acceleration_scaling_factor(0.5)
#arm.set_max_velocity_scaling_factor(0.5)
end_effector_link = arm.get_end_effector_link()
#arm.set_named_target('home')
arm.set_named_target('up')
arm.go()
rospy.sleep(2)
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_link'
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.86
target_pose.pose.position.y = 0.25
target_pose.pose.position.z = 0.02832
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
#next: find workspace
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
#arm.shift_pose_target(2,-0.05,end_effector_link)
#arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('resting')
# Get back the planned trajectory
traj = right_arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class Robot:
def __init__(self, config, debug=0):
print "[INFO] Initialise Robot"
self.DEBUG = debug
# configuration
self.config = config
# initialise move groups
self.arm = MoveGroupCommander(ARM_GROUP)
self.gripper = MoveGroupCommander(GRIPPER_GROUP)
# initialise pick and place manager
if self.DEBUG is 1:
# verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP,
False, True)
else:
# non verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP,
False, False)
# tolerance: position (in m), orientation (in rad)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
self.place_manager = None
self.pick_manager = None
self.initialise_move_actions()
self.start_position()
"""Initialise the place and pick manager.
"""
def initialise_move_actions(self):
self.place_manager = PlaceManager(self.arm, self.pick_and_place,
self.config, self.DEBUG)
self.pick_manager = PickManager(self.arm, self.pick_and_place,
self.DEBUG)
"""Move robot arm to "start position".
"""
def start_position(self):
self.arm.set_named_target(START_POSITION)
self.arm.go()
rospy.sleep(2)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
self.arm = arm
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(1.0)
arm.set_planning_time(0.05) # 规划时间限制
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
arm.set_named_target('work')
arm.go()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
class Robot:
def __init__(self, config, debug=0):
print "[INFO] Initialise Robot"
self.DEBUG = debug
# configuration
self.config = config
# initialise move groups
self.arm = MoveGroupCommander(ARM_GROUP)
self.gripper = MoveGroupCommander(GRIPPER_GROUP)
# initialise pick and place manager
if self.DEBUG is 1:
# verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
else:
# non verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)
# tolerance: position (in m), orientation (in rad)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
self.place_manager = None
self.pick_manager = None
self.initialise_move_actions()
self.start_position()
"""Initialise the place and pick manager.
"""
def initialise_move_actions(self):
self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG)
self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG)
"""Move robot arm to "start position".
"""
def start_position(self):
self.arm.set_named_target(START_POSITION)
self.arm.go()
rospy.sleep(2)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_pick_and_place_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.00001)
arm.set_goal_orientation_tolerance(0.00001)
arm.set_max_velocity_scaling_factor(0.4)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.5) # 规划时间限制为2秒
arm.allow_replanning(False) # 当运动规划失败后，是否允许重新规划
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
def InitializeMoveitCommander():
#First initialize moveit_commander
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
#Instantiate a RobotCommander object. This object is an interface to the robot as a whole.
robot = moveit_commander.RobotCommander()
#Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(1)
print "============ Starting tutorial "
#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.
global group_both_arms, group_left_arm, group_right_arm
group_both_arms = MoveGroupCommander("both_arms")
group_both_arms.set_goal_position_tolerance(0.01)
group_both_arms.set_goal_orientation_tolerance(0.01)
group_left_arm = MoveGroupCommander("left_arm")
group_left_arm.set_goal_position_tolerance(0.01)
group_left_arm.set_goal_orientation_tolerance(0.01)
group_right_arm = MoveGroupCommander("right_arm")
group_right_arm.set_goal_position_tolerance(0.01)
group_right_arm.set_goal_orientation_tolerance(0.01)
#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)
# Obtain current poses of left and right end-effectors: [x,y,z,roll,pitch,yaw].T
P_left_pose = group_left_arm.get_current_pose()
P_right_pose = group_right_arm.get_current_pose()
P_left_euler = group_left_arm.get_current_rpy()
P_right_euler = group_right_arm.get_current_rpy()
global P_left_current, P_right_current
P_left_current = np.array([[P_left_pose.pose.position.x],
[P_left_pose.pose.position.y],
[P_left_pose.pose.position.z],
[P_left_euler[0]], [P_left_euler[1]],
[P_left_euler[2]]])
P_right_current = np.array([[P_right_pose.pose.position.x],
[P_right_pose.pose.position.y],
[P_right_pose.pose.position.z],
[P_right_euler[0]], [P_right_euler[1]],
[P_right_euler[1]]])
print "\nCurrent pose of left and right EE: \n", np.transpose(
P_left_current), "\n", np.transpose(P_right_current)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set an initial position for the arm
start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)
# Plan and execute a trajectory to the goal configuration
arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Move end effector to the right 0.3 meters
wpose.position.y -= 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Move end effector up and back
wpose.position.x -= 0.2
wpose.position.z += 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses
0.025, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# 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.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class ArmTracker:
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the right arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the right arm reference frame accordingly
self.right_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.right_arm.allow_replanning(False)
# Set a position tolerance in meters
self.right_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.right_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the right_arm shoulder lift frame
#target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('right_arm_base_link', target)
# Convert the position values to a Python list
p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.right_arm.set_named_target('resting')
self.right_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame, target)
# Compute the distance between the current target and the last target
p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.right_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame, target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.right_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.right_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.right_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def update_target_pose(self, target):
self.target = target
def relax_all_servos(self):
command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
args = shlex.split(command)
subprocess.Popen(args)
def shutdown(self):
# Stop any further target messages from being processed
self.target_subscriber.unregister()
# Stop any current arm movement
self.right_arm.stop()
# Move to the resting position
self.right_arm.set_named_target('resting')
self.right_arm.go()
# Relax the servos
self.relax_all_servos()
os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('box1')
scene.remove_world_object('box2')
scene.remove_world_object('target')
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table
table_size = [0.2, 0.7, 0.01]
# Set the length, width and height of the object to attach
tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
scene.attach_mesh
# Place the end of the object within the grasp of the gripper
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_footprint'
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
# Update the current state
right_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Return the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.2)
arm.set_goal_orientation_tolerance(0.1)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
print(end_effector_link)
# 控制机械臂先回到初始化位置
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
print(start_pose)
# 初始化路点列表
waypoints = []
os.system("rosrun pick_test send_gripper.py --value 0.0")
# 将初始位姿加入路点列表
# arm.set_named_target('test5')
# arm.go()
# rospy.sleep(1)
######
marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot)
listener = tf.TransformListener()
r = rospy.Rate(10)
while not rospy.is_shutdown():
try:
(trans,
rot) = listener.lookupTransform('/base_link', '/ar_marker_4',
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
######
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
print carrot
wpose.position.z -= carrot.z
waypoints.append(deepcopy(wpose))
# wpose.position.z -= carrot.z+0.1
# wpose.position.y += carrot.y
# waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 控制机械臂先回到初始化位置
waypoints = []
test_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(test_pose)
wpose.position.x -= carrot.x + 0.03
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.02, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
########
os.system("rosrun pick_test send_gripper.py --value 0.8")
########
waypoints = []
pick_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(pick_pose)
wpose.position.z += 0.1
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.02, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class Arm_Controller:
def __init__(self):
# Give the launch a chance to catch up
# rospy.sleep(5)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Arm_Controller')
rospy.loginfo("Launched Arm Controller")
# constants
self.GROUP_NAME_ARM = 'arm'
self.GRIPPER_FRAME = 'gripper_link'
self.REFERENCE_FRAME = 'base_link'
self.ARM_BASE_FRAME = 'arm_base_link'
self.done = True
self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped)
rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1)
self.robot_name = "gatlin"
move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm)
# We need a tf listener to convert poses into arm reference base
self.tfl = tf.TransformListener()
# Initialize the move group for the right arm
self.arm = MoveGroupCommander(self.GROUP_NAME_ARM)
self.gripper = Gripper()
self.robot = moveit_commander.RobotCommander()
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Set the planner
self.arm.set_planner_id("RRTConnectkConfigDefault")
# Set the right arm reference frame
self.arm.set_pose_reference_frame(self.REFERENCE_FRAME)
# Give the scene a chance to catch up
rospy.sleep(1)
# Allow some leeway in position (meters) and orientation (radians)
# USELESS; do not work on pick and place! Explained on this issue:
# https://github.com/ros-planning/moveit_ros/issues/577
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.05)
# Allow 2 seconds per planning attempt
self.arm.set_planning_time(2.0)
# Create a quaternion from the Euler angles
#q = quaternion_from_euler(0, pitch, yaw)
# horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555)
# deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861)
down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
# back_pos = Point(-0.03, 0.0313, 0.476)
# init rest pose
self.rest_pose = PoseStamped()
self.rest_pose.header.frame_id = self.REFERENCE_FRAME
self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35)
self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981)
# init place_upper pose
self.place_upper_pose = PoseStamped()
self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME
self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998)
self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889)
# init current pose
self.current_pose = PoseStamped()
self.current_pose.header.frame_id = self.REFERENCE_FRAME
self.current_pose.pose.position = Point(0,0,0)
self.current_pose.pose.orientation = down
# Open the gripper
rospy.loginfo("Set Gripper: open")
self.gripper.set(1.0)
# self.arm.set_pose_target(self.rest_pose)
# self.arm.go()
# rospy.sleep(1)
# self.ar = ArbotixROS()
# rate = rospy.Rate(30)
# while not rospy.is_shutdown():
# # rospy.logerr(self.ar.getVoltage(4))
# # rospy.logerr(self.ar.getSpeed(5))
# rospy.logerr(self.ar.getPosition(1))
# rate.sleep()
rospy.spin()
def MoveToPoseWithIntermediate(self, ps, offsets) :
success = False
for offset in offsets:
# interpose = getOffsetPose(hand_pose, offset)
interpose = getOffsetPose(ps, offset)
success = self.MoveToPose(interpose, "MoveToIntermediatePose")
rospy.sleep(1)
success = self.MoveToPose(ps, "MoveToPose")
return success
def MoveToPose(self, ps, name) :
newpose = self.transform_pose(self.REFERENCE_FRAME, ps)
down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
newpose.pose.orientation = down
if self.move_arm_to_pose(newpose) :
rospy.loginfo("SUCCEEDED: %s" % name)
return True
else :
rospy.logerr("FAILED %s" % name)
return False
def move_arm_to_pose(self, ps):
arm_target_pose = deepcopy(ps)
arm_target_pose.header.stamp = rospy.Time.now()
self.test_pose_publisher.publish(arm_target_pose)
self.arm.set_pose_target(arm_target_pose)
success = self.arm.go()
return success
def handle_move_arm(self, req):
success = True
gripper = self.gripper
if req.action == "OPEN_GRIPPER":
rospy.loginfo("Beginning to open gripper")
gripper.open(block=True)
rospy.loginfo("Opened Gripper")
elif req.action == "CLOSE_GRIPPER" :
rospy.loginfo("Beginning to close Gripper")
gripper.close(block=True)
rospy.loginfo("Closed Gripper")
elif req.action == "MOVE_TO_POSE_INTERMEDIATE" :
rospy.loginfo("Trying to Move To Pose With Intermediate")
offsets = [Vector3(0,0,.07)]
success = self.MoveToPoseWithIntermediate(req.ps, offsets)
elif req.action == "MOVE_TO_POSE" :
rospy.loginfo("Trying to Move To Pose")
success = self.MoveToPose(req.ps, "FAILED MoveToPose")
elif req.action == "RESET_ARM" :
rospy.loginfo("Trying to Move To Rest Pose")
success = self.move_arm_to_pose(self.rest_pose)
elif req.action == "PLACE_UPPER" :
rospy.loginfo("Trying to Move To Place Upper Pose")
success = self.move_arm_to_pose(self.place_upper_pose)
# success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose")
# elif req.action == "MOVE_TO_POS" :
# rospy.loginfo("Trying to Move To Pos")
# new_pose = Pose()
# if req.limb == 'left':
# try:
# self.initial_left
# new_pose = deepcopy(self.initial_left)
# except AttributeError:
# new_pose = deepcopy(self.hand_pose_left)
# self.initial_left = deepcopy(self.hand_pose_left)
# elif req.limb == 'right':
# try:
# self.initial_right
# new_pose = deepcopy(self.initial_right)
# except AttributeError:
# new_pose = deepcopy(self.hand_pose_right)
# self.initial_right = deepcopy(self.hand_pose_right)
# new_pose.position = deepcopy(req.pose.position)
# # success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose")
# success = self.MoveToPoseWithIntermediate(req.limb, new_pose)
# rospy.loginfo("Moved to pos: %r" % success)
else :
success = False
rospy.logerr("invalid action")
return MoveRobotResponse(success)
def orientation_cb(self, data):
if(data.x == -1.0 and data.y == -2.0):
print "################################################"
print "################### Orientation! #############"
print "################################################"
deg = data.z*15
rad = -deg * pi/180 + pi/2
print rad
q = quaternion_from_euler(0,rad,0)
self.current_pose.pose.orientation.x = q[0]
self.current_pose.pose.orientation.y = q[1]
self.current_pose.pose.orientation.z = q[2]
self.current_pose.pose.orientation.w = q[3]
else:
return
# transform the pose stamped to the new frame
def transform_pose(self, new_frame, pose):
if pose.header.frame_id == new_frame:
return pose
try:
ps = deepcopy(pose)
ps.header.stamp = rospy.Time(0)
self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0))
new_pose = self.tfl.transformPose(new_frame, ps)
new_pose.header.stamp = deepcopy(pose.header.stamp)
return new_pose
except Exception as e:
rospy.logerr(e)
rospy.logerr("no transform")
return None
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface("base_link")
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Connect to the UBR-1 find_objects action server
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Begin the main perception and pick-and-place loop
while not rospy.is_shutdown():
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
# We don't use the UBR-1 grasp planner as it does not work with our gripper
goal.plan_grasps = False
# Send the goal request to the find_objects action server which will trigger
# the perception pipeline
find_objects.send_goal(goal)
# Wait for a result
find_objects.wait_for_result(rospy.Duration(5.0))
# The result will contain support surface(s) and objects(s) if any are detected
find_result = find_objects.get_result()
# Display the number of objects found
rospy.loginfo("Found %d objects" % len(find_result.objects))
# Remove all previous objects from the planning scene
for name in scene.getKnownCollisionObjects():
scene.removeCollisionObject(name, False)
for name in scene.getKnownAttachedObjects():
scene.removeAttachedObject(name, False)
scene.waitForSync()
# Clear the virtual object colors
scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_size = None
the_object = None
the_object_dist = 1.0
count = -1
# Cycle through all detected objects and keep the nearest one
for obj in find_result.objects:
count += 1
scene.addSolidPrimitive("object%d"%count,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
# Choose the object nearest to the robot
dx = obj.object.primitive_poses[0].position.x - args.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist:
the_object_dist = d
the_object = count
# Get the size of the target
target_size = obj.object.primitives[0].dimensions
# Set the target pose
target_pose.pose = obj.object.primitive_poses[0]
# We want the gripper to be horizontal
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Make sure we found at least one object before setting the target ID
if the_object != None:
target_id = "object%d"%the_object
# Insert the support surface into the planning scene
for obj in find_result.support_surfaces:
# Extend surface to floor
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
2.0, # make table wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# Add to scene
scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
# Get the table dimensions
table_size = obj.primitives[0].dimensions
# If no objects detected, try again
if the_object == None or target_size is None:
rospy.logerr("Nothing to grasp! try again...")
continue
# Wait for the scene to sync
scene.waitForSync()
# Set colors of the table and the object we are grabbing
scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange
scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey
scene.sendColors()
# Skip pick-and-place if we are just detecting objects
if args.objects:
if args.once:
exit(0)
else:
continue
# Get the support surface ID
support_surface = find_result.objects[the_object].object.support_surface
# Set the support surface name to the table object
right_arm.set_support_surface_name(support_surface)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = target_pose.pose.position.x
place_pose.pose.position.y = 0.03
place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose half the size of the target to center it in the gripper
try:
grasp_pose.pose.position.x += target_size[0] / 2.0
grasp_pose.pose.position.y -= 0.01
grasp_pose.pose.position.z += target_size[2] / 2.0
except:
rospy.loginfo("Invalid object size so skipping")
continue
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = right_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Set the start state to the current state
#right_arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
rospy.sleep(2)
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
# Give the servos a rest
arbotix_relax_all_servos()
rospy.sleep(2)
if args.once:
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.3
box1_pose.pose.position.y = 0
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.3
box2_pose.pose.position.y = 0.25
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.55
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.54
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.60
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.25
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置每次运动规划的时间限制：1s
arm.set_planning_time(1)
# 控制机械臂运动到之前设置的“home”姿态
arm.set_named_target('home')
arm.go()
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# --------------------- 第一段轨迹生成，关节空间插值 ---------------------#
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0, -1, 1, 0, -1, 0]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(0.5)
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# --------------------- 第一段轨迹生成，关节空间插值 ---------------------#
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.3米，向右运动0.3米
wpose = deepcopy(start_pose)
wpose.orientation.x = 0
wpose.orientation.y = 0.707106781186547
wpose.orientation.z = 0
wpose.orientation.w = 0.707106781186547
wpose.position.x -= 0.2
# wpose.position.y -= 0.4
wpose.position.z -= 1
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.orientation.x = 0
wpose.orientation.y = 0.707106781186547
wpose.orientation.z = 0
wpose.orientation.w = 0.707106781186547
# wpose.position.x -= 0.5
wpose.position.y -= 0.5
wpose.position.z += 0.2
# wpose.position.x += 0.15
# wpose.position.y += 0.1
# wpose.position.z -= 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('TCP_Move', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('xarm6')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('link_base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 角度弧度转换
j1 = 90.0 / 180 * math.pi
j2 = -18.6 / 180 * math.pi
j3 = -28.1 / 180 * math.pi
j4 = 1.0 / 180 * math.pi
j5 = 47.6 / 180 * math.pi
j6 = -0.9 / 180 * math.pi
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [j1, j2, j3, j4, j5, j6]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
# 向下按压门把手
current_pose = arm.get_current_joint_values()
current_pose[4] += (20.0 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
#推开门
current_pose = arm.get_current_joint_values()
current_pose[0] -= (42.0 / 180.0) * math.pi
current_pose[1] += (2.0 / 180.0) * math.pi
current_pose[2] -= (11.4 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.2
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.31
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.29
box2_pose.pose.position.y = -0.4
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.04, 0.04, 0.05]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault");
left_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#left_arm.set_named_target('left_start')
#left_arm.go()
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.56
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.51
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.49
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class ArmTracker:
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the left arm
self.left_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the left gripper
left_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the left arm reference frame accordingly
self.left_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.left_arm.allow_replanning(False)
# Set a position tolerance in meters
self.left_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.left_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.left_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
left_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
left_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose',
PoseStamped,
self.update_target_pose,
queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the left_arm shoulder lift frame
#target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('left_rotate', target)
# Convert the position values to a Python list
p0 = [
target_arm.pose.position.x, target_arm.pose.position.y,
target_arm.pose.position.z
]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.left_arm.set_named_target('l_start')
self.left_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame,
target)
# Compute the distance between the current target and the last target
p1 = [
target_base.pose.position.x, target_base.pose.position.y,
target_base.pose.position.z
]
p2 = [
self.last_target_pose.pose.position.x,
self.last_target_pose.pose.position.y,
self.last_target_pose.pose.position.z
]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.left_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [
ee_pose.pose.position.x, ee_pose.pose.position.y,
ee_pose.pose.position.z
]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame,
target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.left_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.left_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.left_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def update_target_pose(self, target):
self.target = target
def relax_all_servos(self):
command = 'rosrun rbx2_dynamixels arbotix_relax_all_servos.py'
args = shlex.split(command)
subprocess.Popen(args)
def shutdown(self):
# Stop any further target messages from being processed
self.target_subscriber.unregister()
# Stop any current arm movement
self.left_arm.stop()
# Move to the r_start position
self.left_arm.set_named_target('r_start')
self.left_arm.go()
# Relax the servos
self.relax_all_servos()
os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path (
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# 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.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
