scene.add_box("table", p, (1.0, 2.6, 0.2))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.4
scene.add_plane("ground_plane", p)
rospy.sleep(20)
## We can get the name of the reference frame for this robot
print ">>>>> Reference frame: %s" % group.get_planning_frame()
print ">>>>> Printing robot state"
print robot.get_current_state()
print ">>>>> Printing robot pose"
print group.get_current_pose()
## Planning to a Pose goal
print ">>>>> Generating plan"
pose_target = geometry_msgs.msg.Pose()
#pose_target.orientation.w = 1.0
pose_target.position.x = 0.5 #0.4
pose_target.position.y = 0.2 #0.2
pose_target.position.z = 0.2 #0.2
group.set_pose_target(pose_target)
plan = group.plan()
#print "============ Waiting while RVIZ displays plan..."
rospy.sleep(1)
def transform_to_tip(group, ee_frame, goal):
"""
Transform the goal from the ee_frame to the tip link which is
resolved from planning group
return (new_goal, tip_frame)
"""
mc = MoveGroupCommander(group)
tip_frame = mc.get_end_effector_link()
ee_pose = mc.get_current_pose(ee_frame)
tip_pose = mc.get_current_pose(tip_frame)
kdl_ee = tf_conversions.fromMsg(ee_pose.pose)
kdl_tip = tf_conversions.fromMsg(tip_pose.pose)
if isinstance(goal, geometry_msgs.msg.Pose):
kdl_goal = tf_conversions.fromMsg(goal)
elif isinstance(goal, geometry_msgs.msg.PoseStamped):
kdl_goal = tf_conversions.fromMsg(goal.pose)
else:
rospy.logerr("Unknown pose type, only Pose and PoseStamped is allowed")
rospy.logerr(str(type(goal)))
return (None, tip_frame)
grip = kdl_tip.Inverse() * kdl_ee
kdl_pose = kdl_goal * grip.Inverse()
return (tf_conversions.toMsg(kdl_pose), tip_frame)
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)
class TestHironxMoveit(unittest.TestCase):
@classmethod
def setUpClass(self):
rospy.init_node("test_hironx_moveit")
self.rarm = MoveGroupCommander("right_arm")
self.larm = MoveGroupCommander("left_arm")
self.rarm_current_pose = self.rarm.get_current_pose().pose
self.larm_current_pose = self.larm.get_current_pose().pose
def _set_target_random(self):
'''
@type self: moveit_commander.MoveGroupCommander
@param self: In this particular test script, the argument "self" is either
'rarm' or 'larm'.
'''
global current, current2, target
current = self.get_current_pose()
print "*current*", current
target = self.get_random_pose()
print "*target*", target
self.set_pose_target(target)
self.go()
current2 = self.get_current_pose()
print "*current2*", current2
# Associate a method to MoveGroupCommander class. This enables users to use
# the method on interpreter.
# Although not sure if this is the smartest Python code, this works fine from
# Python interpreter.
##MoveGroupCommander._set_target_random = _set_target_random
# ****** Usage ******
#
# See wiki tutorial
# https://code.google.com/p/rtm-ros-robotics/wiki/hironx_ros_bridge_en#Manipulate_Hiro_with_Moveit_via_Python
#
def test_set_pose_target_rpy(self):
# #rpy ver
target=[0.2035, -0.5399, 0.0709, 0,-1.6,0]
self.rarm.set_pose_target(target)
self.rarm.go()
time.sleep(_SEC_WAIT_BETWEEN_TESTCASES)
def test_set_pose_target_quarternion(self):
# #Quaternion ver
target2=[0.2035, -0.5399, 0.0709, 0.000427, 0.000317, -0.000384, 0.999999]
self.rarm.set_pose_target(target2)
self.rarm.go()
time.sleep(_SEC_WAIT_BETWEEN_TESTCASES)
def getPose():
moveit_commander.roscpp_initialize("hello")
# # 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('braccio_arm')
print("pose=", right_arm.get_current_pose())
pose1 = right_arm.get_current_pose()
print("pos seq", pose1.header.frame_id, pose1.header.seq)
print("pos position", pose1.pose)
def execute(self, userdata):
mc = MoveGroupCommander("r1_arm")
userdata.robot_1_state = mc.get_current_joint_values()
userdata.ik_link_1 = mc.get_end_effector_link()
userdata.robot_1_pose = mc.get_current_pose()
mc = MoveGroupCommander("r2_arm")
userdata.robot_2_state = mc.get_current_joint_values()
userdata.ik_link_2 = mc.get_end_effector_link()
userdata.robot_2_pose = mc.get_current_pose()
mc = MoveGroupCommander("arms")
userdata.complete_state = mc.get_current_joint_values()
return 'succeeded'
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)
# 是否需要使用笛卡尔空间的运动规划
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
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__(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()
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端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
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('get_joint_states', anonymous=True)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('tx90_arm')
# Get the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Joints are stored in the order they appear in the kinematic chain
joint_names = right_arm.get_active_joints()
# Display the joint names
rospy.loginfo("Joint names:\n" + str(joint_names))
# Get the current joint angles
joint_values = right_arm.get_current_joint_values()
# Display the joint values
rospy.loginfo("Joint values:\n" + str(joint_values) + "\n")
# # Get the end-effector pose
ee_pose = right_arm.get_current_pose(end_effector_link)
# Display the end-effector pose
rospy.loginfo("End effector pose:\n" + str(ee_pose))
moveit_commander.roscpp_shutdown()
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.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__(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, ())
def get_ur_pose(req):
manipulator = MoveGroupCommander('manipulator')
end_effector_link = manipulator.get_end_effector_link()
ee_pose = manipulator.get_current_pose(end_effector_link)
rospy.loginfo("End effector pose:\n" + str(ee_pose))
resp = PointResponse()
resp.x = ee_pose.pose.position.x
resp.y = ee_pose.pose.position.y
return resp
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 __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('get_joint_states', anonymous=True)
# Initialize the MoveIt! commander for the right arm
arm = MoveGroupCommander('right_arm')
# Get the end-effector link
end_effector_link = arm.get_end_effector_link()
rospy.loginfo("End effector: %s" % end_effector_link)
planning_frame = arm.get_planning_frame()
# Joints are stored in the order they appear in the kinematic chain
joint_names = arm.get_active_joints()
joint_names = [
'right_arm_shoulder_rotate_joint', 'right_arm_shoulder_lift_joint',
'right_arm_elbow_rotate_joint', 'right_arm_elbow_bend_joint',
'right_arm_wrist_bend_joint', 'right_arm_wrist_rotate_joint'
]
# Display the joint names
#rospy.loginfo("Joint names:\n" + str(joint_names) + "\n")
# Get the current joint angles
joint_values = arm.get_current_joint_values()
# Display the joint values
rospy.loginfo("Joint values:\n" + str(joint_values) + "\n")
# Get the end-effector pose
ee_pose = arm.get_current_pose(end_effector_link)
orientation = ee_pose.pose.orientation
ox = orientation.x
oy = orientation.y
oz = orientation.z
ow = orientation.w
euler_pose = euler_from_quaternion([ow, ox, oy, oz])
#euler_pose = euler_from_quaternion([0.0, 0.0, 0.0, 1.0])
# Display the end-effector pose
rospy.loginfo("End effector pose:\n" + str(ee_pose))
rospy.loginfo("RPY?:\n" + str(euler_pose))
moveit_commander.roscpp_shutdown()
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 = MoveGroupComm # 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 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)
Pose_goal.header.frame_id = 'rightbase_link'
Pose_goal.pose.position.x = -0.1955793462195291 # red line 0.2 0.2
Pose_goal.pose.position.y = 0.3456909607161672 # green line 0.15 0.15
Pose_goal.pose.position.z = 0.16049011785234568 # blue line # 0.35 0.6
# Pose_goal.pose.orientation = start_pose.orientation
Pose_goal.pose.orientation.x = 0.28520761755123414
Pose_goal.pose.orientation.y = 0.24468120052517786
Pose_goal.pose.orientation.z = 0.6034841927127607
Pose_goal.pose.orientation.w = 0.7032741671255489
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 main():
rospy.init_node('test_1', anonymous=True)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('tx90_arm')
# Get the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Get the end-effector pose
ee_pose = right_arm.get_current_pose(end_effector_link)
r = rospy.Rate(10)
while not rospy.is_shutdown():
gripper_pose = arm_utils.tool0TgripperTransform(ee_pose.pose)
r.sleep()
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 markerPub():
# Create a marker publisher.
marker_puber = rospy.Publisher('end_effector_trail', Marker, queue_size=10)
rospy.init_node('markerPub', anonymous=True)
rate = rospy.Rate(10)
# Get the trails of the end effector from moveit API.
moveit_commander.roscpp_initialize(sys.argv)
right_arm = MoveGroupCommander('arm')
right_arm.set_pose_reference_frame('base_link')
# Init the Marker and clear the trails created before.
marker = Marker()
marker.points = []
marker.ns = "my_namespace"
marker.header.frame_id = 'base_link'
marker.id = 0
marker.type = 4
marker.action = Marker.ADD
marker.scale.x = 0.005
marker.scale.y = 0
marker.scale.z = 0
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
marker_puber.publish(marker)
# Publish trails contantly.
while not rospy.is_shutdown():
marker.header.stamp = rospy.Time().now()
# points and line type use marker.point and arrow et. use pose
marker.points.append(right_arm.get_current_pose().pose.position)
#marker.pose = right_arm.get_current_pose().pose.position
#marker.pose.orientation.x = 0
#marker.pose.orientation.y = 0
#marker.pose.orientation.z = 0
#marker.pose.orientation.w = 1
marker_puber.publish(marker)
rate.sleep()
# Exit the relevant process.
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_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
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 __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test')
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.allow_replanning(True)
arm.set_planning_time(5)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_init_orientation = Quaternion()
target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))
current_pose = arm.get_current_pose(end_effector_link)
self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
arm.set_pose_target(current_pose)
arm.go()
rospy.sleep(2)
constraints = Constraints()
orientation_constraint = OrientationConstraint()
constraints.name = 'gripper constraint'
orientation_constraint.header = target_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = target_init_orientation[0]
orientation_constraint.orientation.y = target_init_orientation[1]
orientation_constraint.orientation.z = target_init_orientation[2]
orientation_constraint.orientation.w = target_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
arm.set_path_constraints(constraints)
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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.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.go()
rospy.sleep(1)
start_pose = arm.get_current_pose(end_effector_link).pose
waypoints = []
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
#wpose.position.z += 0.6
#wpose.position.x = 0.8
#wpose.position.y = 0.9
#wpose.position.z = 0.9
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class GetArmPose(EventState):
'''
GetArmPose return the pose of the arm
<# pose Pose Target waypoint for navigation.
<= done Finish job.
'''
def __init__(self):
# See example_state.py for basic explanations.
super(GetArmPose, self).__init__(outcomes=['done'],
output_keys=['pose'])
self.group = MoveGroupCommander("RightArm")
def execute(self, userdata):
return 'done'
def on_enter(self, userdata):
Logger.loginfo('Getting arm pose')
userdata.pose = self.group.get_current_pose().pose
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
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)
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)
# 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)
import geometry_msgs.msg
import moveit_msgs.msg
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from math import sin, copysign, sqrt, pi
if __name__ == '__main__':
print "============ Dynamic hand gestures: Right"
roscpp_initialize(sys.argv)
rospy.init_node('pumpkin_planning', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.0176318609849
wpose.orientation.x = 0.392651866792
wpose.orientation.y = 0.918465607415
wpose.orientation.z = 0.0439835989663
wpose.position.y = 0.227115629827
wpose.position.z = 1.32344046934
wpose.position.x = -0.358178766726
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
robot.detach_object("bowl")
rospy.sleep(0.5)
scene.remove_world_object("bowl")
scene.remove_world_object("punch")
scene.remove_world_object("flat")
rospy.sleep(0.5)
#reset the gripper and arm position to home
robot.set_start_state_to_current_state()
robot.set_named_target("home")
robot.go()
startpose = PoseStamped()
startpose.header.frame_id = 'world'
startpose.header.stamp = rospy.Time.now()
startpose = robot.get_current_pose()
startpose.pose.position.x -= 0.3
robot.set_pose_target(startpose)
robot.go()
# gripper.set_start_state_to_current_state()
# gripper.set_named_target("default")
# gripper.go()
#add scene objects
print 'adding scene objects'
scene.add_mesh("bowl", bowl_pose, "8inhemi.STL")
scene.add_mesh("punch", punch_pose, "punch.STL")
rospy.sleep(1)
# gripper.set_named_target("pinch")
# gripper.go()
import rospy
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from geometry_msgs.msg import Pose, PoseStamped
from moveit_msgs.msg import PlanningSceneWorld, CollisionObject
rospy.init_node('move_web')
mg = MoveGroupCommander('right_arm_and_torso')
p = mg.get_current_pose()
print "Start pose:"
print p
p.pose.position.x += 0.3
#ps = PlanningSceneInterface()
#psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
#rospy.sleep(1)
#co = ps.make_sphere("test_co", p, 0.02)
#psw = PlanningSceneWorld()
#psw.collision_objects.append(co)
#psw_pub.publish(psw)
# ps.remove_world_object("test_sphere")
# ps.add_sphere("test_sphere", p, 0.1)
# rospy.sleep(1)
# ps.add_sphere("test_sphere", p, 0.1)
#p.pose.position.x += 0.3
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)
# Author: Kenji Miyake, Isaac Isao Saito
# This test script needs improved so that it becomes call-able from ROS test
# structure.
from geometry_msgs.msg import Pose, PoseStamped
from moveit_commander import MoveGroupCommander, conversions
import rospy
rospy.init_node("test_hironx_moveit")
rarm = MoveGroupCommander("right_arm")
larm = MoveGroupCommander("left_arm")
rarm_current_pose = rarm.get_current_pose().pose
larm_current_pose = larm.get_current_pose().pose
def _set_target_random(self):
'''
@type self: moveit_commander.MoveGroupCommander
@param self: In this particular test script, the argument "self" is either
'rarm' or 'larm'.
'''
global current, current2, target
current = self.get_current_pose()
print "*current*", current
target = self.get_random_pose()
print "*target*", target
self.set_pose_target(target)
gripper.go()
#add scene objects
print 'adding scene objects'
scene.add_mesh("bowl", p, "8inhemi.STL")
scene.add_mesh("punch", p1, "punch.STL")
scene.add_mesh("glovebox", p2, "GloveBox.stl")
print 'attaching bowl...'
robot.attach_object("bowl")
rospy.sleep(2)
currentbowlpose = p;
gripper.set_named_target("pinch")
gripper.go()
robotstart = robot.get_current_pose()
print 'start eef pose:'
print robotstart
robotstart_quat = [robotstart.pose.orientation.x,robotstart.pose.orientation.y,robotstart.pose.orientation.z,robotstart.pose.orientation.w]
M_eef = tf.transformations.quaternion_matrix(robotstart_quat)
currentbowlpose = p
#calculate position offset from bowl to eef frames
xoffset = robotstart.pose.position.x-p.pose.position.x
yoffset = robotstart.pose.position.y-p.pose.position.y
zoffset = robotstart.pose.position.z-p.pose.position.z
print 'x: ',xoffset,' y: ',yoffset,' z: ',zoffset
print 'finished'
rate = rospy.Rate(10.0)
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print group.get_end_effector_link()
print group.get_pose_reference_frame()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
tl = tf.TransformListener()
rospy.sleep(1)
waypoints = []
# start with the current pose
waypoints.append(group.get_current_pose().pose)
print waypoints[0]
currentPose = PoseStamped()
currentPose.header.frame_id = group.get_pose_reference_frame()
currentPose.pose = waypoints[0]
currentPoseHandLink = tl.transformPose("Hand_Link", currentPose)
currentPoseHandLink.pose.position.z= currentPoseHandLink.pose.position.z - 0.1
# first orient gripper and move forward (+x)
wpose = tl.transformPose(group.get_pose_reference_frame(), currentPoseHandLink).pose
#wpose = Pose()
#wpose.orientation.w = 1.0
#wpose.position.x = waypoints[0].position.x + 0.1
#wpose.position.y = waypoints[0].position.y
#wpose.position.z = waypoints[0].position.z
p.header.frame_id = REFERENCE_LINK
p.header.stamp = rospy.Time.now()
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat)
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# add table
scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
i = 1
while i <= 10:
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
rospy.sleep(1)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
#!/usr/bin/env python
from moveit_commander import MoveGroupCommander
import rospy
if __name__ == '__main__':
group = MoveGroupCommander("manipulator")
rospy.init_node("vs060_demo_wy")
temp_pose=group.get_current_pose()
temp_pose.pose.position.z = temp_pose.pose.position.z - 0.1
group.set_pose_target(temp_pose)
group.go()
class BipedCommander():
##########################################################################
##########################################################################
##########################################################################
def __init__(self):
##########################################################################
rospy.loginfo("BipedCommander started")
self.legs_group = MoveGroupCommander("legs")
self.arms_group = MoveGroupCommander("arms")
self.rarm_group = MoveGroupCommander("RArm")
self.larm_group = MoveGroupCommander("LArm")
##########################################################################
def move_lfoot(self, x, y, z):
##########################################################################
rospy.loginfo("BipedCommander move_lfoot")
end_effector_link = "lfoot"
self.move_foot( end_effector_link, x, y, z)
##########################################################################
def move_rfoot(self, x, y, z):
##########################################################################
rospy.loginfo("BipedCommander move_rfoot")
end_effector_link = "rfoot"
self.move_foot( end_effector_link, x, y, z)
##########################################################################
def move_foot(self, end_effector_link, x, y, z):
##########################################################################
pose = [x, y, z, 0, 0.7071, 0.7071, 0]
if len(end_effector_link) > 0 or self.has_end_effector_link():
rospy.logdebug("setting target")
r = self.legs_group.set_pose_target(pose, end_effector_link)
rospy.loginfo("set position target returned %s" % str(r))
rospy.logdebug("going")
r = self.legs_group.go()
rospy.loginfo("go returned %s" % str(r))
else:
rospy.logerr("There is no end effector to set the pose for")
##########################################################################
def move_larm(self, x, y, z):
##########################################################################
rospy.loginfo("BipedCommander move_rarm")
end_effector_link = "ltip3"
group = self.larm_group
self.move_arm( group, end_effector_link, x, y, z)
##########################################################################
def move_rarm(self, x, y, z):
##########################################################################
rospy.loginfo("BipedCommander move_rarm")
end_effector_link = "rtip3"
group = self.arms_group
self.move_arm( group, end_effector_link, x, y, z)
##########################################################################
def move_arm(self, group, end_effector_link, x, y, z):
##########################################################################
#ose = [x, y, z, -1, 0, 0, 0]
pose = [x, y, z ]
if len(end_effector_link) > 0 or self.has_end_effector_link():
rospy.loginfo("setting target to %s" % pose)
r = group.set_position_target(pose, end_effector_link)
rospy.loginfo("set position target returned %s" % str(r))
rospy.logdebug("going")
r = group.go()
rospy.loginfo("go returned %s" % str(r))
else:
rospy.logerr("There is no end effector to set the pose for")
##########################################################################
def move_legs(self, lx, ly, lz, rx, ry, rz):
##########################################################################
pose = [lx, ly, lz, 0, 0.7071, 0.7071, 0]
end_effector_link = "lfoot"
rospy.loginfo("setting lfoot target to %s" % pose)
r = self.legs_group.set_pose_target(pose, end_effector_link)
rospy.loginfo("set lfoot position target returned %s" % str(r))
pose = [rx, ry, rz, 0, 0.7071, 0.7071, 0]
rospy.loginfo("setting rfoot target to %s" % pose)
end_effector_link = "rfoot"
r = self.legs_group.set_pose_target(pose, end_effector_link)
rospy.loginfo("set rfoot position target returned %s" % str(r))
r = self.legs_group.go()
rospy.loginfo("go returned %s" % str(r))
##########################################################################
def pose_print(self):
##########################################################################
end_effector_link = "lfoot"
r = self.legs_group.get_current_pose( end_effector_link)
p = r.pose.position
o = r.pose.orientation
rospy.loginfo("Left foot pose: [%0.3f, %0.3f, %0.3f], [%0.3f, %0.3f, %0.3f, %0.3f] " % (p.x, p.y, p.z, o.x, o.y, o.z, o.w))
end_effector_link = "rfoot"
r = self.legs_group.get_current_pose( end_effector_link)
p = r.pose.position
o = r.pose.orientation
rospy.loginfo("Right foot pose: [%0.3f, %0.3f, %0.3f], [%0.3f, %0.3f, %0.3f, %0.3f] " % (p.x, p.y, p.z, o.x, o.y, o.z, o.w))
end_effector_link = "ltip3"
r = self.arms_group.get_current_pose( end_effector_link)
p = r.pose.position
o = r.pose.orientation
rospy.loginfo("Left arm pose: [%0.5f, %0.5f, %0.5f], [%0.3f, %0.3f, %0.3f, %0.3f] " % (p.x, p.y, p.z, o.x, o.y, o.z, o.w))
end_effector_link = "rtip3"
r = self.arms_group.get_current_pose( end_effector_link)
p = r.pose.position
o = r.pose.orientation
rospy.loginfo("Right arm pose: [%0.5f, %0.5f, %0.5f], [%0.3f, %0.3f, %0.3f, %0.3f] " % (p.x, p.y, p.z, o.x, o.y, o.z, o.w))
##########################################################################
def walk_pose(self, r_pose, l_pose, pose_name):
##########################################################################
ree = "rfoot" # right leg end effector
lee = "lfoot" # right leg end effector
rospy.loginfo("walk: going to %s", pose_name)
rospy.loginfo("poses: right:%s left:%s" % (r_pose, l_pose))
self.legs_group.set_pose_target( r_pose, ree)
self.legs_group.set_pose_target( l_pose, lee)
r = self.legs_group.go()
rospy.loginfo("walk: go returned %s" % r)
if (r == True):
return(True)
else:
rospy.logerr("**walk pose failed on pose %s" % pose_name)
return(False)
return(True)
##########################################################################
def walk_first_step(self):
##########################################################################
# starting pose
r_pose = [-x_spread, y_start, z_start, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread, y_start, z_start, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "start"):
return(False)
# squat a little
r_pose = [-x_spread, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread, y_start, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "squat"):
return(False)
# lean to one side
r_pose = [-x_spread*2, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "lean"):
return(False)
# raise up leg
r_pose = [-x_spread*2, y_start, z_squat + step_height, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "raise_leg"):
return(False)
# step forward
r_pose = [-x_spread*2, y_start - stride_len / 2, z_squat + step_height, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step_forward"):
return(False)
# step down
r_pose = [-x_spread*2, y_start - stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step_down"):
return(False)
return(True)
##########################################################################
def walk_left_step(self):
##########################################################################
# starting pose
r_pose = [-x_spread * 2, y_start - stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "start"):
return(False)
# center forward
r_pose = [-x_spread * 2, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "center_forward"):
return(False)
# center center
r_pose = [-x_spread, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "shift_to_other_foot"):
return(False)
# shift to other foot
r_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "shift_to_other_foot"):
return(False)
# raise rear leg
r_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start + stride_len, z_squat + step_height, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "raise_rear_leg"):
return(False)
# step forward
r_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start - stride_len / 2, z_squat + step_height, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step_forward"):
return(False)
# step down
r_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start - stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step_forward"):
return(False)
return(True)
##########################################################################
def walk_right_step(self):
##########################################################################
# starting pose
r_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start - stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "start"):
return(False)
# center forward
r_pose = [0, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start , z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "center forward"):
return(False)
# center center
r_pose = [-x_spread, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread, y_start , z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "center center"):
return(False)
# shift to other foot
r_pose = [-x_spread * 2, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start , z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "shift_to_other_foot"):
return(False)
# raise rear leg
r_pose = [-x_spread * 2, y_start + stride_len, z_squat + step_height, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "raise rear leg"):
return(False)
# step forward
r_pose = [-x_spread * 2, y_start - stride_len / 2, z_squat + step_height, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step forward"):
return(False)
# step down
r_pose = [-x_spread * 2, y_start - stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step down"):
return(False)
return(True)
##########################################################################
def walk_right2home(self):
##########################################################################
# starting pose
r_pose = [-x_spread * 2, y_start - stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len / 2, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "start"):
return(False)
# center forward
r_pose = [-x_spread * 2, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [0, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "center_forward"):
return(False)
# center center
r_pose = [-x_spread, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "shift_to_other_foot"):
return(False)
# shift to other foot
r_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start + stride_len, z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "shift_to_other_foot"):
return(False)
# raise rear leg
r_pose = [0, y_start, z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start + stride_len, z_squat + step_height, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "raise_rear_leg"):
return(False)
# step forward
r_pose = [0, y_start , z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start , z_squat + step_height, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "step_forward"):
return(False)
# foot down
r_pose = [0, y_start , z_squat, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread * 2, y_start , z_squat, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "foot down"):
return(False)
# home
r_pose = [-x_spread, y_start , z_start, 0, 0.7071, 0.7071, 0]
l_pose = [x_spread, y_start , z_start, 0, 0.7071, 0.7071, 0]
if not self.walk_pose(r_pose, l_pose, "home"):
return(False)
return(True)
start_pose.orientation.z = 0.5
start_pose.orientation.w = 0.5
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
# print "============ Waiting while RVIZ displays plan_start..."
# rospy.sleep(5)
right_arm.execute(plan_start)
print "============ Waiting while RVIZ executes plan_start..."
rospy.sleep(5)
gain = 0.1
points = 20
step = points/4
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
for i in xrange(step):
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[i-1].orientation
wpose.position.y = waypoints[i-1].position.y
wpose.position.z = waypoints[i-1].position.z + gain
wpose.position.x = waypoints[i-1].position.x
waypoints.append(copy.deepcopy(wpose))
(plan_waypoints, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print fraction*100, "% planned"
print "============ Waiting while RVIZ displays plan_waypoints..."
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# arm.set_goal_position_tolerance(0.01)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
waypoints = list()
pose = PoseStamped().pose
# start with the current pose
waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose))
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
# first pose
pose.position.x = 0.35
pose.position.y = 0.25
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# second pose
pose.position.x = 0.25
pose.position.y = -0.3
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# third pose
pose.position.x += 0.15
waypoints.append(copy.deepcopy(pose))
# fourth pose
pose.position.y += 0.15
waypoints.append(copy.deepcopy(pose))
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class WarehousePlanner(object):
def __init__(self):
rospy.init_node('moveit_warehouse_planner', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(2)
group_id = rospy.get_param("~arm_group_name")
self.eef_step = rospy.get_param("~eef_step", 0.01)
self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
self.group = MoveGroupCommander(group_id)
# self._add_ground()
self._robot_name = self.robot._r.get_robot_name()
self._robot_link = self.group.get_end_effector_link()
self._robot_frame = self.group.get_pose_reference_frame()
self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9)
rospy.sleep(4)
rospy.loginfo("Waiting for warehouse services...")
rospy.wait_for_service('moveit_warehouse_services/list_robot_states')
rospy.wait_for_service('moveit_warehouse_services/get_robot_state')
rospy.wait_for_service('moveit_warehouse_services/has_robot_state')
rospy.wait_for_service('/compute_fk')
self._list_states = rospy.ServiceProxy(
'moveit_warehouse_services/list_robot_states', ListStates)
self._get_state = rospy.ServiceProxy(
'moveit_warehouse_services/get_robot_state', GetState)
self._has_state = rospy.ServiceProxy(
'moveit_warehouse_services/has_robot_state', HasState)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
rospy.loginfo("Service proxies connected")
self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list',
PlanTrajectoryFromList,
self._plan_from_list_cb)
self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix',
PlanTrajectoryFromPrefix,
self._plan_from_prefix_cb)
self._execute_tr_srv = rospy.Service('execute_planned_trajectory',
ExecutePlannedTrajectory,
self._execute_plan_cb)
self.__plan = None
def get_waypoint_names_by_prefix(self, prefix):
regex = "^" + str(prefix) + ".*"
waypoint_names = self._list_states(regex, self._robot_name).states
return waypoint_names
def get_pose_from_state(self, state):
header = Header()
fk_link_names = [self._robot_link]
header.frame_id = self._robot_frame
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0].pose
def get_cartesian_waypoints(self, waypoint_names):
waypoints = []
waypoints.append(self.group.get_current_pose().pose)
for name in waypoint_names:
if self._has_state(name, self._robot_name).exists:
robot_state = self._get_state(name, "").state
waypoints.append(self.get_pose_from_state(robot_state))
else:
rospy.logerr("Waypoint %s doesn't exist for robot %s.", name,
self._robot_name)
return waypoints
def _add_ground(self):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below the dome
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.01))
rospy.sleep(1)
def plan_from_filter(self, prefix):
waypoint_names = self.get_waypoint_names_by_prefix(prefix)
waypoint_names.sort()
if 0 == len(waypoint_names):
rospy.logerr(
"No waypoints found for robot %s with prefix %s. " +
"Can't make trajectory :(", self._robot_name, str(prefix))
return False
rospy.loginfo(
"Creating trajectory with %d waypoints selected by " +
"prefix %s.", len(waypoint_names), str(prefix))
return self.plan_from_list(waypoint_names)
def plan_from_list(self, waypoint_names):
self.group.clear_pose_targets()
waypoints = self.get_cartesian_waypoints(waypoint_names)
if len(waypoints) != len(waypoint_names) + 1:
# +1 because current position is used as first waypiont.
rospy.logerr("Not all waypoints existed, not executing.")
return False
(plan,
fraction) = self.group.compute_cartesian_path(waypoints,
self.eef_step,
self.jump_threshold)
if fraction < self._min_wp_fraction:
rospy.logerr(
"Only managed to generate trajectory through" +
"%f of waypoints. Not executing", fraction)
return False
self.__plan = plan
return True
def _plan_from_list_cb(self, request):
# check all exist
self.__plan = None
rospy.loginfo("Creating trajectory from points given: %s",
",".join(request.waypoint_names))
return self.plan_from_list(request.waypoint_names)
def _plan_from_prefix_cb(self, request):
self.__plan = None
rospy.loginfo("Creating trajectory from points filtered by prefix %s",
request.prefix)
return self.plan_from_filter(request.prefix)
def _execute_plan_cb(self, request):
if self.__plan is None:
rospy.logerr("No plan stored!")
return False
rospy.loginfo("Executing stored plan")
response = self.group.execute(self.__plan)
self.__plan = None
return response
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 SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState,
self.__joint_state_cb, queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
def callback(pose):
object_position_info = pose.position
object_orientation_info = pose.orientation
print object_position_info
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_cartesian', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
#set cartesian parameters
ur5_manipulator = MoveGroupCommander('manipulator')
ur5_gripper = MoveGroupCommander('gripper')
ur5_manipulator.allow_replanning(True)
ur5_manipulator.set_pose_reference_frame('base_link')
ur5_manipulator.set_goal_position_tolerance(0.01)
ur5_manipulator.set_goal_orientation_tolerance(0.1)
end_effector_link = ur5_manipulator.get_end_effector_link()
ur5_manipulator.set_named_target('home_j')
ur5_manipulator.go()
ur5_gripper.set_named_target('open')
ur5_gripper.go()
#get the end effort information
start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose
print("The first waypoint:")
print(start_pose)
#define waypoints
waypoints = []
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.z = object_position_info.z+0.25
wpose.position.x = object_position_info.x
wpose.position.y = object_position_info.y
print("The second waypoint:")
print(wpose)
waypoints.append(deepcopy(wpose))
print(" ")
print(waypoints)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = ur5_manipulator.compute_cartesian_path (
waypoints,
0.01,
0.0,
True)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
ur5_manipulator.execute(plan)
rospy.sleep(2)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
rospy.sleep(3)
ur5_gripper.set_named_target("close")
plan = ur5_gripper.go()
rospy.sleep(2)
ur5_manipulator.set_named_target('home_j')
ur5_manipulator.go()
rospy.sleep(3)
moveit_commander.roscpp_shutdown()
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)
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)
rospy.sleep(5)
right_arm.execute(plan_start)
print "Execute start"
rospy.sleep(5)
end_pose = geometry_msgs.msg.Pose()
end_pose.position.x = -0.0434279649929
end_pose.position.y = -0.0562017053887
end_pose.position.z = 1.48763433664
end_pose.orientation.x = 0.5
end_pose.orientation.y = 0.5
end_pose.orientation.z = 0.5
end_pose.orientation.w = 0.5
right_arm.set_pose_target(end_pose)
plan_end = right_arm.plan()
print "Plan end"
rospy.sleep(5)
right_arm.execute(plan_end)
print "Execute end"
rospy.sleep(5)
#rospy.sleep(1)
current_pose = geometry_msgs.msg.Pose()
current_pose = right_arm.get_current_pose()
print(current_pose)
print "---------------------------------------End"
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)
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from interactive_markers.interactive_marker_server import *
from geometry_msgs.msg import Pose, PoseStamped
if __name__=="__main__":
rospy.init_node('marker_teleop')
pub = rospy.Publisher('robot_interaction_interactive_marker_topic', InteractiveMarker)
robot = MoveGroupCommander("sia5d");
server = InteractiveMarkerServer("simple_marker")
# create interactive marker
int_marker = InteractiveMarker()
int_marker.header.frame_id = "world"
int_marker.name = "my_marker"
int_marker.description = "Teleop Control"
p = robot.get_current_pose()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
p.pose.position.x = p.pose.position.x+0.05
int_marker.pose = p.pose
pub.publish(int_marker)
print 'heh'
rate.sleep()
