def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(60)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('right_arm_up')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(5)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
target_x = 0.135
#target_y = -0.32
target_y = -0.285290879999
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target_x
target_pose.pose.position.y = target_y
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = 0
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
p = PoseStamped()
p.header.frame_id = "up1_footprint"
p.pose.position.x = 0.12792118579
p.pose.position.y = -0.285290879999
p.pose.position.z = 0.120301181892
p.pose.orientation.x = 0.0
p.pose.orientation.y = 0.0
p.pose.orientation.z = -0.706825181105
p.pose.orientation.w = 0.707388269167
right_gripper.set_pose_target(p.pose)
# pick an object
right_arm.allow_replanning(True)
right_arm.allow_looking(True)
right_arm.set_goal_tolerance(0.05)
right_arm.set_planning_time(60)
print "arm grasp"
success = 0
attempt = 0
while not success:
p_plan = right_arm.plan()
attempt = attempt + 1
print "Planning attempt: " + str(attempt)
if p_plan.joint_trajectory.points != []:
success = 1
print "arm grasp"
right_arm.execute(p_plan)
rospy.sleep(5)
right_gripper.set_joint_value_target(GRIPPER_GRASP)
right_gripper.go()
print "gripper closed"
rospy.sleep(5)
scene.attach_box(GRIPPER_FRAME, target_id)
print "object attached"
right_arm.set_named_target('right_arm_up')
right_arm.go()
print "arm up"
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
class Jaco_rapper():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.jaco_arm = MoveGroupCommander("Arm")
self.hand = MoveGroupCommander("Hand")
#self.pose_pub = rospy.Publisher("hand_pose", PoseStamped,queue_size = 100)
self.pick_command = rospy.Publisher("pick_command",
Bool,
queue_size=100)
rospy.Subscriber("pick_pose", PoseStamped, self.pick)
self.jaco_arm.allow_replanning(True)
# Set the right arm reference frame
self.jaco_arm.set_pose_reference_frame(REFERENCE_FRAME)
self.jaco_arm.set_planning_time(5)
self.jaco_arm.set_goal_tolerance(0.02)
self.jaco_arm.set_goal_orientation_tolerance(0.1)
#self.pick_command.publish(True)
def test(self):
#self.hand.set_joint_value_target([0, 0, 0, 0])
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = 'arm_stand'
grasp_pose.pose.position.x = 0
grasp_pose.pose.position.y = 0.24
grasp_pose.pose.position.z = -0.4
grasp_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
# self.hand.set_joint_value_target([0, 0.012 ,0.012 ,0.012])
while (True):
self.jaco_arm.set_pose_target(
grasp_pose) # move to the top of the target
self.jaco_arm.go()
rospy.sleep(0.2)
#result = self.jaco_arm.go()
def pick(self, p):
target_id = 'target'
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Remove leftover objects from a previous run
self.scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Open the gripper to the neutral position
self.hand.set_joint_value_target([0.2, 0.2, 0.2, 0.2])
self.hand.go()
rospy.sleep(1)
target_size = [0.01, 0.01, 0.01]
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = p.pose.position.x - 0.015
#p.pose.position.x - 0.015
target_pose.pose.position.y = 0.05
target_pose.pose.position.z = p.pose.position.z
target_pose.pose.orientation.w = 0
print "Arm is catching {} object at ({}, {}, {})".format(
p.header.frame_id, p.pose.position.x, p.pose.position.y,
p.pose.position.z)
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
# Initialize the grasp pose to the target pose
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = REFERENCE_FRAME
grasp_pose.pose = target_pose.pose
grasp_pose.pose.position.y = 0.24
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
#self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = self.jaco_arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(p.header.frame_id)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = self.jaco_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(0.2)
self.scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, target_id)
self.pick_command.publish(Bool(True))
return
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
# Generate a gripper translation in the direction given by vector
def make_gripper_translation(self, min_dist, desired, vector):
# Initialize the gripper translation object
g = GripperTranslation()
# Set the direction vector components to the input
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
# The vector is relative to the gripper frame
g.direction.header.frame_id = 'arm_stand'
# Assign the min and desired distances from the input
g.min_distance = min_dist
g.desired_distance = desired
return g
# Generate a list of possible grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(
0.05, 0.15, [0.0, -1.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(
0.05, 0.1, [0.0, 1.0, 0.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
# Yaw angles to try
yaw_vals = [0]
# A list to hold the grasps
g.grasp_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(yaw_vals))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
grasps = [g]
# Return the list
return grasps
# Generate a list of possible place poses
def make_places(self, color):
# Initialize the place location as a PoseStamped message
x = 0
z = 0
if (color == 'red'):
x = 0.4
z = -0.4
elif (color == 'blue'):
x = 0.4
z = -0.25
else:
x = -0.3
z = -0.4
place_pose = PoseStamped()
place_pose.pose.position.x = x
place_pose.pose.position.y = 0.2
place_pose.pose.position.z = z
# Start with the input place pose
place_pose.header.frame_id = REFERENCE_FRAME
# A list to hold the places
places = []
# Create a quaternion from the Euler angles
place_pose.pose.orientation = Quaternion(0, 0, 0, 0)
# Append this place pose to the list
places.append(deepcopy(place_pose))
# Return the list
return places
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_tolerance(0.00001)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = 0.223606797749979
target_pose.pose.orientation.z = 0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
# Set the start state and target pose, then plan and execute
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.absolute_x_axis_tolerance = 0.00001
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 0.00001
orientation_constraint.weight = 1.0
orientation_constraint.orientation = start_pose.pose.orientation
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = deepcopy(start_pose)
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = -0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = -0.223606797749979
target_pose.pose.orientation.z = -0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
#target_pose.pose.orientation.x = 0.707107
#target_pose.pose.orientation.y = 0
#target_pose.pose.orientation.z = 0
#target_pose.pose.orientation.w = 0.707107
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('up')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def getOrientation(self, xyz):
pass
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_tolerance(0.00001)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('right')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = Pose()
start_pose.orientation.x = 0
start_pose.orientation.y = 0
start_pose.orientation.z = 0
start_pose.orientation.w = 1
#start_pose = right_arm.get_current_pose(end_effector_link).pose
start_pose.position.x = 0.4
start_pose.position.y = 0
start_pose.position.z = 0.4
right_arm.set_pose_target(start_pose)
right_arm.go()
# Initialize the waypoints list
waypoints = []
# Append the pose to the waypoints list
waypoints.append(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose = deepcopy(start_pose)
wpose.position.x += 0.15
waypoints.append(deepcopy(wpose))
wpose.position.z += 0.15
waypoints.append(deepcopy(wpose))
waypoints.append(deepcopy(start_pose))
print waypoints
fraction = 0.0
maxtries = 50
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
right_arm.set_named_target('up')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
grasp_height = 0.1
drop_height = 0.2
moving = False
paused = False
move_group_state = "IDLE"
def __init__(self, num_planning_attempts=20):
rospy.init_node("daarm_server", anonymous=True)
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=20):
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_tolerance(0.2)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = ["table", "tui", "monitor", "overhead", "wall"]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.3556, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8636)
self.wallPose.pose.position = Point(-0.508, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
def init_params(self):
try:
self.grasp_height = get_ros_param("GRASP_HEIGHT", "Grasp height defaulting to 0.1")
self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.2")
except ValueError as e:
rospy.loginfo(e)
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer("calibrate_arm", CalibrateAction, self.calibrate)
self.move_block_server = actionlib.SimpleActionServer("move_block", MoveBlockAction, self.handle_move_block)
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
def init_service_clients(self):
self.is_simulation = None
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm_kinova()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
self.home_arm_client()
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
self.arm.set_named_target("Home")
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct/100.0)*finger_max_turn)
goal.fingers.finger2 = float((finger2_pct/100.0)*finger_max_turn)
goal.fingers.finger3 = float((finger3_pct/100.0)*finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
self.move_fingers(50, 50, 50)
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
self.gripper.set_named_target("Close")
self.gripper.go()
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def move_arm_to_pose(self, position, orientation, delay=0, waypoints=[], corrections=1, action_server=None):
if len(waypoints) > 0:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
self.arm.set_pose_target(p)
plan = self.arm.plan()
if plan:
# get the last pose to correct if desired
ptPos = plan.joint_trajectory.points[-1].positions
# print "=================================="
# print "Last point of the current trajectory: "
angle_set = list()
for i in range(len(ptPos)):
tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
angle_set.append(tempPos)
if action_server:
action_server.publish_feedback(CalibrateFeedback("Plan Found"))
self.arm.execute(plan, wait=False)
while self.move_group_state is not "IDLE":
rospy.sleep(0.001)
print(self.move_group_state)
if self.paused is True:
self.arm.stop()
return
rospy.loginfo("LEAVING THE WHILE LOOP")
print("LEAVING THE LOOOOOOOOOP!!!!")
if corrections > 0:
rospy.loginfo("Correcting the pose")
self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def get_grasp_orientation(self, position):
return Quaternion(1, 0, 0, 0)
def update_joints(self, joints):
self.joint_angles = [joints.joint1, joints.joint2, joints.joint3,
joints.joint4, joints.joint5, joints.joint6, joints.joint7]
def move_z(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
self.open_gripper()
position = Point(location.x, location.y, self.grasp_height)
orientation = self.get_grasp_orientation(position)
try:
self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server)
except Exception as e:
raise(e)
if action_server:
action_server.publish_feedback()
if check_grasp is True:
pass # for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.close_gripper()
self.move_z(0.1)
rospy.sleep(delay)
def place_block(self, location, check_grasp=False, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
position = Point(location.x, location.y, self.drop_height)
orientation = self.get_grasp_orientation(position)
try:
self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server)
except Exception as e:
raise(e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z(0.1)
self.close_gripper()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
# self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
self.calibration_server.publish_feedback(CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(CalibrateFeedback("moving to drop"))
self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(0, 1, 0, 0))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': 0.4, 'id': 0}
calibration_action_belief = {"action": "add", "block": calibration_block}
self.action_belief_publisher.publish(String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise(ValueError("Failed getting block state to calibrate: "+str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise(ValueError("Failed calibration, either couldn't find block or > 1 block on TUI!"))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds)
arm_x_min = end_effector_x - x_ratio*arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio*arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
class PickAndPlace:
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def make_gripper_translation(self, min_dist, desired, vector):
g = GripperTranslation()
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
g.direction.header.frame_id = "panda_link0"
g.min_distance = min_dist
g.desired_distance = desired
return g
def open_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.04, 0.04]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def close_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.0, 0.0]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def clear_all_object(self):
for world_object in (self.scene.get_known_object_names()):
self.scene.remove_world_object(world_object)
for attached_object in (self.scene.get_attached_objects()):
self.scene.remove_attached_object(attached_object)
def add_object_box(self, object_name, pose, color, frame, size):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param size: size of object(mm)
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_box(name=object_name,
pose=object_pose,
size=(size[0] / 1000.0, size[1] / 1000.0,
size[2] / 1000.0))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
class Arm():
def __init__(self):
self.p = Pose()
self.gripper = MoveGroupCommander("onine_gripper")
self.arm = MoveGroupCommander("onine_arm")
self.arm.set_goal_tolerance(0.004)
self.arm.allow_replanning(True)
# self.arm.set_goal_position_tolerance(0.005)
# self.arm.set_goal_orientation_tolerance(0.1)
self.arm.set_num_planning_attempts(10)
self.arm.set_planning_time(5)
self.arm.set_planner_id("RRTkConfigDefault")
def go(self, x, y, z, roll, pitch, yaw):
self.p.position.x = x
self.p.position.y = y
self.p.position.z = z
self.p.orientation = Quaternion(
*quaternion_from_euler(roll, pitch, yaw))
self.arm.set_pose_target(self.p)
os.system("rosservice call clear_octomap")
rospy.loginfo("Moving to arm target")
self.arm.go(wait=True)
rospy.sleep(1)
def get_valid_pose(self, x, y, z, distance):
origin_translation = [0.095, 0.00, 0.00]
delta_x = x - origin_translation[0]
delta_y = y - origin_translation[1]
theta = math.atan(delta_y / delta_x)
grasp_yaw = theta
grasp_x = x + (distance * math.cos(theta))
grasp_y = y + (distance * math.sin(theta))
return (grasp_x, grasp_y, z, grasp_yaw)
def ready(self):
self.arm.set_named_target("onine_ready")
self.arm.go(wait=True)
def home(self):
self.arm.set_named_target("onine_home")
self.arm.go(wait=True)
def feed_pos(self):
self.arm.set_named_target("feed_pos")
self.arm.go(wait=True)
def tilt_food(self):
self.arm.set_named_target("tilt_food")
self.arm.go(wait=True)
def open_gripper(self):
os.system("rostopic pub /onine_gripper std_msgs/Float64 0.085 -1")
os.system("rosservice call clear_octomap")
def close_gripper(self):
os.system("rostopic pub /onine_gripper std_msgs/Float64 0.035 -1")
os.system("rosservice call clear_octomap")
def pickup_sim(self, x, y, z):
# self.ready()
self.open_gripper()
(aim_x, aim_y, aim_z,
aim_yaw) = self.get_valid_pose(x, y, z + 0.15, 0.00)
self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)
(aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z, 0.00)
self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)
self.close_gripper()
class Jaco_rapper():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.jaco_arm = MoveGroupCommander("Arm")
self.hand = MoveGroupCommander("Hand")
self.pose_pub = rospy.Publisher("hand_pose",
PoseStamped,
queue_size=100)
self.pick_command = rospy.Publisher("pick_command",
Bool,
queue_size=100)
rospy.Subscriber("pick_pose", PoseStamped, self.pick)
self.jaco_arm.allow_replanning(True)
self.jaco_arm.set_planning_time(5)
self.jaco_arm.set_goal_tolerance(0.02)
self.jaco_arm.set_goal_orientation_tolerance(0.1)
self.place_pose = PoseStamped()
self.place_pose.header.frame_id = 'arm_stand'
self.place_pose.pose.position.x = 0.4
self.place_pose.pose.position.y = 0.4
self.place_pose.pose.position.z = -0.4
self.place_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
self.orient = [
2.042967990797618, -0.03399658412747265, 1.5807131823846676
]
self.result = False
#self.pick_command.publish(True)
def test(self):
#self.hand.set_joint_value_target([0, 0, 0, 0])
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = 'arm_stand'
grasp_pose.pose.position.x = 0
grasp_pose.pose.position.y = 0.24
grasp_pose.pose.position.z = -0.4
grasp_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
# self.hand.set_joint_value_target([0, 0.012 ,0.012 ,0.012])
while (True):
self.jaco_arm.set_pose_target(
grasp_pose) # move to the top of the target
self.jaco_arm.go()
rospy.sleep(0.2)
#result = self.jaco_arm.go()
def pick(self, p):
#self.pick_command.publish(Bool(False))
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = 'arm_stand'
grasp_pose.pose.position.x = p.pose.position.x - 0.015
grasp_pose.pose.position.y = 0.5
grasp_pose.pose.position.z = p.pose.position.z
print "Arm is catching {} object at ({}, {}, {})".format(
p.header.frame_id, p.pose.position.x, p.pose.position.y,
p.pose.position.z)
#self.orient[0] += 0.5
# self.place_pose.pose.orientation =\
#o = tf.transformations.quaternion_from_euler(self.orient[0], self.orient[1], self.orient[2])
grasp_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
#print(tf.transformations.euler_from_quaternion((0.606301648371, 0.599731279995, 0.381153346104, 0.356991358063)))
#grasp_pose.pose.orientation = Quaternion(o[0], o[1], o[2], o[3])
self.hand.set_joint_value_target([0, 0, 0, 0]) #open the hand
self.hand.go()
rospy.sleep(0.2)
self.jaco_arm.set_pose_target(
grasp_pose) #move to the top of the target
result = self.jaco_arm.go()
rospy.sleep(0.2)
grasp_pose.pose.position.y = 0.24 #lower the arm to grasp
self.pose_pub.publish(grasp_pose)
self.jaco_arm.set_pose_target(grasp_pose)
print(result)
result = result and self.jaco_arm.go()
print(result)
if (result != True):
self.pick_command.publish(Bool(True))
return
rospy.sleep(0.2)
self.hand.set_joint_value_target([0, 0.012, 0.012, 0.012])
self.hand.go()
rospy.sleep(0.2)
grasp_pose.pose.position.y = 0.5
self.jaco_arm.set_pose_target(grasp_pose)
self.jaco_arm.go()
rospy.sleep(0.2)
self.place()
self.pick_command.publish(Bool(True))
def place(self):
self.jaco_arm.set_pose_target(self.place_pose)
self.jaco_arm.go()
rospy.sleep(0.2)
self.place_pose.pose.position.y = 0.3
self.jaco_arm.set_pose_target(self.place_pose)
self.jaco_arm.go()
rospy.sleep(0.2)
self.hand.set_joint_value_target([0, 0, 0, 0])
self.place_pose.pose.position.y = 0.5
self.jaco_arm.set_pose_target(self.place_pose)
self.jaco_arm.go()
rospy.sleep(0.2)
