p1.pose.orientation.z = b1[2]
p1.pose.orientation.w = b1[3]
#glovebox pose
p2.pose.position.x = 0
p2.pose.position.y = 0
p2.pose.position.z = 0.56
b2 = tf.transformations.quaternion_from_euler(1.57,0,0)
p2.pose.orientation.x = b2[0]
p2.pose.orientation.y = b2[1]
p2.pose.orientation.z = b2[2]
p2.pose.orientation.w = b2[3]
#detach/remove current scene objects
print 'removing objects...'
robot.detach_object("bowl")
rospy.sleep(1)
scene.remove_world_object("bowl")
scene.remove_world_object("punch")
scene.remove_world_object("glovebox")
rospy.sleep(2)
#reset the gripper and arm position to home
robot.set_start_state_to_current_state()
robot.set_named_target("start_glove")
robot.go()
gripper.set_start_state_to_current_state()
gripper.go()
#add scene objects
print state
if (state):
place_pose = PoseStamped()
place_pose.header.frame_id = robot.get_planning_frame()
place_pose.pose.position.x = -0.5
place_pose.pose.position.y = -0.6
place_pose.pose.position.z = 0.35
place_pose.pose.orientation.y = 1
arm.set_pose_target(place_pose)
pl = arm.plan()
state = arm.execute(pl)
print state
if (state):
group_variable_values = eef.get_current_joint_values()
group_variable_values[0] = 0.0
eef.detach_object("part")
place_pose = PoseStamped()
place_pose.header.frame_id = robot.get_planning_frame()
place_pose.pose.position.x = -0.495562
place_pose.pose.position.y = -0.607059
place_pose.pose.position.z = 0.417763
place_pose.pose.orientation.y = 1
arm.set_pose_target(place_pose)
pl = arm.plan()
state = arm.execute(pl)
print state
# if(state):
# place_pose = PoseStamped()
# place_pose.header.frame_id = robot.get_planning_frame()
# place_pose.pose.position.x = -0.5
# place_pose.pose.position.y = -0.6
class Ur10eRobot():
def __init__(self):
self.GRIPPER_GROUP = "ur10e_electric_gripper"
self.CAMERA_GROUP = "ur10e_camera"
self.CHANGER_GROUP = "ur10e_tool_changer"
self.GRIPPER_EF_GROUP = "ur10e_gripper_ef"
self.GRIPPER_EF_LINK = "ur10e_gripper_ef"
self.CAMERA_EF_LINK = "ur10e_camera_ef"
self.CHANGER_EF_LINK = "ur10e_changer_ef"
self.___PLANNING_JOINT_NAMES = [
"ur10e_shoulder_pan_joint", "ur10e_shoulder_lift_joint",
"ur10e_elbow_joint", "ur10e_wrist_1_joint", "ur10e_wrist_2_joint",
"ur10e_wrist_3_joint"
]
self.__GRIPPER_EF_JOINT_NAMES = [
"ur10e_upper_finger_base", "ur10e_lower_finger_base"
]
self.__FORCE_SENSOR_TOPIC = "/ur10e_force_topic"
# create move group commander
self.__gripper_group_commander = MoveGroupCommander(self.GRIPPER_GROUP)
self.__camera_group_commander = MoveGroupCommander(self.CAMERA_GROUP)
self.__changer_group_commander = MoveGroupCommander(self.CHANGER_GROUP)
self.__gripper_ef_commander = MoveGroupCommander(self.GRIPPER_EF_GROUP)
"""
Update the current value from force sensor
"""
def update_force_sensor_value(self, force_sensor_value):
self.__force_sensor_value = force_sensor_value
"""
Get the current value from force sensor
"""
def get_force_sensor_value(self):
return self.__force_sensor_value
"""
Open electric gripper
"""
def open_electric_gripper(self):
self.__gripper_ef_commander.set_joint_value_target([0, 0])
self.__gripper_ef_commander.go()
def close_electric_gripper(self, object_width):
max_width = 0.02
if (object_width > max_width):
print("cannot close gripper, object width exceeds the max width")
else:
gripper_dist = (max_width - object_width) / 2
self.__gripper_ef_commander.set_joint_value_target(
[-gripper_dist, -gripper_dist])
self.__gripper_ef_commander.go()
def move_tool_in_straight_line(self,
pose_goal,
avoid_collisions=True,
group_name=None,
n_way_points=2):
if (group_name == self.CAMERA_GROUP):
group = self.__camera_group_commander
elif (group_name == self.CHANGER_GROUP):
group = self.__changer_group_commander
else:
group = self.__gripper_group_commander
way_points_list = []
way_point = Pose()
x_way_points = np.linspace(0, pose_goal[0], n_way_points)
y_way_points = np.linspace(0, pose_goal[1], n_way_points)
z_way_points = np.linspace(0, pose_goal[2], n_way_points)
qx_way_points = np.linspace(0, 0, n_way_points)
qy_way_points = np.linspace(0, 0, n_way_points)
qz_way_points = np.linspace(0, 0, n_way_points)
qw_way_points = np.linspace(0, 0, n_way_points)
for i in range(n_way_points):
way_point.position.x = x_way_points[i]
way_point.position.y = y_way_points[i]
way_point.position.z = z_way_points[i]
way_point.orientation.x = qx_way_points[i]
way_point.orientation.y = qy_way_points[i]
way_point.orientation.z = qz_way_points[i]
way_point.orientation.w = qw_way_points[i]
way_points_list.append(way_point)
group.set_pose_reference_frame(group.get_end_effector_link())
# group.set_goal_tolerance(0.001)
plan, fraction = group.compute_cartesian_path(way_points_list, 0.005,
0.0)
print(fraction)
# print plan
# post processing for the trajectory to ensure its validity
last_time_step = plan.joint_trajectory.points[0].time_from_start.to_sec
new_plan = RobotTrajectory()
new_plan.joint_trajectory.header = plan.joint_trajectory.header
new_plan.joint_trajectory.joint_names = plan.joint_trajectory.joint_names
new_plan.joint_trajectory.points.append(
plan.joint_trajectory.points[0])
for i in range(1, len(plan.joint_trajectory.points)):
point = plan.joint_trajectory.points[i]
if (point.time_from_start.to_sec > last_time_step):
new_plan.joint_trajectory.points.append(point)
last_time_step = point.time_from_start.to_sec
# execute the trajectory
group.execute(new_plan)
"""
Go to pose goal. Plan and execute and wait for the controller respons
:param pose_goal list in this form [x, y, z, r, p, y]
or [x, y, z, qx, qy, qz, qw]
or [x, y, z]
:type list of int
:param ef_link
:type: string
"""
def go_to_pose_goal(self, pose_goal, group_name=None):
if (group_name == self.CAMERA_GROUP):
group = self.__camera_group_commander
elif (group_name == self.CHANGER_GROUP):
group = self.__changer_group_commander
else:
group = self.__gripper_group_commander
if (len(pose_goal) == 6 or len(pose_goal) == 7):
group.set_pose_target(pose_goal)
group.go()
elif (len(pose_goal) == 3):
group.set_position_target(pose_goal)
group.go()
else:
print("Invalid inputs")
"""
Go to joint goal
"""
def go_to_joint_goal(self, joint_goal):
if (len(joint_goal) == 6):
joint_goal_msg = JointState()
joint_goal_msg.name = self.___PLANNING_JOINT_NAMES
joint_goal_msg.position = joint_goal
self.__camera_group_commander.set_joint_value_target(
joint_goal_msg)
self.__camera_group_commander.go()
else:
print("Invalid inputs")
"""
pick an object using electric gripper
This function assumes that the electric gripper is already in a place near to the object. With an appropriate orientation.
So what is left in order to pick. is to approach the object. turn electric gripper on and then retreat.
"""
def pick_object(self, object_name, table_name, approach_dist, retreat_dist,
object_width):
# disable collision between object and table
self.__gripper_group_commander.set_support_surface_name(table_name)
# approach the object
self.move_tool_in_straight_line(approach_dist, avoid_collisions=False)
# attach object and open gripper
self.__gripper_group_commander.attach_object(object_name)
# close electric gripper
self.close_electric_gripper(object_width)
rospy.sleep(2)
# retreat
self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True)
"""
Place an object using electric gripper
"""
def place_object(self, object_name, table_name, approach_dist,
retreat_dist):
# disable collision between object and table
self.__gripper_group_commander.set_support_surface_name(table_name)
# approach the table
self.move_tool_in_straight_line(approach_dist, avoid_collisions=False)
# detach object and turn off gripper
self.__gripper_group_commander.detach_object(object_name)
# open electric gripper
self.open_electric_gripper()
rospy.sleep(2)
# retreat
self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True)
class KukaRobot:
def __init__(self):
self.VACUUM_GROUP = "kuka_vacuum_gripper"
self.CAMERA_GROUP = "kuka_camera"
self.IMPACT_GROUP = "kuka_impact_wrench"
self.VACUUM_EF_LINK = "kuka_vacuum_ef"
self.CAMERA_EF_LINK = "kuka_camera_ef"
self.IMPACT_EF_LINK = "kuka_impact_ef"
self.___PLANNING_JOINT_NAMES = [
"kuka_joint_a1", "kuka_joint_a2", "kuka_joint_a3", "kuka_joint_a4",
"kuka_joint_a5", "kuka_joint_a6", "kuka_joint_a7"
]
self.__FORCE_SENSOR_TOPIC = "/kuka_force_topic"
self.__VACUUM_STATUS_TOPIC = "/kuka_vacuum_topic"
# create move group commander
self.__vacuum_group_commander = MoveGroupCommander(self.VACUUM_GROUP)
self.__camera_group_commander = MoveGroupCommander(self.CAMERA_GROUP)
self.__impact_group_commander = MoveGroupCommander(self.IMPACT_GROUP)
# initialize vacuum services
self.__vacuum_on_service = rospy.ServiceProxy("/on", Empty)
self.__vacuum_off_service = rospy.ServiceProxy("/off", Empty)
# initialize feedback variables
self.__vacuum_status = Bool()
self.__force_sensor_value = WrenchStamped()
"""
Update the current value from force sensor
"""
def update_force_sensor_value(self, force_sensor_value):
self.__force_sensor_value = force_sensor_value
"""
Get the current value from force sensor
"""
def get_force_sensor_value(self):
return self.__force_sensor_value
"""
Update Vacuum status
"""
def update_vacuum_status(self, vacuum_status):
self.__vacuum_status = vacuum_status
"""
Get Vacuum status
"""
def get_vacuum_status(self):
return self.__vacuum_status
"""
Turn on Vacuum gripper
"""
def vacuum_gripper_on(self):
self.__vacuum_on_service.wait_for_service(0.2)
request = EmptyRequest()
self.__vacuum_on_service(request)
"""
Turn off Vacuum gripper
"""
def vacuum_gripper_off(self):
self.__vacuum_off_service.wait_for_service(0.2)
request = EmptyRequest()
self.__vacuum_off_service(request)
"""
move in a straight line with respect to the tool reference frame
"""
def move_tool_in_straight_line(self,
pose_goal,
avoid_collisions=True,
group_name=None,
n_way_points=2):
if group_name == self.CAMERA_GROUP:
group = self.__camera_group_commander
elif group_name == self.IMPACT_GROUP:
group = self.__impact_group_commander
else:
group = self.__vacuum_group_commander
way_points_list = []
way_point = Pose()
x_way_points = np.linspace(0, pose_goal[0], n_way_points)
y_way_points = np.linspace(0, pose_goal[1], n_way_points)
z_way_points = np.linspace(0, pose_goal[2], n_way_points)
qx_way_points = np.linspace(0, 0, n_way_points)
qy_way_points = np.linspace(0, 0, n_way_points)
qz_way_points = np.linspace(0, 0, n_way_points)
qw_way_points = np.linspace(0, 0, n_way_points)
for i in range(n_way_points):
way_point.position.x = x_way_points[i]
way_point.position.y = y_way_points[i]
way_point.position.z = z_way_points[i]
way_point.orientation.x = qx_way_points[i]
way_point.orientation.y = qy_way_points[i]
way_point.orientation.z = qz_way_points[i]
way_point.orientation.w = qw_way_points[i]
way_points_list.append(way_point)
group.set_pose_reference_frame(group.get_end_effector_link())
# group.set_goal_tolerance(0.001)
plan, fraction = group.compute_cartesian_path(way_points_list, 0.005,
0.0, avoid_collisions)
print(fraction)
# print plan
# post processing for the trajectory to ensure its validity
last_time_step = plan.joint_trajectory.points[0].time_from_start.to_sec
new_plan = RobotTrajectory()
new_plan.joint_trajectory.header = plan.joint_trajectory.header
new_plan.joint_trajectory.joint_names = plan.joint_trajectory.joint_names
new_plan.joint_trajectory.points.append(
plan.joint_trajectory.points[0])
for i in range(1, len(plan.joint_trajectory.points)):
point = plan.joint_trajectory.points[i]
if point.time_from_start.to_sec > last_time_step:
new_plan.joint_trajectory.points.append(point)
last_time_step = point.time_from_start.to_sec
# execute the trajectory
group.execute(new_plan)
"""
Go to pose goal. Plan and execute and wait for the controller response
:param pose_goal list in this form [x, y, z, r, p, y]
or [x, y, z, qx, qy, qz, qw]
or [x, y, z]
:type list of int
:param ef_link
:type: string
"""
def go_to_pose_goal(self, pose_goal, group_name=None):
if group_name == self.CAMERA_GROUP:
group = self.__camera_group_commander
elif group_name == self.IMPACT_GROUP:
group = self.__impact_group_commander
else:
group = self.__vacuum_group_commander
if len(pose_goal) == 6 or len(pose_goal) == 7:
group.set_pose_target(pose_goal)
group.go()
elif len(pose_goal) == 3:
group.set_position_target(pose_goal)
group.go()
else:
print("Invalid inputs")
"""
Go to joint goal
"""
def go_to_joint_goal(self, joint_goal):
if len(joint_goal) == 7:
joint_goal_msg = JointState()
joint_goal_msg.name = self.___PLANNING_JOINT_NAMES
joint_goal_msg.position = joint_goal
self.__camera_group_commander.set_joint_value_target(
joint_goal_msg)
self.__camera_group_commander.go()
else:
print("Invalid inputs")
"""
pick an object using vacuum gripper
This function assumes that the vacuum gripper is already in a place near to the object.
With an appropriate orientation.
So what is left in order to pick. is to approach the object. turn vacuum gripper on and then retreat.
"""
def pick_object(self, object_name, table_name, approach_dist,
retreat_dist):
# disable collision between object and table
self.__vacuum_group_commander.set_support_surface_name(table_name)
# approach the object
self.move_tool_in_straight_line(approach_dist, avoid_collisions=True)
# attach object and open gripper
self.__vacuum_group_commander.attach_object(object_name)
# turn on vacuum gripper
self.vacuum_gripper_on()
rospy.sleep(2)
# retreat
self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True)
"""
Place an object using vacuum gripper
"""
def place_object(self, object_name, table_name, approach_dist,
retreat_dist):
# disable collision between object and table
self.__vacuum_group_commander.set_support_surface_name(table_name)
# approach the table
self.move_tool_in_straight_line(approach_dist, avoid_collisions=True)
# detach object and turn off gripper
self.__vacuum_group_commander.detach_object(object_name)
# turn off vacuum gripper
self.vacuum_gripper_off()
rospy.sleep(2)
# retreat
self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True)
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ",robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault")
arm.set_planning_time(5.0)
arm.detach_object("pen")
# set the arm to a safe target
arm.set_named_target("gamma")
# plan and execute the motion
arm.go()
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault")
hand.set_planning_time(10.0)
# set the hand to a safe target
hand.set_named_target("open")
# plan and execute the motion
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
gripper = MoveGroupCommander('gripper')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_attached_object(box2_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
'''arm.set_named_target('home')
arm.go()
rospy.sleep(2)'''
# 设置桌面的高度
table_ground = 0.6
# 设置table、box1和box2的三维尺寸
table_size = [1.5, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.05]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.0
table_pose.pose.position.y = 0.5
table_pose.pose.position.z = 0.825
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.2
box1_pose.pose.position.y = 0.3
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = -0.1
box2_pose.pose.position.y = 0.5
box2_pose.pose.position.z = 0.85
box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.3, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置机械臂的运动目标位置，进行避障规划
orient = transformations.quaternion_from_euler(0, 1.57075, 0)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = -0.1
target_pose.pose.position.y = 0.5
target_pose.pose.position.z = 1.05
target_pose.pose.orientation.x = orient[0]
target_pose.pose.orientation.y = orient[1]
target_pose.pose.orientation.z = orient[2]
target_pose.pose.orientation.w = orient[3]
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(0.5)
target_pose.pose.position.z -= 0.05
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
self.gripper_control(0.355)
'''arm.attach_object( box2_id)
arm.set_named_target('home')
arm.go()'''
#self.gripper_control(0)
arm.detach_object(box2_id)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen", p, resourcepath + 'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ", robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault")
arm.set_planning_time(5.0)
arm.detach_object("pen")
# set the arm to a safe target
arm.set_named_target("gamma")
# plan and execute the motion
arm.go()
# setup the hand group and its planner
hand = MoveGroupCommander("hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault")
hand.set_planning_time(10.0)
# set the hand to a safe target
hand.set_named_target("open")
# plan and execute the motion
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
#self.robot_arm.set_num_planning_attempts(5)
self.robot_arm.remember_joint_values("zeros", None)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
#add Tabl
p = PoseStamped()
p.header.frame_id = FIXED_FRAME
p.pose.position.x = 0.3
p.pose.position.y = 0.2
p.pose.position.z = 0.1
self.scene.add_box("table", p, (0.02, 0.02, 0.02))
pose_goal.position.x = 0.3
pose_goal.position.y = 0.2
pose_goal.position.z = 0.15
#Orientation = [1.57,0,0]
Orientation[0] = 0
Orientation[1] = 0
Orientation[2] = 1.57
def WASD(self):
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
rospy.sleep(2)
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
rospy.sleep(2)
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
print("trying done")
print("\n")
print("WASD keyboard control")
while (True):
print("Available commands : ")
print(" W - move X forward")
print(" S - move X backward")
print(" A - move Y left")
print(" D - move Y right")
print(" R - move Z left")
print(" F - move Z right")
print("*****************")
print(" X - pick object")
print(" Y - pick object")
print("current orient", Orientation)
command = raw_input("Control : ")
if command == 'w':
pose_goal.orientation.w = 0.0
pose_goal.position.x = pose_goal.position.x + 0.05
pose_goal.position.y = pose_goal.position.y
pose_goal.position.z = pose_goal.position.z
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 's':
pose_goal.orientation.w = 0.0
pose_goal.position.x = pose_goal.position.x - 0.05
pose_goal.position.y = pose_goal.position.y
pose_goal.position.z = pose_goal.position.z
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'a':
pose_goal.orientation.w = 0.0
pose_goal.position.x = pose_goal.position.x
pose_goal.position.y = pose_goal.position.y - 0.05
pose_goal.position.z = pose_goal.position.z
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'd':
pose_goal.orientation.w = 0.0
pose_goal.position.x = pose_goal.position.x
pose_goal.position.y = pose_goal.position.y + 0.05
pose_goal.position.z = pose_goal.position.z
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'r':
pose_goal.orientation.w = 0.0
pose_goal.position.x = pose_goal.position.x
pose_goal.position.y = pose_goal.position.y
pose_goal.position.z = pose_goal.position.z + 0.05
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'f':
pose_goal.orientation.w = 0.0
pose_goal.position.x = pose_goal.position.x
pose_goal.position.y = pose_goal.position.y
pose_goal.position.z = pose_goal.position.z - 0.05
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'x':
self.robot_arm.attach_object("table")
elif command == 'y':
self.robot_arm.detach_object("table")
elif command == 'v':
self.set_pose_target(pose_goal)
elif command == 'i':
Orientation[0] = Orientation[0] + 0.2
Orientation[1] = Orientation[1]
Orientation[2] = Orientation[2]
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'k':
Orientation[0] = Orientation[0] - 0.2
Orientation[1] = Orientation[1]
Orientation[2] = Orientation[2]
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
# yaw
elif command == 'u':
Orientation[0] = Orientation[0]
Orientation[1] = Orientation[1] + 0.2
Orientation[2] = Orientation[2]
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'o':
Orientation[0] = Orientation[0]
Orientation[1] = Orientation[1] - 0.2
Orientation[2] = Orientation[2]
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
# roll
elif command == 'j':
Orientation[0] = Orientation[0]
Orientation[1] = Orientation[1]
Orientation[2] = Orientation[2] + 0.2
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
elif command == 'l':
Orientation[0] = Orientation[0]
Orientation[1] = Orientation[1]
Orientation[2] = Orientation[2] - 0.2
quater = quaternion_from_euler(Orientation[0], Orientation[1],
Orientation[2])
pose_goal.orientation.x = quater[0]
pose_goal.orientation.y = quater[1]
pose_goal.orientation.z = quater[2]
pose_goal.orientation.w = quater[3]
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.robot_arm.remember_joint_values("zeros", None)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
#self.robot_arm.allow_replanning(True)
#scene = moveit_commander.PlanningSceneInterface()
#robot = moveit_commander.RobotCommander()
#rospy.sleep(2)
#add Table
p = PoseStamped()
p.header.frame_id = FIXED_FRAME
p.pose.position.x = 0.0
p.pose.position.y = 0.2
p.pose.position.z = 0.1
self.scene.add_box("table", p, (0.3, 0.3, 0.3))
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(1)
self.robot_arm.attach_object("table")
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.4 # red line 0.2 0.2
pose_goal.position.y = 0.15 # green line 0.15 0.15
pose_goal.position.z = 0.2 # blue line # 0.35 0.6
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
#if (input == 'w')
#pose_goal.position.x += 0.1 # red line 0.2 0.2
print("====== move plan go to up ======")
self.robot_arm.set_named_target("zeros") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to zeros ======") # rospy.sleep(1)
#self.robot_arm.place("table", pose_goal)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
self.robot_arm.detach_object("table")
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.2 # red line 0.2 0.2
pose_goal.position.y = 0.3 # green line 0.15 0.15
pose_goal.position.z = 0.1 # blue line # 0.35 0.6
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
print(robot_state)
