scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
i = 1
while i <= 10:
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(),
GRIPPER_JOINTS)
rospy.sleep(1)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT,
generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " +
str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
# remove part
scene.remove_world_object(PICK_OBJECT)
scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT)
rospy.sleep(2)
print temPose
temPose.pose.position.z += 0.20
temPose.pose.orientation.y = 1
arm.set_pose_target(temPose)
pl = arm.plan()
state = arm.execute(pl)
print state
if (state):
group_variable_values = eef.get_current_joint_values()
group_variable_values[0] = 0.185
eef.set_joint_value_target(group_variable_values)
plan2 = eef.plan()
eef.execute(plan2)
#Planning Part 3: Grip
x = eef.attach_object("part", "robotiq_arg2f_base_link")
if (x):
place_pose = PoseStamped()
place_pose.header.frame_id = robot.get_planning_frame()
place_pose.pose.position.x = 0.324157
place_pose.pose.position.y = 0.354343
place_pose.pose.position.z = 0.575525
place_pose.pose.orientation.x = 0.00014
place_pose.pose.orientation.y = 0.884293
place_pose.pose.orientation.z = 0.466933
arm.set_pose_target(place_pose)
pl = arm.plan()
state = arm.execute(pl)
print state
if (state):
scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
i = 1
while i <= 10:
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
rospy.sleep(1)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
# remove part
scene.remove_world_object(PICK_OBJECT)
scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT)
rospy.sleep(2)
i += 1
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 'adding scene objects'
scene.add_mesh("bowl", p, "8inhemi.STL")
scene.add_mesh("punch", p1, "punch.STL")
scene.add_mesh("glovebox", p2, "GloveBox.stl")
print 'attaching bowl...'
robot.attach_object("bowl")
rospy.sleep(2)
currentbowlpose = p;
gripper.set_named_target("pinch")
gripper.go()
robotstart = robot.get_current_pose()
print 'start eef pose:'
print robotstart
robotstart_quat = [robotstart.pose.orientation.x,robotstart.pose.orientation.y,robotstart.pose.orientation.z,robotstart.pose.orientation.w]
M_eef = tf.transformations.quaternion_matrix(robotstart_quat)
currentbowlpose = p
#calculate position offset from bowl to eef frames
xoffset = robotstart.pose.position.x-p.pose.position.x
class Grasp(object):
def __init__(self):
# shadow hand
self.grasp_name = ""
self.joints_and_positions = {}
# smart grasp
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_ball_pose = rospy.ServiceProxy("/gazebo/get_model_state",
GetModelState)
self.__visualisation = rospy.Publisher("~display",
Marker,
queue_size=1,
latch=True)
self.__arm_commander = MoveGroupCommander("arm")
self.__hand_commander = MoveGroupCommander("hand")
self.pick()
def display_grasp(self):
print self.grasp_name
print self.joints_and_positions
def convert_to_xml(self):
grasp = ' <grasp name="' + self.grasp_name + '">' + '\n'
for key, value in self.joints_and_positions.items():
grasp = grasp + ' <joint name="' + \
str(key) + '">' + str(value) + '</joint>\n'
grasp = grasp + ' </grasp>' + '\n'
return grasp
def pick(self):
self.go_to_start()
arm_target = self.compute_arm_target()
self.pre_grasp(arm_target)
self.grasp(arm_target)
self.lift(arm_target)
def compute_arm_target(self):
ball_pose = self.__get_ball_pose.call("cricket_ball", "world")
# come at it from the top
arm_target = ball_pose.pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi / 2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
target_marker = Marker()
target_marker.action = Marker.MODIFY
target_marker.lifetime.from_sec(10.0)
target_marker.type = Marker.ARROW
target_marker.header.stamp = rospy.Time.now()
target_marker.header.frame_id = "world"
target_marker.pose = arm_target
target_marker.scale.z = 0.01
target_marker.scale.x = 0.1
target_marker.scale.y = 0.01
target_marker.color.r = 1.0
target_marker.color.g = 0.8
target_marker.color.b = 0.1
target_marker.color.a = 1.0
self.__visualisation.publish(target_marker)
return arm_target
def pre_grasp(self, arm_target):
self.__hand_commander.set_named_target("open")
plan = self.__hand_commander.plan()
self.__hand_commander.execute(plan, wait=True)
for _ in range(10):
self.__arm_commander.set_start_state_to_current_state()
self.__arm_commander.set_pose_targets([arm_target])
plan = self.__arm_commander.plan()
if self.__arm_commander.execute(plan):
return True
def grasp(self, arm_target):
waypoints = []
waypoints.append(
self.__arm_commander.get_current_pose(
self.__arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.__arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.__arm_commander.compute_cartesian_path(
waypoints, 0.01, 0.0)
print fraction
if not self.__arm_commander.execute(plan):
return False
self.__hand_commander.set_named_target("close")
plan = self.__hand_commander.plan()
if not self.__hand_commander.execute(plan, wait=True):
return False
self.__hand_commander.attach_object("cricket_ball__link")
def lift(self, arm_target):
waypoints = []
waypoints.append(
self.__arm_commander.get_current_pose(
self.__arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.__arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.__arm_commander.compute_cartesian_path(
waypoints, 0.01, 0.0)
print fraction
if not self.__arm_commander.execute(plan):
return False
def go_to_start(self):
self.__arm_commander.set_named_target("start")
plan = self.__arm_commander.plan()
if not self.__arm_commander.execute(plan, wait=True):
return False
self.__hand_commander.set_named_target("open")
plan = self.__hand_commander.plan()
self.__hand_commander.execute(plan, wait=True)
self.__reset_world.call()
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)
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 SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState,
self.__joint_state_cb, queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
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)
def simple_function():
rc = RobotCommander()
mgc = MoveGroupCommander("manipulator")
# print(rc.get_group_names())
# print(rc.get_group('manipulator'))
# exit()
eef_step = 0.01
jump_threshold = 2
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rc.get_current_state()
rospy.logwarn(rc.get_current_state())
sss.wait_for_input()
#rate = rospy.Rate(0.1) # 10hz
rate = rospy.Rate(1) # 10hz
rospy.sleep(1)
theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
while not rospy.is_shutdown():
approach_pose = PoseStamped()
approach_pose.header.frame_id = "table"
approach_pose.header.stamp = rospy.Time(0)
approach_pose.pose.position.x = 0.146
approach_pose.pose.position.y = 0.622
approach_pose.pose.position.z = 0.01
quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
approach_pose.pose.orientation.x = quat[0]
approach_pose.pose.orientation.y = quat[1]
approach_pose.pose.orientation.z = quat[2]
approach_pose.pose.orientation.w = quat[3]
# mgc.set_start_state_to_current_state()
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed before adding collision objects")
# else:
# rospy.logwarn("Planning succeeded before adding collision objects")
#
# rospy.logwarn("waiting for input to add dummy box")
# sss.wait_for_input()
#
box_dummy_pose = PoseStamped()
box_dummy_pose.header.frame_id = "table"
box_dummy_pose.pose.position.x = 0.147
box_dummy_pose.pose.position.y = 0.636
box_dummy_pose.pose.position.z = 0
psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC
# rospy.logwarn("I have added the box")
# rospy.sleep(1)
# rospy.logwarn("waiting for input to try planning with dummy box")
# sss.wait_for_input()
#
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
# else:
# rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#
rospy.logwarn("waiting for input to add end effector box box")
sss.wait_for_input()
# end effector collision object
link_attached_to_ef = "ee_link"
mb_ef_collisonobj = "metal_bracket"
mb_ef_pose = PoseStamped()
mb_ef_pose.header.frame_id = link_attached_to_ef
mb_ef_pose.pose.position.x = 0.065/2.0
mb_ef_pose.pose.position.y = 0.0
mb_ef_pose.pose.position.z = 0.0
mb_ef_size = (0.065,0.06,0.06)
psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
raw_input("0 hi")
mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
rospy.logwarn("I have added the attached box to the end effector")
rospy.sleep(1)
raw_input("1 hi")
rospy.logwarn(rc.get_current_state())
# mgc.attach_object(object_name, link_name, touch_links)
mgc.set_start_state_to_current_state()
rospy.logwarn(rc.get_current_state())
raw_input("2 hi")
rospy.logwarn("waiting for input to try planning with end effector box")
sss.wait_for_input()
(traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
if(frac_approach != 1):
rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
else:
rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
rospy.logwarn("waiting for input to try planning next loop")
sss.wait_for_input()
rate.sleep()
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)
