class Pick_Place:
def __init__(self):
self.object_list = {}
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.05)
self.gripper.set_goal_tolerance(0.02)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.add_objects()
self.add_table()
#self.add_ground()
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
rospy.sleep(1.0)
# pick up object in pose
def pickup(self, object_name, pose):
grasps = self.generate_grasps(object_name, pose)
self.arm.pick(object_name, grasps)
#self.gripper.stop()
rospy.loginfo('Pick up successfully')
self.arm.detach_object(object_name)
self.clean_scene(object_name)
#rospy.sleep(1)
# place object to goal pose
def place(self, pose):
self.move_pose_arm(pose)
rospy.sleep(1)
# pose.position.z -= 0.1
# self.move_pose_arm(pose)
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z -= 0.15
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.move_joint_hand(0)
rospy.sleep(1)
# pose.position.z += 0.1
# self.move_pose_arm(pose)
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
rospy.loginfo('Place successfully')
def get_object_pose(self, object_name):
pose = self.object_list[object_name].pose
return pose
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def add_ground(self):
self.clean_scene("ground")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
size = (5, 5, 0.02)
#self.scene.add_box("ground",p, size)
rospy.sleep(2)
def add_table(self):
self.clean_scene("table")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.8
p.pose.position.y = 0.0
p.pose.position.z = 0.1
size = (0.8, 1.5, 0.028)
self.scene.add_box("table", p, size)
rospy.sleep(2)
def add_objects(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add box
name = "box"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.025 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.05, 0.05, 0.05)
self.scene.add_box(name, p, size)
height = 0.05
width = 0.05
shape = "cube"
color = "red"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add box")
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add cylinder
name = "cylinder"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = 0.2
p.pose.position.z = 0.05 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
height = 0.1
radius = 0.03
self.scene.add_cylinder(name, p, height, radius)
height = 0.1
width = 0.03
shape = "cylinder"
color = "green"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add cylinder")
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add sphere
name = "ball"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = -0.2
p.pose.position.z = 0.03 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
radius = 0.03
self.scene.add_sphere(name, p, radius)
height = 0.03
width = 0.03
shape = "sphere"
color = "red"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add ball")
rospy.sleep(1)
#print(self.object_list)
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def back_to_home(self):
self.move_joint_arm(0, 0, 0, 0, 0, 0)
self.move_joint_hand(0)
rospy.sleep(1)
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
# Inverse Kinematics: Move the robot arm to desired pose
def move_pose_arm(self, pose_goal):
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.5
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.5
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
grasp.max_contact_force = 1000
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.0)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.4)
elif name == "ball":
traj.positions.append(0.3)
elif name == "cylinder":
traj.positions.append(0.3)
#traj.velocities.append(0.2)
#traj.effort.append(100)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
# Implement the main algorithm here
def MyAlgorithm(self):
self.back_to_home()
# pick cylinder
object_name = "cylinder"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.16
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = 0
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
# pick box
object_name = "box"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.15
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = 0.1
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
# pick ball
object_name = "ball"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.14
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = -0.1
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
class Pick_Place:
def __init__(self):
self.objects_name = ["cylinder", "box", "sphere"]
self.object_width = 0.03
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.2)
self.gripper.set_goal_tolerance(0.05)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.add_ground()
rospy.sleep(1.0)
for i in range(3):
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 1.57, 0, 0.4, 0, 0.6)
rospy.sleep(1)
self.object_name = self.objects_name[i]
self.scene.remove_world_object(self.object_name)
self.pose_object, dx, dy, dz = self.add_object(self.object_name)
rospy.sleep(1.0)
self.pose_object.position.x += dx
self.pose_object.position.y += dy
self.pose_object.position.z += dz
print(self.objects_name[i], self.pose_object.position)
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
print("start pick and place")
# Pick
grasps = self.generate_grasps(self.object_name, self.pose_object)
self.arm.pick(self.object_name, grasps)
self.gripper.stop()
rospy.loginfo('Pick up successfully')
self.arm.detach_object(self.object_name)
self.clean_scene(self.object_name)
rospy.sleep(1)
curr_pose = self.arm.get_current_pose().pose
x = curr_pose.position.x
y = curr_pose.position.y
z = curr_pose.position.z
# quaternion = (curr_pose.orientation.x,
# curr_pose.orientation.y,
# curr_pose.orientation.z,
# curr_pose.orientation.w)
# euler = euler_from_quaternion(quaternion)
# roll = euler[0]
# pitch = euler[1]
# yaw = euler[2]
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
z += 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
x = -curr_pose.position.y
y = 0.6
z -= 0.1
#z = self.pose_object.position.z+0.05
# if self.object_name == "cylinder":
# yaw = numpy.deg2rad(90.0)
#z+= 0.001
self.move_pose_arm(roll, pitch, yaw, x, y, z)
rospy.sleep(0.5)
z -= 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
self.move_joint_hand(0)
#if self.object_name == "cylinder":
z += 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
# target_pose = curr_pose
# target_pose.position.x = x
# target_pose.position.y = y
# target_pose.position.z = z
# q = quaternion_from_euler(roll, pitch, yaw)
# target_pose.orientation = Quaternion(*q)
# place = self.generate_places(target_pose)
# self.arm.place(self.object_name, place)
# self.arm.detach_object(self.object_name)
# self.clean_scene(self.object_name)
rospy.loginfo('Place successfully')
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6)
rospy.sleep(1)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def add_ground(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
size = (5, 5, 0.02)
self.scene.add_box("ground", p, size)
def add_object(self, name):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
dx = 0
dy = 0
dz = 0
if name == "box":
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.015 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.03, 0.03, 0.03)
self.scene.add_box(name, p, size)
dz = 0.16
elif name == "cylinder":
p.pose.position.x = 0.5
p.pose.position.y = 0.2
p.pose.position.z = 0.05 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
height = 0.1
radius = 0.03
self.scene.add_cylinder(name, p, height, radius)
dz = 0.16
#dx = -0.135
elif name == "sphere":
p.pose.position.x = 0.5
p.pose.position.y = -0.2
p.pose.position.z = 0.03 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
radius = 0.03
self.scene.add_sphere(name, p, radius)
dz = 0.15
return p.pose, dx, dy, dz
def move_pose_arm(self, roll, pitch, yaw, x, y, z):
pose_goal = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose_goal.orientation.x = quat[0]
pose_goal.orientation.y = quat[1]
pose_goal.orientation.z = quat[2]
pose_goal.orientation.w = quat[3]
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
#for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)):
# Create place location:
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.2
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.2
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
# if name != "cylinder":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# else:
# #grasp.pre_grasp_approach.direction.vector.z = -0.05
# grasp.pre_grasp_approach.direction.vector.x = 0.1
# #grasp.post_grasp_retreat.direction.vector.z = 0.05
# grasp.post_grasp_retreat.direction.vector.x = -0.1
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.03)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.57)
elif name == "sphere":
traj.positions.append(0.3)
else:
traj.positions.append(0.3)
#traj.velocities.append(0.2)
traj.effort.append(800)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
def generate_places(self, target):
#places = []
now = rospy.Time.now()
# for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self.robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
# q = quaternion_from_euler(0.0, 0, 0.0)
# place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self.approach_retreat_desired_dist
place.pre_place_approach.min_distance = self.approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist
place.post_place_retreat.min_distance = self.approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
place.allowed_touch_objects.append(self.object_name)
place.post_place_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0)
#traj.effort.append(0)
traj.time_from_start = rospy.Duration.from_sec(1.0)
place.post_place_posture.points.append(traj)
# Add place:
#places.append(place)
return place
class ModelManager:
def __init__(self):
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model",
SpawnModel)
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model",
DeleteModel)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.object_list = {}
def pose2msg(self, x, y, z, roll, pitch, yaw):
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def spawn_model(self, model_name, model_pose):
with open(
os.path.join(
rospkg.RosPack().get_path('irb120_robotiq85_gazebo'),
'models', model_name, 'model.sdf'), "r") as f:
model_xml = f.read()
self.spawn_model_srv(model_name, model_xml, "", model_pose, "world")
def delete_model(self, model_name):
self.delete_model_srv(model_name)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def respawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
this_object = self.object_list[object_name]
print("Respawning {}".format(object_name))
# remove old objects in Gazebo
self.delete_model(object_name)
# respawn new objects in Gazebo
roll, pitch, yaw, x, y, z = self.msg2pose(this_object.abs_pose)
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
# respawn objects in Rviz
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(object_name)
p.pose = copy.copy(this_object.relative_pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
rospy.loginfo("All objects are respawned")
def spawn_all_model(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
robot_roll = objects_info["robot"]["pose"]["roll"]
robot_pitch = objects_info["robot"]["pose"]["pitch"]
robot_yaw = objects_info["robot"]["pose"]["yaw"]
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
# add object in Gazebo
# self.delete_model(object_name)
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
## add object in planning scene(Rviz)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = x - robot_x
p.pose.position.y = y - robot_y
p.pose.position.z = z - robot_z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
size = (x, y, z)
self.scene.add_box(name, p, size)
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, object_pose,
height, width, length,
shape, color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.scene.add_cylinder(name, p, height, radius)
self.object_list[name] = Object(p.pose, object_pose,
height, radius * 2,
radius * 2, shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.scene.add_sphere(name, p, radius)
self.object_list[name] = Object(p.pose, object_pose,
radius * 2, radius * 2,
radius * 2, shape, color)
rospy.sleep(0.5)
rospy.loginfo("Spawning Obstacles in planning scene")
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for obstacle_name in obstacles_name:
name = obstacle_name
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = obstacles[name]["pose"]["x"] - robot_x
p.pose.position.y = obstacles[name]["pose"]["y"] - robot_y
p.pose.position.z = obstacles[name]["pose"]["z"] - robot_z
q = quaternion_from_euler(robot_roll, robot_pitch, robot_yaw)
p.pose.orientation = Quaternion(*q)
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
size = (x, y, z)
self.scene.add_box(name, p, size)
rospy.sleep(0.5)
def init():
global marker_array_pub, marker_pub, tf_broadcaster, tf_listener
global move_group, turntable
global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir
rospy.init_node('acquisition')
camera_mesh = rospy.get_param('~camera_mesh', None)
camera_orientation = rospy.get_param('~camera_orientation', None)
camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0])
camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1])
min_distance = rospy.get_param('~min_distance', 0.2)
max_distance = rospy.get_param('~max_distance', min_distance)
max_velocity = rospy.get_param('~max_velocity', 0.1)
num_positions = rospy.get_param('~num_positions', 15)
num_spins = rospy.get_param('~num_spins', 8)
object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2]))
photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0])
photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0])
ptpip = rospy.get_param('~ptpip', None)
reach = rospy.get_param('~reach', 0.85)
simulation = '/gazebo' in get_node_names()
test = rospy.get_param('~test', True)
turntable_pos = rospy.get_param(
'~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05])
turntable_radius = rospy.get_param('~turntable_radius', 0.2)
wall_thickness = rospy.get_param('~wall_thickness', 0.04)
marker_array_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1,
latch=True)
marker_pub = rospy.Publisher('visualization_marker',
Marker,
queue_size=1,
latch=True)
tf_broadcaster = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
move_group = MoveGroupCommander('manipulator')
move_group.set_max_acceleration_scaling_factor(
1.0 if simulation else max_velocity)
move_group.set_max_velocity_scaling_factor(
1.0 if simulation else max_velocity)
robot = RobotCommander()
scene = PlanningSceneInterface(synchronous=True)
try:
st_control = load_source(
'st_control',
os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts',
'iai_scanning_table', 'st_control.py'))
turntable = st_control.ElmoUdp()
if turntable.check_device():
turntable.configure()
turntable.reset_encoder()
turntable.start_controller()
turntable.set_speed_deg(30)
except Exception as e:
print(e)
sys.exit('Could not connect to turntable.')
if simulation:
move_home()
elif not test:
working_dir = rospy.get_param('~working_dir', None)
if working_dir is None:
sys.exit('Working directory not specified.')
elif not camera.init(
os.path.join(os.path.dirname(os.path.abspath(__file__)), '..',
'out', working_dir, 'images'), ptpip):
sys.exit('Could not initialize camera.')
# add ground plane
ps = PoseStamped()
ps.header.frame_id = robot.get_planning_frame()
scene.add_plane('ground', ps)
# add photobox
ps.pose.position.x = photobox_pos[
0] + photobox_size[0] / 2 + wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_left', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[
0] - photobox_size[0] / 2 - wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_right', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[
1] - photobox_size[1] / 2 - wall_thickness / 2
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_back', ps,
(photobox_size[0], wall_thickness, photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] - wall_thickness / 2
scene.add_box('box_ground', ps,
(photobox_size[0], photobox_size[1], wall_thickness))
# add turntable
turntable_height = turntable_pos[2] - photobox_pos[2]
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = photobox_pos[2] + turntable_height / 2
scene.add_cylinder('turntable', ps, turntable_height, turntable_radius)
# add object on turntable
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = turntable_pos[2] + object_size[2] / 2
scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2)
# add cable mounts
scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount')
scene.remove_attached_object('forearm_link', 'forearm_cable_mount')
scene.remove_world_object('upper_arm_cable_mount')
scene.remove_world_object('forearm_cable_mount')
if ptpip is None and not simulation:
size = [0.08, 0.08, 0.08]
ps.header.frame_id = 'upper_arm_link'
ps.pose.position.x = -0.13
ps.pose.position.y = -0.095
ps.pose.position.z = 0.135
scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size)
ps.header.frame_id = 'forearm_link'
ps.pose.position.x = -0.275
ps.pose.position.y = -0.08
ps.pose.position.z = 0.02
scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size)
# add camera
eef_link = move_group.get_end_effector_link()
ps = PoseStamped()
ps.header.frame_id = eef_link
scene.remove_attached_object(eef_link, 'camera_0')
scene.remove_attached_object(eef_link, 'camera_1')
scene.remove_world_object('camera_0')
scene.remove_world_object('camera_1')
ps.pose.position.y = camera_pos[1]
ps.pose.position.z = camera_pos[2]
if camera_mesh:
ps.pose.position.x = camera_pos[0]
quaternion = tf.transformations.quaternion_from_euler(
math.radians(camera_orientation[0]),
math.radians(camera_orientation[1]),
math.radians(camera_orientation[2]))
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
scene.attach_mesh(eef_link, 'camera_0', ps,
os.path.expanduser(camera_mesh), camera_size)
if not simulation:
scene.attach_mesh(eef_link, 'camera_1', ps,
os.path.expanduser(camera_mesh),
numpy.array(camera_size) * 1.5)
vertices = scene.get_attached_objects(
['camera_0'])['camera_0'].object.meshes[0].vertices
camera_size[0] = max(vertice.x for vertice in vertices) - min(
vertice.x for vertice in vertices)
camera_size[1] = max(vertice.y for vertice in vertices) - min(
vertice.y for vertice in vertices)
camera_size[2] = max(vertice.z for vertice in vertices) - min(
vertice.z for vertice in vertices)
else:
ps.pose.position.x = camera_pos[0] + camera_size[0] / 2
scene.attach_box(eef_link, 'camera_0', ps, camera_size)
class Pick_Place:
def __init__(self):
# rospy.init_node('pick_and_place')
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
jointslimit = joints_setup["joints_limit"]
home_value = joints_setup["home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_home_value([j1, j2, j3, j4, j5, j6, g])
home_value = joints_setup["pick_place_home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_pick_place_home_value([j1, j2, j3, j4, j5, j6, g])
self.object_list = {}
self.obstacle_list = {}
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, p.pose, height,
width, length, shape,
color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.object_list[name] = Object(p.pose, p.pose, height,
radius * 2, radius * 2,
shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.object_list[name] = Object(p.pose, p.pose, radius * 2,
radius * 2, radius * 2,
shape, color)
objects = objects_info["objects"]
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for object_name in obstacles_name:
name = object_name
x = obstacles[name]["pose"]["x"]
y = obstacles[name]["pose"]["y"]
z = obstacles[name]["pose"]["z"]
roll = obstacles[name]["pose"]["roll"]
pitch = obstacles[name]["pose"]["pitch"]
yaw = obstacles[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.obstacle_list[name] = Obstacle(p.pose, p.pose, height,
width, length)
# self.object_list = object_list
self.goal_list = {}
self.set_target_info()
self.gripper_width = {}
self.set_gripper_width_relationship()
self.arm = moveit_commander.MoveGroupCommander("ur10_manipulator")
self.arm.set_goal_tolerance(0.01)
self.arm.set_pose_reference_frame("ur10_base_link")
self.gripperpub = rospy.Publisher("gripper_controller/command",
JointTrajectory,
queue_size=0)
self.transform_arm_to_baselink = Point()
self.get_arm_to_baselink()
self.gripper_length = 0.33
self.get_workspace()
self.message_pub = rospy.Publisher("/gui_message",
String,
queue_size=0)
self.updatepose_pub = rospy.Publisher("/updatepose",
Bool,
queue_size=0)
self.robot_pose = Pose()
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.robot_pose_callback)
def send_message(self, message):
msg = String()
msg.data = message
self.message_pub.publish(msg)
def updatepose_trigger(self, value):
msg = Bool()
msg.data = value
self.updatepose_pub.publish(msg)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def clean_all_objects_in_scene(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
self.clean_scene(object_name)
def spawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
self.spawn_object_rviz(object_name)
def spawn_object_rviz(self, object_name):
self.clean_scene(object_name)
this_object = self.object_list[object_name]
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose = copy.deepcopy(this_object.abs_pose)
robot_pose = self.get_robot_pose()
p.pose.position.x -= robot_pose.position.x
p.pose.position.y -= robot_pose.position.y
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = -euler[2]
quat = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
self.object_list[object_name].relative_pose = self.baselink2arm(p.pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
def spawn_obstacle_rviz(self, obstacle_name):
self.clean_scene(obstacle_name)
this_object = self.obstacle_list[obstacle_name]
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose = copy.deepcopy(this_object.abs_pose)
robot_pose = self.get_robot_pose()
# p.pose.position.x -= robot_pose.position.x
# p.pose.position.y -= robot_pose.position.y
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = -euler[2]
quat = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
x = p.pose.position.x - robot_pose.position.x
y = p.pose.position.y - robot_pose.position.y
p.pose.position.x = math.cos(yaw) * x - math.sin(yaw) * y
p.pose.position.y = math.sin(yaw) * x + math.cos(yaw) * y
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(obstacle_name, p, size)
rospy.sleep(0.5)
def set_target_info(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
targets = objects_info["targets"]
target_name = targets.keys()
for name in target_name:
position = Point()
position.x = targets[name]["x"] - robot_x
position.y = targets[name]["y"] - robot_y
position.z = targets[name]["z"] - robot_z
self.goal_list[name] = position
def set_gripper_width_relationship(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
gripper_joint_value = objects_info["gripper_joint_value"]
objects_width = gripper_joint_value.keys()
for object_width in objects_width:
self.gripper_width[object_width] = gripper_joint_value[
object_width]
def get_object_list(self):
return self.object_list.keys()
def get_target_list(self):
return self.goal_list.keys()
def get_object_pose(self, object_name):
return copy.deepcopy(self.object_list[object_name].relative_pose)
def get_object_info(self, object_name):
this_object = copy.deepcopy(self.object_list[object_name])
pose = this_object.relative_pose
height = this_object.height
width = this_object.width
length = this_object.length
shape = this_object.shape
color = this_object.color
return pose, height, width, length, shape, color
def get_target_position(self, target_name):
position = copy.deepcopy(self.goal_list[target_name])
# print("before transformation",position)
p = Pose()
p.position = position
robot_pose = self.get_robot_pose()
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
yaw = -euler[2]
x = position.x - robot_pose.position.x
y = position.y - robot_pose.position.y
position.x = math.cos(yaw) * x - math.sin(yaw) * y
position.y = math.sin(yaw) * x + math.cos(yaw) * y
# print("after transformation",position, yaw)
position = self.baselink2arm(p).position
return position
def robot_pose_callback(self, msg):
self.robot_pose.position = msg.pose.pose.position
self.robot_pose.orientation = msg.pose.pose.orientation
def get_robot_pose(self):
# try:
# listener = tf.TransformListener()
# (trans,rot) = listener.lookupTransform('/map', "/base_link", rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# rospy.loginfo("no transformation")
# return
# pose = Pose()
# pose.position.x = trans[0]
# pose.position.y = trans[1]
# pose.position.z = trans[2]
# pose.orientation.x = rot[0]
# pose.orientation.y = rot[1]
# pose.orientation.z = rot[2]
# pose.orientation.w = rot[3]
return self.robot_pose
def get_arm_to_baselink(self):
# try:
# listener = tf.TransformListener()
# (trans,rot) = listener.lookupTransform('/base_link', "/ur10_base_link", rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# rospy.loginfo("no transformation")
# return
# self.transform_arm_to_baselink.x = trans[0]
# self.transform_arm_to_baselink.y = trans[1]
# self.transform_arm_to_baselink.z = trans[2]
# print(self.transform_arm_to_baselink)
self.transform_arm_to_baselink.x = 0.205
self.transform_arm_to_baselink.y = 0
self.transform_arm_to_baselink.z = 0.802
def get_workspace(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
workspace = joints_setup["workspace"]
x = workspace["center"]["x"]
y = workspace["center"]["y"]
z = workspace["center"]["z"]
min_r = workspace["r"]["min"]
max_r = workspace["r"]["max"]
min_z = workspace["min_z"]
self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z)
# check if the position is inside workspace
def is_inside_workspace(self, x, y, z):
if z > self.workspace.min_z:
dx = x - self.workspace.x
dy = y - self.workspace.y
dz = z - self.workspace.z
r = math.sqrt(dx**2 + dy**2 + dz**2)
if self.workspace.min_r < r < self.workspace.max_r:
return True
return False
# move one joint of the arm to value
def set_arm_joint(self, joint_id, value):
joint_goal = self.arm.get_current_joint_values()
joint_goal[joint_id - 1] = value
self.arm.go(joint_goal, wait=True)
self.arm.stop()
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
def get_joint_value(self, joint_id):
joints = self.arm.get_current_joint_values()
return joints[joint_id - 1]
def get_joints_value(self):
joints = self.arm.get_current_joint_values()
return joints
def get_arm_pose(self):
pose = self.arm.get_current_pose().pose
pose = self.baselink2arm(pose)
pose = self.TCP2gripper(pose, self.gripper_length)
# print(pose)
return self.msg2pose(pose)
def set_random_pose(self):
self.arm.set_random_target()
def set_gripper_length(self, length):
self.gripper_length = length
def set_home_value(self, home_value):
self.home_value = home_value
def back_to_home(self, move_gripper=True):
j1, j2, j3, j4, j5, j6, g = self.home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
if move_gripper:
self.move_joint_hand(g)
def set_pick_place_home_value(self, home_value):
self.pick_place_home_value = home_value
def move_to_pick_place_home(self, move_gripper=True):
j1, j2, j3, j4, j5, j6, g = self.pick_place_home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
if move_gripper:
self.move_joint_hand(g)
def fold_robot_arm(self, ):
self.move_joint_arm(0, 0, 0, 0, 0, 0)
self.move_joint_hand(0)
def gripper2TCP(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([0, -length, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def TCP2gripper(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([0, length, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def arm2baselink(self, in_pose):
pose = copy.deepcopy(in_pose)
pose.position.x += self.transform_arm_to_baselink.x
pose.position.y += self.transform_arm_to_baselink.y
pose.position.z += self.transform_arm_to_baselink.z
return pose
def baselink2arm(self, in_pose):
pose = copy.deepcopy(in_pose)
pose.position.x -= self.transform_arm_to_baselink.x
pose.position.y -= self.transform_arm_to_baselink.y
pose.position.z -= self.transform_arm_to_baselink.z
return pose
# Inverse Kinematics (IK): move TCP to given position and orientation
def move_pose_arm(self, pose_goal):
# pose_goal = self.pose2msg(roll, pitch, yaw, x, y, z)
# pose_goal = self.gripper2TCP(pose_goal, self.gripper_length)
x = pose_goal.position.x
y = pose_goal.position.y
z = pose_goal.position.z
if not self.is_inside_workspace(x, y, z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
return False
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
return True
# Move gripper
def move_joint_hand(self, finger1_goal, finger2_goal=10, finger3_goal=10):
if finger2_goal == 10 and finger3_goal == 10:
finger2_goal, finger3_goal = finger1_goal, finger1_goal
jointtrajectory = JointTrajectory()
jointtrajectory.header.stamp = rospy.Time.now()
jointtrajectory.joint_names.extend(["H1_F1J3", "H1_F2J3", "H1_F3J3"])
joint = JointTrajectoryPoint()
joint.positions.extend([finger1_goal, finger2_goal, finger3_goal])
joint.time_from_start = rospy.Duration(1)
jointtrajectory.points.append(joint)
self.gripperpub.publish(jointtrajectory)
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def set_grasp_distance(self, min_distance, desired_distance):
self.approach_retreat_min_dist = min_distance
self.approach_retreat_desired_dist = desired_distance
def count_gripper_width(self, object_name):
object_width = self.object_list[object_name].width
if object_width in self.gripper_width:
return self.gripper_width[object_width]
else:
rospy.loginfo(
"Cannot find suitable gripper joint value for this object width"
)
return 0
# pick object to goal position
def pickup(self, object_name, position, width, distance=0.12):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180))
pose.orientation = Quaternion(*q)
# transform from gripper to TCP
pose = self.gripper2TCP(pose, self.gripper_length)
pose.position.z += distance
rospy.loginfo('Start picking')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
print(wpose)
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# pick
rospy.sleep(0.5)
self.move_joint_hand(width)
rospy.sleep(3)
# if object_name != "yellow_ball":
# self.arm.attach_object(object_name)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Pick finished')
# place object to goal position
def place(self, object_name, position, width=-0.2, distance=0.12):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180))
pose.orientation = Quaternion(*q)
# transform from gripper to TCP
pose = self.gripper2TCP(pose, self.gripper_length)
pose.position.z += distance
rospy.loginfo('Start placing')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
print(wpose)
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# place
self.move_joint_hand(width)
rospy.sleep(3)
# if object_name != "yellow_ball":
# self.arm.detach_object(object_name)
# self.clean_scene(object_name)
self.object_list.pop(object_name)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Place finished')
def __init__(self):
# Initialize move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize ROS node
rospy.init_node('moveit_obstacles_demo')
# Initialize scene
scene = PlanningSceneInterface()
# Create a publisher for scene change information
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to store object color
self.colors = dict()
# Wait for scene to be ready
rospy.sleep(1)
# Initialize arm_group which needs to be controlled by move_group
arm = MoveGroupCommander('arm')
# Get end link name
end_effector_link = arm.get_end_effector_link()
# Set tolerance for position (m) and orientation (rad)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning if failed
arm.allow_replanning(True)
# Set reference frame for target position
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# Set planning time limitation
arm.set_planning_time(5)
# Set scene object name
front_board_id = 'front_board'
back_board_id = 'back_board'
left_board_id = 'left_board'
right_board_id = "right_board"
bottle_id = 'bottle'
# Remove previous object
scene.remove_world_object(front_board_id)
scene.remove_world_object(back_board_id)
scene.remove_world_object(left_board_id)
scene.remove_world_object(right_board_id)
scene.remove_world_object(bottle_id)
rospy.sleep(1)
# Move arm to initial position
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
# Initialize gripper_group which needs to be controlled by move_group
gripper = moveit_commander.MoveGroupCommander('gripper')
# Set object dimension
front_board_size = [0.2, 0.25, 0.01]
back_board_size = [0.2, 0.25, 0.01]
left_board_size = [0.25, 0.14, 0.01]
right_board_size = [0.25, 0.14, 0.01]
bottle_height, bottle_radius = 0.06, 0.02
# Add object to scene
front_board_pose = PoseStamped()
front_board_pose.header.frame_id = reference_frame
front_board_pose.pose.position.x = 0
front_board_pose.pose.position.y = 0.1
front_board_pose.pose.position.z = 0.125
front_board_pose.pose.orientation.w = 0.707
front_board_pose.pose.orientation.x = 0.707
scene.add_box(front_board_id, front_board_pose, front_board_size)
back_board_pose = PoseStamped()
back_board_pose.header.frame_id = reference_frame
back_board_pose.pose.position.x = 0
back_board_pose.pose.position.y = 0.24
back_board_pose.pose.position.z = 0.125
back_board_pose.pose.orientation.w = 0.707
back_board_pose.pose.orientation.x = 0.707
scene.add_box(back_board_id, back_board_pose, back_board_size)
left_board_pose = PoseStamped()
left_board_pose.header.frame_id = reference_frame
left_board_pose.pose.position.x = 0.1
left_board_pose.pose.position.y = 0.17
left_board_pose.pose.position.z = 0.125
left_board_pose.pose.orientation.w = 0.707
left_board_pose.pose.orientation.y = 0.707
scene.add_box(left_board_id, left_board_pose, left_board_size)
right_board_pose = PoseStamped()
right_board_pose.header.frame_id = reference_frame
right_board_pose.pose.position.x = -0.1
right_board_pose.pose.position.y = 0.17
right_board_pose.pose.position.z = 0.125
right_board_pose.pose.orientation.w = 0.707
right_board_pose.pose.orientation.y = 0.707
scene.add_box(right_board_id, right_board_pose, right_board_size)
bottle_pose = PoseStamped()
bottle_pose.header.frame_id = reference_frame
bottle_pose.pose.position.x = 0.1
bottle_pose.pose.position.y = -0.2
bottle_pose.pose.position.z = 0.02
bottle_pose.pose.orientation.w = 0.707
bottle_pose.pose.orientation.y = 0.707
scene.add_cylinder(bottle_id, bottle_pose, bottle_height,
bottle_radius)
# Set object color
self.setColor(front_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(back_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(left_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(right_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(bottle_id, 0.6, 0.1, 0, 1.0)
# Publish color
self.sendColors()
rospy.sleep(3)
# Set target position for gripper and move gripper
gripper_position = (np.array([36, -36, 36, 36]) * np.pi /
180.0).tolist()
gripper.set_joint_value_target(gripper_position)
gripper.go()
rospy.sleep(5)
# Set target position for arm
joint_position = (np.array([28, -10, 50, -88, 58]) * np.pi /
180.0).tolist()
arm.set_joint_value_target(joint_position)
arm.go()
# Set target position for arm
joint_position = (np.array([28, -44, 30, -91, 58]) * np.pi /
180.0).tolist()
arm.set_joint_value_target(joint_position)
arm.go()
# Set target position for gripper and move gripper
gripper_position = (np.array([20, -20, 20, 20]) * np.pi /
180.0).tolist()
gripper.set_joint_value_target(gripper_position)
gripper.go()
rospy.sleep(10)
# Move arm to initial position
arm.set_named_target('home')
arm.go()
# Set target position for gripper and move gripper
gripper_position = (np.array([-33, 33, -33, -33]) * np.pi /
180.0).tolist()
gripper.set_joint_value_target(gripper_position)
gripper.go()
# Close and exit MoveIt
moveit_commander.roscpp_shutdown()
