def main():
init_node()
# Preparing Left Arm
rospy.loginfo("Preparing Left Arm...")
larmg = MoveGroupCommander("left_arm")
larmg.set_pose_target([0.325, 0.182, 0.067, 0.0, -math.pi / 2, 0.0])
larmg.go()
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/LARM_JOINT5_Link'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id)
# Move to a point above target
if pose_target:
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
def __init__(self):
group = MoveGroupCommander("arm")
#group.set_orientation_tolerance([0.3,0.3,0,3])
p = PoseStamped()
p.header.frame_id = "/katana_base_link"
p.pose.position.x = 0.4287
#p.pose.position.y = -0.0847
p.pose.position.y = 0.0
p.pose.position.z = 0.4492
q = quaternion_from_euler(2, 0, 0)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
print "Planning frame: ", group.get_planning_frame()
print "Pose reference frame: ", group.get_pose_reference_frame()
#group.set_pose_reference_frame("katana_base_link")
print "RPy target: 0,0,0"
#group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
#group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
group.set_pose_target(p, "katana_gripper_tool_frame")
group.go()
print "Current rpy: ", group.get_current_rpy(
"katana_gripper_tool_frame")
def main():
init_node()
# Preparing Head
rospy.loginfo("Preparing Head...")
headg = MoveGroupCommander("head")
headg.set_joint_value_target([0.0, 60.0 / 180.0 * math.pi])
headg.go()
# Preparing Both Arms
rospy.loginfo("Preparing Left Arm...")
barmg = MoveGroupCommander("botharms")
barmg.set_pose_target([0.325, 0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'LARM_JOINT5_Link')
barmg.set_pose_target([0.325, -0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'RARM_JOINT5_Link')
barmg.go()
rospy.sleep(2.0)
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/ar_marker_4'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id, 5.0)
# Move to a point above target
if pose_target:
# Rotate Pose for Right Hand
quat = []
quat.append(pose_target.pose.orientation.x)
quat.append(pose_target.pose.orientation.y)
quat.append(pose_target.pose.orientation.z)
quat.append(pose_target.pose.orientation.w)
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (1, 0, 0)))
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (0, 0, 1)))
pose_target.pose.orientation.x = quat[0]
pose_target.pose.orientation.y = quat[1]
pose_target.pose.orientation.z = quat[2]
pose_target.pose.orientation.w = quat[3]
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
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)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
arm.set_planning_time(10)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
print "======== create new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
# Test Position
start_pose.pose.position.x = 0.2 # 20 cm
start_pose.pose.position.y = -0.11 # -11 cm
start_pose.pose.position.z = 0.6 # 60 cm
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print start_pose
print "====== move to position ======="
# KDL
# arm.set_joint_value_target(start_pose, True)
# IK Fast
arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
arm.get_end_effector_link())
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# max planning time
arm.set_planning_time(10)
# start pose
arm.set_start_state_to_current_state()
end_effector = arm.get_end_effector_link()
rospy.sleep(1)
print "======== create 100 new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
i = 1
while i <= 1000:
# random position
start_pose = arm.get_random_pose(end_effector)
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print "====== move to position", i, "======="
# KDL
arm.set_joint_value_target(start_pose, True)
# IK Fast
# arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y,
# start_pose.pose.position.z], end_effector)
i += 1
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def change_tray_model(scene, onigiri_offset):
botharms = MoveGroupCommander("botharms")
box_pose = PoseStamped()
box_pose.header.frame_id = botharms.get_planning_frame()
scene.remove_world_object("tray")
rospack = rospkg.RosPack()
resourcepath = rospack.get_path(
'khi_duaro_onigiri_system') + "/config/meshes/"
rot_o = 0.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
if onigiri_offset == True:
meshpath = resourcepath + "tray.stl"
zoffset = 0.02
xoffset = 0.0085
yoffset = -0.005
else:
meshpath = resourcepath + "tray_nooffset.stl"
zoffset = 0.0
xoffset = 0.0
yoffset = 0.0
#setting origin of tray
box_pose.pose.position.x = -0.58952 + xoffset
box_pose.pose.position.y = -0.0613 + yoffset
box_pose.pose.position.z = 0.6375 + zoffset
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
scene.add_mesh('tray', box_pose, meshpath, (1, 1, 1))
rospy.sleep(1)
def __init__(self):
group = MoveGroupCommander("arm")
#group.set_orientation_tolerance([0.3,0.3,0,3])
p = PoseStamped()
p.header.frame_id = "/katana_base_link"
p.pose.position.x = 0.4287
#p.pose.position.y = -0.0847
p.pose.position.y = 0.0
p.pose.position.z = 0.4492
q = quaternion_from_euler(2, 0, 0)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
print "Planning frame: " ,group.get_planning_frame()
print "Pose reference frame: ",group.get_pose_reference_frame()
#group.set_pose_reference_frame("katana_base_link")
print "RPy target: 0,0,0"
#group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
#group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
group.set_pose_target(p, "katana_gripper_tool_frame")
group.go()
print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
pose_target_1.position.y = -0.1
pose_target_1.position.z = 0.15
pose_target_1.orientation.x = 0.0
pose_target_1.orientation.y = -0.707
pose_target_1.orientation.z = 0.0
pose_target_1.orientation.w = 0.707
rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_1))
group.set_pose_target(pose_target_1)
group.go()
# Add Object to the Planning Scene
rospy.loginfo("Add Objects to the Planning Scene...")
box_pose = PoseStamped()
box_pose.header.frame_id = group.get_planning_frame()
box_pose.pose.position.x = 0.3
box_pose.pose.position.y = -0.3
box_pose.pose.position.z = -0.25
box_pose.pose.orientation.w = 1.0
scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5))
rospy.sleep(2)
rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names()))
# Set Path Constraint
constraints = Constraints()
constraints.name = "down"
orientation_constraint = OrientationConstraint()
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.position_list = []
self.orientation_list = []
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.5)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
'''
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
'''
marker_joint_goal = [
4.721744537353516, -0.7451499144183558, -1.6199515501605433,
-1.2175200621234339, 1.6366002559661865, 3.1263363361358643
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
'''
def move_code2(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose(1) : ", self.wpose
marker_joint_goal = [
4.768651485443115, -1.1165898481952112, -2.1672890822040003,
5.898628234863281, 1.7003079652786255, 3.2297513484954834
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose(2) : ", self.wpose
def plan_cartesian_path(self, scale=1.0):
waypoints = []
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
'''
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x += scale * 0.009
self.wpose.position.y += scale * 0.07
self.wpose.position.z -= scale * 0.06
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
'''
return plan, fraction
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
detection_start = time.time()
rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
print("detection time", format(time.time() - detection_start))
detection_start = time.time()
def collectJsk(self, msg):
# detection_start = time.time()
try:
detection_start = time.time()
(trans,
rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
rospy.Time(0))
self.goal_x = trans[0]
self.goal_y = trans[1]
self.goal_z = trans[2]
self.goal_ori_x = rot[0]
self.goal_ori_y = rot[1]
self.goal_ori_z = rot[2]
self.goal_ori_w = rot[3]
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.goal_ori_x, self.goal_ori_y, self.goal_ori_z,
self.goal_ori_w
]
(self.goal_roll, self.goal_pitch,
self.goal_yaw) = euler_from_quaternion(self.orientation_list)
except:
return
print("detection time", format(time.time() - detection_start))
# print("detection time",format(time.time()-detection_start))
def go_to_move(self, scale=1.0):
'''
planning_frame = self.robot_arm.get_planning_frame()
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
'''
self.calculated_tomato.position.x = (scale * self.goal_x)
self.calculated_tomato.position.y = (scale * self.goal_y)
self.calculated_tomato.position.z = (scale * self.goal_z)
self.calculated_tomato.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 1.57))
print("============ tomato_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("======= Press `Enter` to if plan in correct!======")
raw_input()
self.robot_arm.go(True)
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
'''
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
pose_target_1.orientation.z = 0.0
pose_target_1.orientation.w = 0.707
rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_1))
group.set_pose_target(pose_target_1)
group.go()
# Add Object to Planning Scene
rospy.loginfo("Planning Scene Settings")
scene = PlanningSceneInterface()
rospy.sleep(2) # Waiting for PlanningSceneInterface
box_pose = PoseStamped()
box_pose.header.frame_id = group.get_planning_frame()
box_pose.pose.position.x = 0.3
box_pose.pose.position.y = -0.3
box_pose.pose.position.z = -0.25
box_pose.pose.orientation.w = 1.0
scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5))
rospy.sleep(2)
rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names()))
# Pose Target 2
rospy.loginfo("Start Pose Target 2")
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
pose_target_2.position.y = -0.5
class BaxterMoveit(BaxterRobot):
LEFT = -1
RIGHT = 1
def __init__(self, node, limb=+1):
super(BaxterMoveit, self).__init__(node, limb=+1)
self.group = MoveGroupCommander(self.lns + "_arm")
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self._info()
def _info(self):
'''Getting Basic Information'''
# name of the reference frame for this robot:
print("@@@@@@@@@@@@ Reference frame: %s" %
self.group.get_planning_frame())
# We can also print the name of the end-effector link for this group:
print("@@@@@@@@@@@@ End effector: %s" %
self.group.get_end_effector_link())
# We can get a list of all the groups in the robot:
print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@",
self.robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@")
print(self.robot.get_current_state())
def obstacle(self, name, position, size):
planning_frame = self.robot.get_planning_frame()
pose = PoseStamped()
pose.header.frame_id = planning_frame
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
self.scene.add_box(name, pose, tuple(size))
def movej(self, q, raw=False):
''' move in joint space by giving a joint configuration as dictionary'''
if raw:
print("in moveit 'raw' motion is not avaiable")
# succ = False
# while succ is False:
succ = self.group.go(q, wait=True)
# self.group.stop() # ensures that there is no residual movement
def showplan(self, target):
if type(target) is PyKDL.Frame or type(target) is Pose:
q = self.ik(target)
elif type(target) is dict:
q = target
else:
print("Target format error")
return
self.group.set_joint_value_target(q)
self.group.plan()
def movep(self, pose, raw=False):
''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame'''
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
q = self.ik(pose)
if q is not None:
self.movej(q, raw=raw)
else:
print("\nNO SOLUTION TO IK\n" * 20)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
self.listener = tf.TransformListener()
# self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
self.tomatoboundingBox = BoundingBox()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
self.distance = 0
self.trans = [0.0, 0.0, 0.0]
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
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.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print ("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print ("====== current pose : ", self.wpose)
joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait = True)
def go_to_desired_coordinate(self):
self.calculated_tomato.position.x = goal_x
self.calculated_tomato.position.y = goal_y
self.calculated_tomato.position.z = goal_z
self.calculated_tomato.orientation.x = goal_roll
self.calculated_tomato.orientation.y = goal_pitch
self.calculated_tomato.orientation.z = goal_yaw
tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z]
tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato))
(tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print ("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
# rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
try:
(trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0))
goal_x = round(trans[0],2)
goal_y = round(trans[1],2)
goal_z = round(trans[2],2)
print("====== trans [x, y, z]: ", trans)
print("====== rotat [x, y, z, w]: ", rot)
orientation = euler_from_quaternion(rot)
goal_roll = orientation[0]
goal_pitch = orientation[1]
goal_yaw = orientation[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('there is no tf')
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
self.distance = 0
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.calculated_tomato_coor = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
2.6482303142547607, -0.3752959410296839, -2.1118043104754847,
-0.4801228682147425, -1.4944542090045374, -4.647655431424276
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
def go_to_goal_point(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
print(">> robot arm planning frame: \n", planning_frame)
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation.w = (scale * ori_w)
self.calculated_tomato.orientation.x = (scale * ori_x)
self.calculated_tomato.orientation.y = (scale * ori_y)
self.calculated_tomato.orientation.z = (scale * ori_z)
print(">> robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("======= Press `Enter` to if plan in correct!======")
raw_input()
self.robot_arm.go(True)
'''
def go_to_designated_coordinate(self):
desired_goal_pose = [-0.537,0.166, 0.248]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n",Cplanning_frame)
self.calculated_tomato_coor.position.x = desired_goal_pose[0]
self.calculated_tomato_coor.position.y = desired_goal_pose[1]
self.calculated_tomato_coor.position.z = desired_goal_pose[2]
self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2]))
print(">> goal frame", self.calculated_tomato_coor)
self.robot_arm.set_pose_target(self.calculated_tomato_coor)
tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z]
tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor))
(plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0)
self.display_trajectory(plan)
print("=========== Press `Enter` to if plan is correct!!...")
raw_input()
self.robot_arm.go(True)
'''
def plan_cartesian_path(self, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x -= scale * 0.1
self.wpose.position.y += scale * 0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
self.wpose.position.x -= scale*0.2
waypoints.append(copy.deepcopy(self.wpose))
'''
'''
self.wpose.position.x -= scale*0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
try:
(trans,
rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
rospy.Time(0))
goal_x = trans[0]
goal_y = trans[1]
goal_z = trans[2]
ori_w = rot[0]
ori_x = rot[1]
ori_y = rot[2]
ori_z = rot[3]
print("==== trans[x,y,z]: ", trans)
print("==== rot[x,y,z,w]: ", rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn('there is no tf')
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.position_list = []
self.orientation_list = []
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.5)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
marker_joint_goal = [4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
def execute_plan(self,plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
if __name__=='__main__':
rospy.init_node("cartesianpathtest")
rospy.loginfo("Starting up move group commander for right arm")
right_arm_mgc = MoveGroupCommander("right_arm_torso")
#init_pose = right_arm_mgc.get_current_pose()
init_pose = Pose(Point(0.4, -0.2, 1.2),Quaternion(0.0,0.0,0.0,1.0))
goal_point = Point(0.45, -0.2, 1.4)
goal_ori = Quaternion(0.0,0.0,0.0,1.0)
goal_pose = Pose(goal_point, goal_ori)
right_arm_mgc.set_pose_reference_frame('base_link')
rospy.loginfo("Planning on frame:" + str(right_arm_mgc.get_planning_frame()))
waypoints = []
waypoints.append(init_pose)#.pose)
waypoints.append(goal_pose)
eef_step_max = 0.05
jump_thresh_max = 10000
while True:
curr_eef_step = eef_step_max * random()
curr_jump_thresh = jump_thresh_max * random()
#path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, curr_eef_step, curr_jump_thresh, False)
path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, 0.01, 1.2, False)
rospy.loginfo("Fraction and path:")
rospy.loginfo(str(fraction))
if fraction <= 0:
class LinkProxy():
def __init__(self, arm):
self._arm = arm
self._group_name = None
self._group = None
self._planner = None
self.orientation_frozen = False
def _initialize_group(self):
# get list of planners available to this move group
available_planners = rospy.get_param(
"/move_group/%s/planner_configs" % self._group_name, None)
if available_planners is None:
ROS_ERR("The Move group `%s` does not have planners configured.",
self._group_name)
return False
if self._planner not in available_planners:
ROS_WARN(
"The planner `%s` is not available for the group `%s`, using `%s` instead.",
self._planner, self._group_name, available_planners[0])
self._planner = available_planners[0]
self._group = MoveGroupCommander(self._group_name)
# set planner
self._group.set_planner_id(self._planner)
def _is_group_valid(self):
if self._group is None:
if isinstance(self, TerminalLink) or isinstance(
self, IntermediateLink):
ROS_ERR("Link object was created without a group")
return False
else:
ROS_ERR("Cannot instantiate abstract classes")
return False
return True
def plan_to_pose(self, pose, *args):
raise NotImplentedError()
def plan_cartesian_path(self, waypoints, *args):
raise NotImplentedError()
def execute_plan(self, plan):
if self._is_group_valid():
return self._arm._execute_plan(self._group, plan)
return False
def get_pose(self):
return self._group.get_current_pose().pose
def is_at(self, pose, epsilon):
cur_pose = self.get_pose()
cur_position = [
cur_pose.position.x, cur_pose.position.y, cur_pose.position.z
]
goal_position = [pose.position.x, pose.position.y, pose.position.z]
eucl_dist = np.linalg.norm(
np.array(cur_position) - np.array(goal_position))
return eucl_dist < epsilon
def get_planning_frame(self):
if self._is_group_valid():
return self._group.get_planning_frame()
return False
def freeze_orientation(self):
# get current pose
cur_pose = self._group.get_current_pose()
# create pose constraint
constraints = Constraints()
constraints.name = "orientation_constraints"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = cur_pose.header
orientation_constraint.header.stamp = rospy.Time.now()
orientation_constraint.link_name = self._group.get_end_effector_link()
orientation_constraint.orientation = cur_pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 0.5
orientation_constraint.absolute_y_axis_tolerance = 0.5
orientation_constraint.absolute_z_axis_tolerance = 0.5
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
self._group.set_path_constraints(constraints)
self.orientation_frozen = True
def clear_path_constraints(self):
self._group.set_path_constraints(None)
self.orientation_frozen = False
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.target_ar_id = 9
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
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.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
"""
move to initial pose of the UR3
"""
#self.clear_octomap()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def move_moveit_setting_pose(self, pose_name):
"""
move to one of the moveit_setup pose: "home", "zeros", "table"
"""
if pose_name == "home":
self.robot_arm.set_named_target("home")
elif pose_name == "zeros":
self.robot_arm.set_named_target("zeros")
elif pose_name == "table":
self.robot_arm.set_named_target("table")
#print "Press the Enter"
#raw_input()
self.robot_arm.go(wait=True)
def go_to_move(self, scale=1.0):
"""
manipulator is moving to the target ar_marker. \n
the start ar_marker id is 9, goal ar_marker id is 10.
"""
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
coke_offset = [0, -0.35, -0.1] #x y z
# gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품.
# linear offset = abs([0, 0.0625, 0.13])
robot_base_offset = 0.873
base_wrist2_offset = 0.1 #for avoiding link contact error
if self.target_ar_id == 9:
print ">> robot arm plannig frame: \n", planning_frame
print ">> move mode id: ", self.target_ar_id
self.calculed_coke_pose.position.x = (
scale * self.goal_x) # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale *
self.goal_y) + coke_offset[1]
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 1.57))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
elif self.target_ar_id == 10:
print ">> robot arm plannig frame: \n", planning_frame
print ">> move mode id: ", self.target_ar_id
self.calculed_coke_pose.position.x = (scale *
self.goal_x) + coke_offset[1]
self.calculed_coke_pose.position.y = (scale * self.goal_y) + 0
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 0))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [
self.calculed_coke_pose.position.x,
self.calculed_coke_pose.position.y,
self.calculed_coke_pose.position.z
]
tf_display_orientation = [
self.calculed_coke_pose.orientation.x,
self.calculed_coke_pose.orientation.y,
self.calculed_coke_pose.orientation.z,
self.calculed_coke_pose.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
coke_waypoints = []
coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose))
(coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(
coke_waypoints, 0.01, 0.0)
self.display_trajectory(coke_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
"""
Manipulator is moving to the desired coordinate. \n
Now move to the ar_10 marker.
"""
calculed_ar_id_10 = Pose()
#desired_goal_pose = [0.171, -0.113, 1.039]
#desired_goal_euler = [3.14, 0.17, 0]
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print ">> current planning frame: \n", Cplanning_frame
calculed_ar_id_10.position.x = desired_goal_pose[0] + 0.1
calculed_ar_id_10.position.y = desired_goal_pose[1]
calculed_ar_id_10.position.z = desired_goal_pose[2]
calculed_ar_id_10.orientation = Quaternion(*quaternion_from_euler(
desired_goal_euler[0], desired_goal_euler[1],
desired_goal_euler[2]))
print ">>> ar id 10 goal frame: ", desired_goal_pose
self.robot_arm.set_pose_target(calculed_ar_id_10)
tf_display_position = [
calculed_ar_id_10.position.x, calculed_ar_id_10.position.y,
calculed_ar_id_10.position.z
]
tf_display_orientation = [
calculed_ar_id_10.orientation.x, calculed_ar_id_10.orientation.y,
calculed_ar_id_10.orientation.z, calculed_ar_id_10.orientation.w
]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
ar_id_10_waypoints = []
ar_id_10_waypoints.append(copy.deepcopy(calculed_ar_id_10))
(ar_id_10_plan,
ar_id_10_fraction) = self.robot_arm.compute_cartesian_path(
ar_id_10_waypoints, 0.01, 0.0)
self.display_trajectory(ar_id_10_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
"""
Visualized trajectory of the manipulator
"""
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
"""
Working on the "linear move": x-axis -> y-axis -> z-axis
"""
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
#print "waypoints:", waypoints
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
"""
Working on the "linear move": x-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
"""
Working on the "linear move": y-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
"""
Working on the "linear move": z-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
"""
execute planned "plan" trajectory.
"""
group = self.robot_arm
group.execute(plan, wait=True)
def ar_pose_subscriber(self):
"""
ar_maker subscriber
"""
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#print "======= pos(meter): ", self.position_list
#print "======= orientation: ", self.orientation_list
def ar_tf_listener(self, msg):
"""
Listening the ar_marker pose coordinate.
"""
try:
self.marker = msg.markers
ml = len(self.marker)
target_start_point_id = self.target_ar_id
#target_id = target_ar_id
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_start_point_id:
pass
#target_id_flage = False
elif self.m_idd == target_start_point_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[
target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[
target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[
target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[
target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[
target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[
target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[
target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.ar_pose.orientation.x, self.ar_pose.orientation.y,
self.ar_pose.orientation.z, self.ar_pose.orientation.w
]
(self.goal_roll,
self.goal_pitch, self.goal_yaw) = euler_from_quaternion(
self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
rospy.init_node("motion_planning")
scene = PlanningSceneInterface()
robot = RobotCommander()
L_arm = MoveGroupCommander("L_arm")
rospy.sleep (1)
#print (robot.get_current_variable_values())
print (L_arm.get_current_pose())
print (L_arm.get_planning_frame())
L_arm.set_planning_time(10);
L_arm.set_pose_reference_frame("omo_L")
L_arm.set_position_target([-0.068, -0.120, 0.520])
print (L_arm.get_end_effector_link())
plan = L_arm.plan()
print (plan)
L_arm.execute(plan)
print (L_arm.get_current_pose(L_arm.get_end_effector_link()))
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
roscpp_initialize(sys.argv)
rospy.init_node('joint_control_UR', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
group_name = "ur3_manipulator"
move_group = MoveGroupCommander(group_name)
FIXED_FRAME = 'world'
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
scene.add_sphere("ball", p, 0.1)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.1
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)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
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.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
self.clear_octomap()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose : ", self.wpose
marker_joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def look_object(self):
look_object = self.robot_arm.get_current_joint_values()
## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함
print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5]
#look_object[5] = math.radians(90)
look_object[5] = 0
'''
if look_object[5] > abs(math.radians(180)):
if look_object[5] < 0:
look_object[5] = look_object[5] + math.radians(90) # wrist3
elif look_object[5] > 0:
look_object[5] = look_object[5] - math.radians(90)
else:
look_object[5] = look_object[5] - math.radians(90) # wrist3
'''
print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5]
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap... Please wait...."
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
print "waypoints:", waypoints
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
print "======= pos(meter): ", self.position_list
print "======= orientation: ", self.orientation_list
def go_to_move(self, scale = 1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
coke_offset = [0, -0.35, -0.1] #x y z
# gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품.
# linear offset = abs([0, 0.0625, 0.13])
robot_base_offset = 0.873
base_wrist2_offset = 0.1 #for avoiding link contact error
print "========== robot arm plannig frame: \n", planning_frame
self.calculed_coke_pose.position.x = (scale * self.goal_x) + base_wrist2_offset # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1]
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset
self.calculed_coke_pose.position.z = (scale * self.goal_z) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z]
tf_display_orientation = [self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
coke_waypoints = []
coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose))
(coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(coke_waypoints, 0.01, 0.0)
self.display_trajectory(coke_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def ar_tf_listener(self, msg):
try:
self.marker = msg.markers
ml = len(self.marker)
target_id = 9
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_id:
pass
#target_id_flage = False
elif self.m_idd == target_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w]
(self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_demo')
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
#cartesian = rospy.get_param('~cartesian', True)
print "===== It is OK ===="
rospy.sleep(3)
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=1)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Set the left arm reference frame
left_arm.set_pose_reference_frame('base')
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 10
# Give the scene a chance to catch up
rospy.sleep(2)
#object1_id = 'object1'
table_id = 'table'
target_id = 'target'
#tool_id = 'tool'
#obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
#scene.remove_world_object(object1_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
#scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object('base', target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.0
#object1_size = [0.088, 0.04, 0.02]
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
#obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = left_reference_frame
#obstacle1_pose.pose.position.x = 0.96
#obstacle1_pose.pose.position.y = 0.24
#obstacle1_pose.pose.position.z = 0.04
#obstacle1_pose.pose.orientation.w = 1.0
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
#self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
#object1_pose = PoseStamped()
#object1_pose.header.frame_id = left_reference_frame
#object1_pose.pose.position.x = 0.80
#object1_pose.pose.position.y = 0.04
#object1_pose.pose.position.z = table_ground + table_size[2] + object1_size[2] / 2.0
#object1_pose.pose.orientation.w = 1.0
#scene.add_box(object1_id, object1_pose, object1_size)
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = left_reference_frame
table_pose.pose.position.x = 1
table_pose.pose.position.y = 0.7
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = left_reference_frame
target_pose.pose.position.x = 1
target_pose.pose.position.y = 0.7
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
#self.setColor(object1_id, 0.8, 0, 0, 1.0)
self.setColor(table_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
left_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = left_reference_frame
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = -0.18
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = left_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = left_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
# Open the gripper to the neutral position
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def main():
rospy.init_node('duaro')
botharms = MoveGroupCommander("botharms")
upper = MoveGroupCommander("upper_arm")
lower = MoveGroupCommander("lower_arm")
duaro = RobotCommander()
# Add Object to Planning Scene
rospy.loginfo("Planning Scene Settings")
scene = PlanningSceneInterface()
rospy.sleep(2) # Waiting for PlanningSceneInterface
scene.remove_world_object()
current_robot = duaro.get_current_state()
current_joint = current_robot.joint_state.position
rospack = rospkg.RosPack()
resourcepath = rospack.get_path(
'khi_duaro_onigiri_system') + "/config/meshes/"
#conveyer
box_pose = PoseStamped()
box_pose.header.frame_id = botharms.get_planning_frame()
rot_o = 180.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
#setting origin of conveyer
box_pose.pose.position.x = 0.2153
box_pose.pose.position.y = 0.32454
box_pose.pose.position.z = 0.0
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
meshpath = resourcepath + "conveyer.stl"
scene.add_mesh('conveyer', box_pose, meshpath, (1, 1, 1))
rospy.sleep(3) # Waiting for setting conveyer
#stand
rot_o = 0.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
#setting origin of stand
box_pose.pose.position.x = -0.385
box_pose.pose.position.y = 0.2342
box_pose.pose.position.z = 0.0
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
meshpath = resourcepath + "stand.stl"
scene.add_mesh('stand', box_pose, meshpath, (1, 1, 1))
rospy.sleep(3) # Waiting for setting stand
rospy.loginfo("Planning Scene Settings Finish")
#tray
rot_o = 0.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
#setting origin of tray
box_pose.pose.position.x = -0.58952
box_pose.pose.position.y = -0.0613
box_pose.pose.position.z = 0.6375
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
meshpath = resourcepath + "tray_nooffset.stl"
scene.add_mesh('tray', box_pose, meshpath, (1, 1, 1))
rospy.loginfo("Planning Scene Settings Finish")
class Move():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.br = tf.TransformBroadcaster()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
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.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def initial_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
2.3884003162384033, -1.6826804319964808, 0.7239289283752441,
-2.2834256331073206, 4.623193740844727, -0.025881592427388966
]
print("init pose : ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_move(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
2.3886876106262207, -1.6729376951800745, 1.6410179138183594,
-2.855138603840963, 4.623205661773682, -0.025869671498433888
]
print("move_pose : ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_tomato(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
2.238821506500244, -0.8660662809955042, 0.8210840225219727,
-2.8785727659808558, 4.622917175292969, -0.025917832051412404
]
print("tomato_pose : ",
self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
class Arm(object):
def __init__(self, move_group):
# create move group
self.group = MoveGroupCommander(move_group)
def get_pose(self):
"""
Get current pose for specified move group
Returns
ros msg (geometry_msgs.msg._PoseStamped.PoseStamped) - A message
specifying the pose of the move group as position (XYZ) and
orientation (xyzw).
"""
return self.group.get_current_pose().pose
def get_joint_values(self):
"""
Get joint values for specified move group
Returns
joint value (list) - A list of joint values in radians
"""
return self.group.get_current_joint_values()
def get_planning_frame(self):
"""
Get planning frame of move group
"""
return self.group.get_planning_frame()
def clear_pose_targets(self, move_group):
"""
Clear target pose for planning
"""
self.group.clear_pose_targets()
def plan_to_pose(self, pose):
"""
Plan to a given pose for the end-effector
Args
pose (msg): geometry_msgs.msg.Pose
"""
# Clear old pose targets
self.group.clear_pose_targets()
# Set target pose
self.group.set_pose_target(pose)
numTries = 0
while numTries < 5:
plan = self.group.plan()
numTries += 1
if len(plan.joint_trajectory.points) > 0:
print('succeeded in %d tries' % numTries)
return True
print('Planning failed')
return False
def plan_to_joints(self, joint_angles):
"""Plan to a given joint configuration
Args
joint_angles (list of floats): list of len 7 of joint angles in radians
Returns
True if planning succeeds, False if not
"""
# Clear old pose targets
self.group.clear_pose_targets()
# Set Joint configuration target
self.group.set_joint_value_target(joint_angles)
numTries = 0
while numTries < 5:
plan = group.plan()
numTries += 1
if len(plan.joint_trajectory.points) > 0:
print('succeeded in %d tries' % numTries)
return True
print("Planning failed")
return False
def move_to_joints(self, joint_angles):
"""
Move to specified joint configuration
Args
joint_angles (list of floats): list of len 7 of joint angles in radians
blocking (bool): If True,
Returns
True if planning succeeds, False if not
"""
# plan
plan = self.plan_to_joints(group.get_name(), joint_angles)
# move to joint configuration
if plan:
group.go(wait=True)
return plan
def move_to_pose(self, pose):
"""
Move to specified joint configuration
Args
pose (msg): geometry_msgs.msg.Pose
joint_angles (list of floats): list of len 7 of joint angles in radians
blocking (bool): If True,
Returns
True if planning succeeds, False if not
"""
# plan
plan = self.plan_to_pose(pose)
if plan:
group.go(wait=blocking)
group.stop()
group.clear_pose_targets()
return
def _change_planner(self, planner):
planners = [
"SBLkConfigDefault", "ESTkConfigDefault", "LBKPIECEkConfigDefault",
"BKPIECEkConfigDefault", "KPIECEkConfigDefault",
"RRTkConfigDefault", "RRTConnectkConfigDefault",
"RRTstarkConfigDefault", "TRRTkConfigDefault", "PRMkConfigDefault",
"PRMstarkConfigDefault"
]
if planner in planners:
self.group.set_planner_id(planner)
return
def create_subscriber(self, topic, message_type):
return
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
# self.listener = tf.TransformListener()
#self.goal_x = 0
#self.goal_y = 0
#self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.t_idd = 0
self.t_pose_x = []
self.t_pose_y = []
self.t_pose_z = []
self.t_ori_w = []
self.t_ori_x = []
self.t_ori_y = []
self.t_ori_z = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
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.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_move(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
tomato_offset = [0, -0.35, -0.1]
robot_base_offset = 0.873
base_wrist2_offset = 0.1
print(">> robot arm planning frame: \n", planning_frame)
'''
self.calculated_tomato.position.x = (scale*self.goal_x)
self.calculated_tomato.position.y = (scale*self.goal_y)
self.calculated_tomato.position.z = (scale*self.goal_z)
'''
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation = Quaternion(
*quaternion_from_euler(3.14, 0.1, 1.57))
print("=== robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("=== Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
'''
Manipulator is moving to the desired coordinate
Now move to the calculated_tomato_id_goal
'''
calculated_tomato_id_goal = Pose()
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n", Cplanning_frame)
calculated_tomato_id_goal.position.x = desired_goal_pose[0]
calculated_tomato_id_goal.position.y = desired_goal_pose[1]
calculated_tomato_id_goal.position.z = desired_goal_pose[2]
calculated_tomato_id_goal.orientation = Quaternion(
*quaternion_from_euler(desired_goal_euler[0],
desired_goal_euler[1],
desired_goal_euler[2]))
print(">> tomato_id_goal frame: ", desired_goal_pose)
self.robot_arm.set_pose_target(calculated_tomato_id_goal)
tf_display_position = [
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.z
]
tf_display_orientation = [
calculated_tomato_id_goal.orientation.x,
calculated_tomato_id_goal.orientation.y,
calculated_tomato_id_goal.orientation.z,
calculated_tomato_id_goal.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(
copy.deepcopy(calculated_tomato_id_goal))
(tomato_id_goal_plan,
tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(
tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
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)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
0.07100913858593216, -1.8767615298285376, 2.0393206555899503,
-1.8313959190971882, -0.6278395875738125, 1.6918219826764682
]
self.robot_arm.go(marker_joint_goal, wait=True)
def look_object(self):
look_object = self.robot_arm.get_current_joint_values()
## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함
print "wrist3 joint value(deg, rad): ", math.degrees(
look_object[5]), look_object[5]
look_object[5] = math.radians(90)
'''
if look_object[5] > abs(math.radians(180)):
if look_object[5] < 0:
look_object[5] = look_object[5] + math.radians(90) # wrist3
elif look_object[5] > 0:
look_object[5] = look_object[5] - math.radians(90)
else:
look_object[5] = look_object[5] - math.radians(90) # wrist3
'''
print "wrist3 joint value(deg, rad): ", math.degrees(
look_object[5]), look_object[5]
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap... Please wait...."
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def plan_cartesian_path(self, x_offset, y_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.goal_y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
print "======= pos(meter): ", self.position_list
print "======= orientation: ", self.orientation_list
def go_to_move(self, scale=1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
print "========== robot arm plannig frame: \n", planning_frame
self.calculed_coke_pose.position.x = (
scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale * self.goal_y) - 0.25
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + 0.72 + 0.2 # world to base_link z-offset real version offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 1.54))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [
self.calculed_coke_pose.position.x,
self.calculed_coke_pose.position.y,
self.calculed_coke_pose.position.z
]
tf_display_orientation = [
self.calculed_coke_pose.orientation.x,
self.calculed_coke_pose.orientation.y,
self.calculed_coke_pose.orientation.z,
self.calculed_coke_pose.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def ar_tf_listener(self, msg):
try:
self.marker = msg.markers
ml = len(self.marker)
target_id = 9
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_id:
pass
#target_id_flage = False
elif self.m_idd == target_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[
target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[
target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[
target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[
target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[
target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[
target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[
target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.ar_pose.orientation.x, self.ar_pose.orientation.y,
self.ar_pose.orientation.z, self.ar_pose.orientation.w
]
(self.goal_roll,
self.goal_pitch, self.goal_yaw) = euler_from_quaternion(
self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
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)
#self.clear_octomap()
def ar_callback(self, msg):
#marker = msg.markers[1]
self.marker = msg.markers
ml = len(self.marker)
m_id = self.marker[0].id
print "marker length: ", len(self.marker)
print "marker id: ", m_id
m_idd = []
'''
for ii in range(0, 2):
print(ii)
m_idd[ii] = marker[ii].id
#m_id[ii] = marker[ii].id
print(m_idd[ii])
'''
pos_x = self.marker[0].pose.pose.position.x
pos_y = self.marker[0].pose.pose.position.y
pos_z = self.marker[0].pose.pose.position.z
dist = math.sqrt((pos_x * pos_x) + (pos_y * pos_y))
#ori_x = marker.pose.pose.orientation.x
#ori_y = marker.pose.pose.orientation.y
#ori_z = marker.pose.pose.orientation.z
#ori_w = marker.pose.pose.orientation.w
#print(m_id)
print "==========="
print 'id: ', m_id
print 'pos: ', pos_x, pos_y, pos_z
#print('ori: ', ori_x, ori_y, ori_z, ori_w)
self.goal_x = pos_x - 1.0
self.goal_y = pos_y - pos_y
self.goal_z = pos_z - pos_z
def move_code(self):
robot_state = self.robot_arm.get_current_pose()
print "====== robot pose: \n",
print robot_state.pose.position
#marker_joint_goal = self.robot_arm.get_current_joint_values()
marker_joint_goal = [
0.07100913858593216, -1.8767615298285376, 2.0393206555899503,
-1.8313959190971882, -0.6278395875738125, 1.6918219826764682
]
self.robot_arm.go(marker_joint_goal, wait=True)
print "====== robot joint value: \n"
print marker_joint_goal
self.robot_arm.go(marker_joint_goal, wait=True)
self.robot_arm.go(True)
print "look at the markers"
pose_goal = Pose()
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.4
pose_goal.position.y = -0.4
pose_goal.position.z = 0.7
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
#self.robot_arm.set_pose_target(pose_goal)
#7self.robot_arm.go(True)
def plan_cartesian_path(self, scale=1):
waypoints = []
wpose = self.robot_arm.get_current_pose().pose
wpose.position.z -= scale * 0.2
wpose.position.x -= scale * 0.2
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
rospy.loginfo("Maker messages detected. Starting followers...")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
object1_id = 'object1'
obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
scene.remove_world_object(object1_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
object1_size = [0.088, 0.04, 0.02]
# Add a table top and two boxes to the scene
obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
obstacle1_pose = PoseStamped()
obstacle1_pose.header.frame_id = left_reference_frame
obstacle1_pose.pose.position.x = 0.96
obstacle1_pose.pose.position.y = 0.5
obstacle1_pose.pose.position.z = 0.04
obstacle1_pose.pose.orientation.w = 1.0
scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
object1_pose = PoseStamped()
object1_pose.header.frame_id = left_reference_frame
object1_pose.pose.position.x = 0.80
object1_pose.pose.position.y = 0.3
object1_pose.pose.position.z = 0
object1_pose.pose.orientation.w = 1.0
scene.add_box(object1_id, object1_pose, object1_size)
self.setColor(object1_id, 0.8, 0, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the start state to the current state
left_arm.set_start_state_to_current_state()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = left_reference_frame
target_pose.pose.position.x = 0.58
target_pose.pose.position.y = 0.1878
target_pose.pose.position.z = .1116
target_pose.pose.orientation.x = 0.1325
target_pose.pose.orientation.y = 0.9908
target_pose.pose.orientation.z = 0.0095
target_pose.pose.orientation.w = 0.0254
# Set the target pose for the arm
left_arm.set_pose_target(target_pose, left_eef)
# Move the arm to the target pose (if possible)
left_arm.go()
# Pause for a moment...
rospy.sleep(2)
left_gripper.set_joint_value_target(GRIPPER_CLOSE)
left_gripper.go()
rospy.sleep(1)
scene.attach_box(left_eef, object1_id, object1_pose, object1_size)
# Cartesian Paths
waypoints1 = []
start_pose = left_arm.get_current_pose(left_eef).pose
wpose = deepcopy(start_pose)
waypoints1.append(deepcopy(wpose))
wpose.position.x -= 0.05
wpose.position.y += 0.105
wpose.position.z += 0.07
waypoints1.append(deepcopy(wpose))
wpose.position.x -= 0.05
wpose.position.y += 0.105
wpose.position.z -= 0.07
waypoints1.append(deepcopy(wpose))
(cartesian_plan, fraction) = left_arm.compute_cartesian_path(
waypoints1, # waypoint poses
0.01, # eef_step 1cm
0.0, # jump_threshold
True) # avoid_collisions
left_arm.execute(cartesian_plan)
rospy.sleep(2)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
scene.remove_attached_object(left_eef, object1_id)
# Exit MoveIt cleanly
waypoints2 = []
start_pose = left_arm.get_current_pose(left_eef).pose
wpose = deepcopy(start_pose)
waypoints2.append(deepcopy(wpose))
wpose.position.z += 0.07
waypoints2.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.y -= 0.21
waypoints2.append(deepcopy(wpose))
wpose.position.z -= 0.07
waypoints2.append(deepcopy(wpose))
(cartesian_plan, fraction) = left_arm.compute_cartesian_path(
waypoints2, # waypoint poses
0.01, # eef_step 1cm
0.0, # jump_threshold
True) # avoid_collisions
left_arm.execute(cartesian_plan)
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_roll = 0
self.goal_pitch = 0
self.goal_yaw = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.wpose = []
self.marker = []
self.tf_list = []
self.m_idd = []
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.trans = []
self.rot = []
self.pose_goal = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
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)
self.clear_octomap()
def ar_callback(self, msg):
#marker = msg.markers[1]
self.marker = msg.markers
ml = len(self.marker)
m_id = self.marker[0].id
#print "marker length: ", ml, ml-1
#print "marker id: ", m_id, self.marker[1].id
#self.m_idd = []
#self.m_pose_x = []
#self.m_pose_y = []
#self.m_pose_z = []
#self.m_ori_w = []
#self.m_ori_x = []
#self.m_ori_y = []
#self.m_ori_z = []
for ii in range(0, ml):
self.m_idd.append(self.marker[ii].id)
self.m_pose_x.append(self.marker[ii].pose.pose.position.x)
self.m_pose_y.append(self.marker[ii].pose.pose.position.y)
self.m_pose_z.append(self.marker[ii].pose.pose.position.z)
self.m_ori_w.append(self.marker[ii].pose.pose.orientation.w)
self.m_ori_x.append(self.marker[ii].pose.pose.orientation.x)
self.m_ori_y.append(self.marker[ii].pose.pose.orientation.y)
self.m_ori_z.append(self.marker[ii].pose.pose.orientation.z)
'''
print "id: ", self.m_idd
print "pose_x: ", self.m_pose_x
print "pose_y: ", self.m_pose_y
print "pose_z: ", self.m_pose_z
print "ori_w: ", self.m_ori_w
print "ori_x: ", self.m_ori_x
print "ori_y: ", self.m_ori_y
print "ori_z: ", self.m_ori_z
'''
m_pose_goal = Pose()
m_pose_goal.orientation = Quaternion(*quaternion_from_euler(0, -1.5, 0))
m_pose_goal.position.x = self.m_pose_x[0] + 0.029# red line 0.2 0.2
m_pose_goal.position.y = self.m_pose_y[0] - 0.7 # green line 0.15 0.15
m_pose_goal.position.z = self.m_pose_z[0] + 0.3 # blue line # 0.35 0.6
'''
print "goal_pose: ", m_pose_goal
self.robot_arm.set_pose_target(m_pose_goal)
self.robot_arm.go(True)
print "==== check check"
marker_joint_goal = [0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682]
self.robot_arm.go(marker_joint_goal, wait=True)
print "====== robot joint value: \n"
print marker_joint_goal
#self.robot_arm.go(marker_joint_goal, wait=True)
pos_x = self.marker[0].pose.pose.position.x
pos_y = self.marker[0].pose.pose.position.y
pos_z = self.marker[0].pose.pose.position.z
dist = math.sqrt(
(pos_x*pos_x) + (pos_y * pos_y)
)
'''
#ori_x = marker.pose.pose.orientation.x
#ori_y = marker.pose.pose.orientation.y
#ori_z = marker.pose.pose.orientation.z
#ori_w = marker.pose.pose.orientation.w
#print(m_id)
#print "==========="
#print 'id: ', m_id
#print 'pos: ', pos_x, pos_y, pos_z
#print('ori: ', ori_x, ori_y, ori_z, ori_w)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose : ", self.wpose
#self.pose_goal = self.robot_arm.get_current_pose()
self.pose_goal.position.x = 0.062
self.pose_goal.position.y = 0.194
self.pose_goal.position.z = 0.878
self.pose_goal.orientation.x = 0.673
self.pose_goal.orientation.y = 0.673
self.pose_goal.orientation.z = -0.217
self.pose_goal.orientation.w = 0.217
#print "========== goal frame: ", self.pose_goal
self.robot_arm.set_pose_target(self.pose_goal)
self.robot_arm.go(True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap"
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def plan_cartesian_path(self, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
#self.wpose.position.z -= scale * (0.695) # First move up (z)
#self.wpose.position.y += scale * (0.199) # and sideways (y)
#waypoints.append(copy.deepcopy(self.wpose))
print "===== robot arm pose: ", self.wpose
''' # 19.06.11 10:45 분 코딩
print self.wpose.position.x," + (",scale," * ",self.goal_x,")"
print self.wpose.position.y," + (",scale," * ",self.goal_y,")"
self.wpose.position.x = self.wpose.position.x - (scale * self.goal_x)
self.wpose.position.y = self.wpose.position.y + (scale * self.goal_y)
print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
print self.wpose.position.z," + (",scale," * ",self.goal_z,")"
self.wpose.position.z += (scale * self.goal_z)
print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
'''
self.wpose.position.x = (scale * self.wpose.position.x) - 0.10
#self.wpose.position.y = (scale * self.goal_y) - 0.1
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
#print self.wpose.position.z," + (",scale," * ",self.goal_z,")"
#self.wpose.position.z = (scale * self.goal_z) + 0.1
self.wpose.position.y = (scale * self.goal_y) + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
rospy.loginfo("Maker messages detected. Starting followers...")
def go_to_move(self, scale = 1.0):
self.wpose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.pose_goal.position.x = (scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset
self.pose_goal.position.y = (scale * self.goal_y) - 0.25
self.pose_goal.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset
self.pose_goal.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54))
''' 19.06.1 15:58 코딩
self.pose_goal.orientation.x = self.rot[0]
self.pose_goal.orientation.y = self.rot[1]
self.pose_goal.orientation.z = self.rot[2]
self.pose_goal.orientation.w = self.rot[3]
'''
#self.pose_goal.orientation.x = self.goal_roll + 1.57
#self.pose_goal.orientation.y = self.goal_pitch + 1.57
#self.pose_goal.orientation.z = self.goal_yaw
print "========== goal frame: ", self.pose_goal
self.robot_arm.set_pose_target(self.pose_goal)
#self.robot_arm.go(True)
def tf_listener(self): # 수정사안: ar_pose id의 가져와서 위치 값을 읽어와야함.
try:
child_tf = '/ar_marker_9'
parent_tf = '/base_link'
(self.trans,self.rot) = self.listener.lookupTransform(parent_tf, child_tf, rospy.Time(0))
# rosrun tf tf_echo /base_link /ar_marker_9 랑 같은 역할
# transformation: linear transformation, rotation: quaternion
print "======= trans[x, y, z]: ", self.trans
print "======= rotat[x, y, z, w]: ", self.rot
(self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.rot)
print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#convert 확인용
#quat = quaternion_from_euler(goal_roll, goal_pitch, goal_yaw)
#print quat
self.goal_x = self.trans[0]
self.goal_y = self.trans[1]
self.goal_z = self.trans[2]
except Exception as ex:
print "======== tf_listener Exception!!"
rospy.loginfo(ex)
def look_object(self):
try:
look_object = self.robot_arm.get_current_joint_values()
#look_object[4] = look_object[4] + (math.radians(90)) # wrist2
look_object[5] = look_object[5] + (3.1) # wrist3
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
except Exception as ex:
print "======== look_object Exception!!"
rospy.loginfo(ex)
