class MotionPlayer(object):
def __init__(self):
rospy.loginfo("Initializing MotionPlayer...")
self.ac = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Connecting to /play_motion...")
# If more than 5s pass by, we crash
if not self.ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Could not connect to /tts action server!")
exit(0)
rospy.loginfo("Connected!")
def play_motion(self, motion_name, block=True):
# To check motions:
# rosparam list | grep play_motion | grep joints
# for example:
# open_hand, close_hand, offer_hand, wave
# shake_hands, thumb_up_hand, pointing_hand, pinch_hand
# head_tour, do_weights, pick_from_floor
g = PlayMotionGoal()
g.motion_name = motion_name
if block:
self.ac.send_goal_and_wait(g)
else:
self.ac.send_goal(g)
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation',
GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
class MoveBaseDancer(PathDancerBase):
def __init__(self):
super(MoveBaseDancer,self).__init__()
self.client = SimpleActionClient('move_base', MoveBaseAction)
pass
def run(self, points):
tf_listener = tf.TransformListener()
while True:
try:
now = rospy.Time.now()
tf_listener.waitForTransform('map','base_link', now, rospy.Duration(4.0))
t,r = tf_listener.lookupTransform('map','base_link', now)
break
except (tf.Exception) as e:
print e
self.x0, self.y0 = t[0], t[1]
self.client.wait_for_server()
for pt in points:
goal = self.make_goal(pt)
self.client.send_goal_and_wait(goal)
def make_goal(self, point):
x,y = point
x += self.x0 # w.r.t. initial point
y += self.y0
theta = np.arctan2(y,x)
angle = Quaternion(0, 0, np.sin(theta / 2), np.cos(theta / 2))
dest = PoseStamped(
header=Header(frame_id='map'),
pose=Pose(position=Point(x=x, y=y, z=0), orientation=angle))
goal = MoveBaseGoal(target_pose=dest)
return goal
def main():
rospy.init_node('trigger_open_door')
# initial door
prior_door = Door()
prior_door.frame_p1.x = 1.0
prior_door.frame_p1.y = -0.5
prior_door.frame_p2.x = 1.0
prior_door.frame_p2.y = 0.5
prior_door.door_p1.x = 1.0
prior_door.door_p1.y = -0.5
prior_door.door_p2.x = 1.0
prior_door.door_p2.y = 0.5
prior_door.travel_dir.x = 1.0
prior_door.travel_dir.y = 0.0
prior_door.travel_dir.z = 0.0
prior_door.rot_dir = Door.ROT_DIR_COUNTERCLOCKWISE
prior_door.hinge = Door.HINGE_P2
prior_door.header.frame_id = "base_footprint"
door = DoorGoal()
door.door = prior_door
ac = SimpleActionClient('move_through_door', DoorAction)
print 'Waiting for open door action server'
ac.wait_for_server()
print 'Sending goal to open door action server'
ac.send_goal_and_wait(door, rospy.Duration(500.0), rospy.Duration(2.0))
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation', GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
def time_plate_main():
rospy.loginfo('Reading file')
[result, goal, collision_objects, robot_state] = pickle.load(open(sys.argv[1], 'r'))
rospy.loginfo('Original starting state was\n'+str(robot_state))
markers = vt.robot_marker(robot_state, ns='robot_starting_state', a = 0.5)
pub = rospy.Publisher('/darrt_planning/robot_state_markers', MarkerArray)
for i in range(10):
rospy.loginfo('Publishing starting state!')
pub.publish(markers)
rospy.sleep(0.1)
wi = WorldInterface()
wi.reset(repopulate=False)
psi = get_planning_scene_interface()
psi.reset()
plate = None
for co in collision_objects:
wi.add_object(co)
if (co.id == goal.pickup_goal.collision_object_name):
plate = co
psi.reset()
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
#the old pickup goal may have used base link to generate grasps
#i don't know what we do with things for which we need a point cloud
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name=goal.pickup_goal.collision_object_name,
collision_support_surface_name=goal.pickup_goal.collision_support_surface_name,
desired_grasps=plate_grasps(ops, ops.header.frame_id))
rospy.loginfo('Teleop to initial state. Ready to go?')
raw_input()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.execute = False
goal.planning_time = 200
goal.tries = 10000
goal.debug_level = 1
goal.do_pause = False
goal.goal_bias = 0.2
# goal.place_goal.place_locations[0].pose.position.x = 1
# goal.place_goal.place_locations[0].pose.position.y = 1
# goal.place_goal.place_locations[0].pose.position.z = 0.86
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: '+sys.argv[1]+', restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def pickplace_main():
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
wi = WorldInterface()
psi = get_planning_scene_interface()
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.lift_distance = 0.3
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -1.3
place_pose_stamped.pose.position.y = 0.7
if 'graspable' in goal.pickup_goal.collision_object_name:
place_pose_stamped.pose.position.z = 0.8
else:
place_pose_stamped.pose.position.z = 0.75
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'serving_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.object_sampling_fraction = 0.3
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 30
goal.tries = 10
goal.goal_bias = 0.2
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('Restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def main():
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = 'map'
place_pose_stamped.pose.position.x -= 1.3
place_pose_stamped.pose.position.y -= 0.3
place_pose_stamped.pose.position.z = 0.98
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = det.table_name,
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.17
goal.object_sampling_fraction = 0.9
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'pickplace_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
marray = MarkerArray()
if result.error_code == result.SUCCESS:
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
def tts_callback(self, msg):
# Connect to the text-to-speech action server
client = SimpleActionClient('/tts', TtsAction)
rospy.loginfo("SPEECH TTS NODE: Waiting for the server")
if client.wait_for_server(timeout = rospy.Duration(2)):
# Create a goal to say our sentence
goal = TtsGoal()
goal.rawtext.text = msg.data
goal.rawtext.lang_id = "en_GB"
# Send the goal and wait
client.send_goal_and_wait(goal)
def close_gripper(which_arm):
""" close the gripper """
gripper_client = SimpleActionClient(which_arm[0] + '_gripper_controller/gripper_action', Pr2GripperCommandAction)
gripper_client.wait_for_server()
##### Define goals for gripper: close all the way
goal_close = Pr2GripperCommandGoal()
goal_close.command.position = 0.0
goal_close.command.max_effort = 20.0
gripper_client.send_goal_and_wait(goal_close) # open gripper for knob
def open_gripper(which_arm):
""" Open the gripper """
gripper_client = SimpleActionClient(which_arm[0] + '_gripper_controller/gripper_action',
Pr2GripperCommandAction)
gripper_client.wait_for_server()
##### Define goals for gripper: open all the way
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = 0.05
goal_open.command.max_effort = 20.0
gripper_client.send_goal_and_wait(goal_open) # open gripper for switch-flipping
def tts_with_state_callback(self, msg):
if self.states == {}:
self.read_states()
# Connect to the text-to-speech action server
client = SimpleActionClient('/tts', TtsAction)
rospy.loginfo("SPEECH TTS NODE: Waiting for the server")
if client.wait_for_server(timeout = rospy.Duration(2)):
# Create a goal to say our sentence
goal = TtsGoal()
goal.rawtext.text = self.states[msg.state]
goal.rawtext.lang_id = self.language
# Send the goal and wait
client.send_goal_and_wait(goal)
def process_word(req):
text = req.x
rospy.loginfo("I'll say: " + text)
# Connect to the text-to-speech action server
client = SimpleActionClient('/tts', TtsAction)
client.wait_for_server()
# Create a goal to say our sentence
goal = TtsGoal()
goal.rawtext.text = text
goal.rawtext.lang_id = "en_GB"
# Send the goal and wait
client.send_goal_and_wait(goal)
return 1
def main():
client = SimpleActionClient("base_trajectory_action", BaseTrajectoryAction)
client.wait_for_server()
rospy.loginfo('Found action server!')
goal = BaseTrajectoryGoal()
goal.world_frame = "odom_combined"
goal.robot_frame = "base_footprint"
goal.trajectory = [Pose2D(x=0, y=0, theta=0)]
goal.linear_velocity = 0.2
goal.angular_velocity = np.pi / 4.0
goal.angular_error = 0.01
goal.linear_error = 0.02
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
class SphericalService(object):
def __init__(self):
rospy.loginfo("Starting Spherical Grab Service")
self.pick_type = PickAruco()
rospy.loginfo("Finished SphericalService constructor")
self.place_gui = rospy.Service("/place_gui", Empty,
self.start_aruco_place)
self.power_gui = rospy.Service("/power_gui", Empty,
self.start_aruco_power)
self.pick_gui = rospy.Service("/pick_gui", Empty,
self.start_aruco_pick)
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
self.home_gui = rospy.Service("/home_gui", Empty,
self.start_aruco_home)
def start_aruco_pick(self, req):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'pregrasp_weight'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
# rospy.loginfo("Start look around.")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'head_look_around'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Look around have been done.")
self.pick_type.pick_aruco("pick")
return {}
def start_aruco_power(self, req):
self.pick_type.pick_aruco("power")
return {}
def start_aruco_place(self, req):
self.pick_type.pick_aruco("place")
return {}
def start_aruco_home(self, req):
rospy.loginfo("Go back to home")
pmg = PlayMotionGoal()
pmg.motion_name = 'home'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
class TextReader(object):
def __init__(self):
self.last_img = None
self.img_sub = rospy.Subscriber('/wide_stereo/left/image_color/compressed',
CompressedImage,
self.img_cb)
self.pub_text = rospy.Publisher('/recognized_text', String)
self.speaking = False
try:
self.tts_ac = SimpleActionClient('/tts', TtsAction)
rospy.loginfo("Waiting for TTS AS...")
self.tts_ac.wait_for_server()
rospy.loginfo("Found TTS AS!")
self.speaking = True
except NameError:
pass
def img_cb(self, data):
self.last_img = data
def get_text(self, image):
np_arr = np.fromstring(image.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
cv2.imwrite('/tmp/current_image_reading.png', image_np)
text = pytesseract.image_to_string(
Image.open('/tmp/current_image_reading.png'))
return text
def say_sentence(self, text):
rospy.loginfo("I'll say:" + text)
text = text.replace('1', 'I')
text = text.replace('\n', '')
g = TtsGoal()
g.text = text
self.tts_ac.send_goal_and_wait(g)
rospy.sleep(1)
def run(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
if self.last_img:
text = self.get_text(self.last_img)
if text:
rospy.loginfo("I read: " + str(text))
self.pub_text.publish(text)
if self.speaking:
self.say_sentence(text)
rospy.loginfo("I couldn't read anything.")
r.sleep()
def main():
client = SimpleActionClient("base_trajectory_action", BaseTrajectoryAction)
client.wait_for_server()
rospy.loginfo('Found action server!')
goal = BaseTrajectoryGoal()
goal.robot_frame = "base_footprint"
goal.world_frame = "map"
for i in range(50):
goal.trajectory.append(Pose2D(x=-1.0/49.0*i, y=-1.0/49.0*i, theta=np.pi/49.0*i))
# goal.trajectory = [Pose2D(x=-1, y=0, theta=np.pi), Pose2D(x=-2, y=-1, theta=0)]
#goal.linear_velocity = 0.35;
#goal.angular_velocity = np.pi/4.0;
#goal.angular_error = 0.05;
#goal.linear_error = 0.05;
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
def main():
[result, goal, collision_objects,
robot_state] = pickle.load(open(sys.argv[1], 'r'))
wi = WorldInterface()
#set the planning scene up correctly
wi.reset(repopulate=False)
for co in collision_objects:
#for running easy version
if ('table' in co.id or co.id == 'plate'):
wi.add_object(co)
psi = get_planning_scene_interface()
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
average_time = 0
goal.planning_time = 30
goal.tries = 10000
goal.debug_level = 0
goal.do_pause = False
goal.goal_bias = 0.2
#i think that this does not work
#goal.robot_state = robot_state
goal.execute = False
rospy.loginfo('First place goal is\n' +
str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: ' + sys.argv[1] + ', restart after ' +
str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error ' +
str(result.error_code) + '. We had ' + str(i) +
' successful trials with an average time of ' +
str(average_time / float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL ' + str(i) + ': ' + str(result.planning_time))
rospy.loginfo(
str(NTRIALS) + ' averaged ' + str(average_time / float(NTRIALS)) +
' seconds to solve')
class MoveToWaypoint(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = PNPSimplePluginServer(
name,
MoveToWaypointAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.msg_store = MessageStoreProxy(
database=rospy.get_param("~db_name", "semantic_map"),
collection=rospy.get_param("~collection_name", "waypoints")
)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self.client = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base client.")
self.client.wait_for_server()
self._as.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
self.stop_client.send_goal(TrackPersonGoal())
if goal.to != "none":
pose, _ = self.load({"waypoint_name": goal.to})
rospy.loginfo("Going from '%s' to '%s'" % (goal.from_, goal.to))
self.client.send_goal_and_wait(MoveBaseGoal(target_pose=pose))
res = MoveToWaypointResult()
res.result.append(ActionResult(cond="robot_at_waypoint__"+goal.to, truth_value=True))
res.result.append(ActionResult(cond="robot_at_waypoint__"+goal.from_, truth_value=False))
res.result.append(ActionResult(cond="robot_pose_unknown", truth_value=True))
self._as.set_succeeded(res)
def load(self, meta):
message = self.msg_store.query(PoseStamped._type, {}, meta)
if len(message) == 0:
raise Exception("Desired data set %s: %s not in datacentre."% meta.items()[0])
else:
return message[0][0], message[0][1]["_id"]
def _send_action_and_wait(
action_client: SimpleActionClient,
goal,
timeout: rospy.Duration = rospy.Duration()) -> bool:
if timeout == rospy.Duration():
if not action_client.wait_for_server():
# Action server is not available
return False
assert action_client.simple_state == SimpleGoalState.DONE
status = action_client.send_goal_and_wait(goal=goal)
else:
deadline = rospy.Time.now() + timeout
if not action_client.wait_for_server(timeout=deadline -
rospy.Time.now()):
# Action server is not available
return False
assert action_client.simple_state == SimpleGoalState.DONE
status = action_client.send_goal_and_wait(
goal=goal, execute_timeout=deadline - rospy.Time.now())
return status == GoalStatus.SUCCEEDED
class Gripper(object):
def __init__(self):
self.gripper_client = SimpleActionClient(GRIPPER_CLIENT_TOPIC,
GripperApplyEffortAction)
rospy.loginfo("GRIPPER CLIENT: Waiting for gripper action server...")
self.gripper_client.wait_for_server(rospy.Duration(2))
self.wrist_wrench = WrenchStamped()
rospy.Subscriber(GRIPPER_WRENCH_TOPIC, WrenchStamped, self._wrench_cb)
def _wrench_cb(self, msg):
self.wrist_wrench = msg
def grasp(self, effort=0.0):
goal = GripperApplyEffortGoal()
goal.effort = effort
result = self.gripper_client.send_goal_and_wait(goal)
if result == GoalStatus.SUCCEEDED:
return True
else:
return False
def release(self):
rospy.loginfo("GRIPPER CLIENT: Releasing gripper.")
self.grasp(0.1)
def grab(self):
rospy.loginfo("GRIPPER CLIENT: Closing gripper.")
self.grasp(-0.3)
def get_wrist_force(self):
self.wrist_wrench = rospy.wait_for_message(GRIPPER_WRENCH_TOPIC,
WrenchStamped)
return self.wrist_wrench.wrench.force.x
def wait_for_push(self, timeout=0.0):
if timeout == 0.0:
while not rospy.is_shutdown():
x = self.wrist_wrench.wrench.force.x
if x > 17:
return True
rospy.Rate(10).sleep()
else:
end = time() + timeout
while not rospy.is_shutdown() and (time() < end):
x = self.wrist_wrench.wrench.force.x
if x > 17:
return True
rospy.Rate(10).sleep()
return False
def main():
#Find an object to pick up
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
#DARRT action client
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
rospy.loginfo('Waiting for DARRT action')
client.wait_for_server()
rospy.loginfo('Found DARRT action')
#DARRTGoal
goal = DARRTGoal()
#pickup goal (from the tabletop detection)
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
#place goal (move left 20 cm)
place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
place_pose_stamped.pose.position.y += 0.2
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name,
[place_pose_stamped],
collision_support_surface_name =
det.table_name,
collision_object_name =
goal.pickup_goal.collision_object_name)
#Send the goal to the action
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
#Check the result
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Planning failed. Error code: '+str(result.error_code))
return
rospy.loginfo('Planning succeeded!')
#at this point the planning is done.
#now we execute and visualize the plan
#visualize trajectory
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
marray = get_trajectory_markers(result.primitive_trajectories)
for i in range(10):
pub.publish(marray)
rospy.sleep(0.05)
#Executor is a Python class for executing DARRT plans
executor = Executor()
#execute trajectory
rospy.loginfo('Press enter to execute')
raw_input()
executor.execute_trajectories(result)
class PickupHandler(object):
def __init__(self):
# maximum height of the torso
self.HEIGHT_MAX = rospy.get_param('~torso_height_limit')
'''Start the action client services and initialize publishers.'''
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
# Initialize publishers
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.head_cmd = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
# Connect to MoveIt Commander
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('arm_torso')
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
self.eef_link = self.group.get_end_effector_link()
self.goals = []
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickupHandler.")
def lift_torso(self, height=0.0):
'''
Move the torso to the desired height.
If height is less than 0 or greater than HEIGHT_MAX, height is
set to those values.
'''
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
height = min(self.HEIGHT_MAX, max(0.0, height))
jtp.positions = [height]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def lower_head(self):
'''
Lower robot head.
There's currently no purpose to this, it just looks cool.
'''
rospy.loginfo("Lowering head for a e s t h e t i c")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -1.0]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
def move_arm_to_final(self):
'''
Moves arm to the final pose after a successful pickup.
The final pose of the arm is given in config/pick_motions.yaml
as 'pick_final_pose'. Currently this flips the arm over causing
the object to fall...
'''
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
rospy.loginfo("Done!")
def pickup(self, goal_pose):
'''
Move the gripper to goal_pose and pick up the object at that position.
Parameters:
goal_pose: PoseStamped object indicating the position of the object
to be picked up. Currently the frame of the object has to
be 'base_footprint'
'''
# Remove leading slash from frame id
goal_pose.header.frame_id = self.strip_leading_slash(
goal_pose.header.frame_id)
rospy.loginfo("Goal received:\n" + str(goal_pose))
self.prepare_robot()
rospy.sleep(2.0)
# Create and initialize pickup goal with position from goal pose
# TODO: if the goal pose frame is not in base, need to transform
pick_g = PickUpPoseGoal()
pick_g.object_pose.pose.position = goal_pose.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
# rospy.loginfo("goal pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
# Send pickup goal to pick and place server
rospy.loginfo("Attempting to pick up:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Check that the pickup was successful
# TODO: what to do if pickup wasn't successful??
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr(
"Failed to pick, not trying further - error code was {}".
format(str(moveit_error_dict[result.error_code])))
# return
# move arm to final position
self.lift_torso(height=0.2)
# self.move_arm_to_final() # pick_final_pose causes object to fall
def place(self, goal_pose):
# move arm to desired pose
# get goal and plan path
# rospy.loginfo('Planning path to new goal...')
# self.group.set_pose_target(goal_pose)
# plan = self.group.plan()
# # # visualize path in rviz
# # rospy.loginfo('Visualizing in RViz...')
# # display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# # display_trajectory.trajectory_start = self.robot.get_current_state()
# # display_trajectory.trajectory.append(plan)
# # self.display_trajectory_publisher.publish(display_trajectory)
# # rospy.sleep(2) # allow time for visualization
# # perform movement
# self.group.go(wait=True)
# print('Path planning and movement successful!')
# # clear current goal
# self.group.stop()
# self.group.clear_pose_targets()
# open gripper so object falls out
# self.open_gripper()
# Remove leading slash from frame id
goal_pose.header.frame_id = self.strip_leading_slash(
goal_pose.header.frame_id)
rospy.loginfo("Goal received:\n" + str(goal_pose))
# Create and initialize pickup goal with position from goal pose
pick_g = PickUpPoseGoal()
pick_g.object_pose.pose.position = goal_pose.pose.position
# rospy.loginfo("goal pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
rospy.loginfo("Placing object at goal pose")
self.place_as.send_goal_and_wait(pick_g)
def prepare_robot(self):
'''Prepare the robot for picking up an object.'''
self.unfold_arm()
self.lower_head()
rospy.loginfo("Robot prepared.")
def unfold_arm(self):
'''
Unfolds the robot arm.
The configuration of the unfolded arm is given in
config/pick_motions.yaml as 'pregrasp'.
'''
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'pregrasp'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done unfolding arm.")
def open_gripper(self):
'''
Opens the gripper.
The configuration is given somewhere in tiago_gazebo...
'''
rospy.loginfo('Opening gripper')
pmg = PlayMotionGoal()
pmg.motion_name = 'open'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo('Done opening gripper')
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
class ObjectCategorizer():
def __init__(self):
self.object_detector = ObjectDetector()
# connect to collision map processing service
rospy.loginfo('waiting for tabletop_collision_map_processing service')
rospy.wait_for_service('/tabletop_collision_map_processing/tabletop_collision_map_processing')
self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing)
rospy.loginfo('connected to tabletop_collision_map_processing service')
# connect to gripper action server
rospy.loginfo('waiting for wubble_gripper_grasp_action')
self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.posture_controller.wait_for_server()
rospy.loginfo('connected to wubble_gripper_grasp_action')
rospy.loginfo('waiting for wubble_grasp_status service')
rospy.wait_for_service('/wubble_grasp_status')
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
rospy.loginfo('connected to wubble_grasp_status service')
rospy.loginfo('waiting for classify service')
rospy.wait_for_service('/classify')
self.classification_srv = rospy.ServiceProxy('/classify', classify)
rospy.loginfo('connected to classify service')
# connect to gripper action server
rospy.loginfo('waiting for wubble_gripper_action')
self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction)
self.gripper_controller.wait_for_server()
rospy.loginfo('connected to wubble_gripper_action')
# connect to wubble actions
rospy.loginfo('waiting for drop_object')
self.drop_object_client = SimpleActionClient('/drop_object', DropObjectAction)
self.drop_object_client.wait_for_server()
rospy.loginfo('connected to drop_object')
rospy.loginfo('waiting for grasp_object')
self.grasp_object_client = SimpleActionClient('/grasp_object', GraspObjectAction)
self.grasp_object_client.wait_for_server()
rospy.loginfo('connected to grasp_object')
rospy.loginfo('waiting for lift_object')
self.lift_object_client = SimpleActionClient('/lift_object', LiftObjectAction)
self.lift_object_client.wait_for_server()
rospy.loginfo('connected to lift_object')
rospy.loginfo('waiting for place_object')
self.place_object_client = SimpleActionClient('/place_object', PlaceObjectAction)
self.place_object_client.wait_for_server()
rospy.loginfo('connected to plcae_object')
rospy.loginfo('waiting for push_object')
self.push_object_client = SimpleActionClient('/push_object', PushObjectAction)
self.push_object_client.wait_for_server()
rospy.loginfo('connected to push_object')
rospy.loginfo('waiting for shake_roll_object')
self.shake_roll_object_client = SimpleActionClient('/shake_roll_object', ShakeRollObjectAction)
self.shake_roll_object_client.wait_for_server()
rospy.loginfo('connected to drop_object')
rospy.loginfo('waiting for shake_pitch_object')
self.shake_pitch_object_client = SimpleActionClient('/shake_pitch_object', ShakePitchObjectAction)
self.shake_pitch_object_client.wait_for_server()
rospy.loginfo('connected to pitch_object')
rospy.loginfo('waiting for ready_arm')
self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction)
self.ready_arm_client.wait_for_server()
rospy.loginfo('connected to ready_arm')
# advertise InfoMax service
rospy.Service('get_category_distribution', InfoMax, self.process_infomax_request)
rospy.loginfo('all services contacted, object_categorization is ready to go')
self.ACTION_INFO = {
InfomaxAction.GRASP: {
'client': self.grasp_object_client,
'goal': GraspObjectGoal(),
'prereqs': [InfomaxAction.GRASP]
},
InfomaxAction.LIFT: {
'client': self.lift_object_client,
'goal': LiftObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT]
},
InfomaxAction.SHAKE_ROLL: {
'client': self.shake_roll_object_client,
'goal': ShakeRollObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_ROLL]
},
InfomaxAction.DROP: {
'client': self.drop_object_client,
'goal': DropObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP]
},
InfomaxAction.PLACE: {
'client': self.place_object_client,
'goal': PlaceObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE]
},
InfomaxAction.PUSH: {
'client': self.push_object_client,
'goal': PushObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE, InfomaxAction.PUSH]
},
InfomaxAction.SHAKE_PITCH: {
'client': self.shake_pitch_object_client,
'goal': ShakePitchObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.SHAKE_PITCH]
},
}
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def gentle_close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = 0.0
goal.dynamic_torque_control = False
self.gripper_controller.send_goal(goal)
self.gripper_controller.wait_for_result()
def segment_objects(self):
res = self.object_detector.detect()
if res is None:
rospy.logerr('TabletopSegmentation did not find any clusters')
return None
segmentation_result = res['segmentation_result']
self.info = res['cluster_information']
tdr = TabletopDetectionResult()
tdr.table = segmentation_result.table
tdr.clusters = segmentation_result.clusters
tdr.result = segmentation_result.result
tcmpr = self.update_collision_map(tdr)
return tcmpr
def reset_collision_map(self):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = TabletopDetectionResult()
req.reset_collision_models = True
req.reset_attached_models = True
self.collision_map_processing_srv(req)
rospy.loginfo('collision map reset')
def update_collision_map(self, tabletop_detection_result):
rospy.loginfo('collision_map update in progress')
req = TabletopCollisionMapProcessingRequest()
req.detection_result = tabletop_detection_result
req.reset_collision_models = True
req.reset_attached_models = True
req.desired_frame = 'base_link'
res = self.collision_map_processing_srv(req)
rospy.loginfo('collision_map update is done')
rospy.loginfo('there are %d graspable objects %s, collision support surface name is "%s"' %
(len(res.graspable_objects), res.collision_object_names, res.collision_support_surface_name))
return res
def reset_robot(self, tabletop_collision_map_processing_result=None):
rospy.loginfo('resetting robot')
ordered_collision_operations = OrderedCollisionOperations()
if tabletop_collision_map_processing_result:
closest_index = self.info[0][0]
collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index]
collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name
collision_operation = CollisionOperation()
collision_operation.object1 = collision_support_surface_name
collision_operation.object2 = ARM_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations = [collision_operation]
collision_operation = CollisionOperation()
collision_operation.object1 = collision_support_surface_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations.append(collision_operation)
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
collision_operation.penetration_distance = 0.02
ordered_collision_operations.collision_operations.append(collision_operation)
else:
self.reset_collision_map()
current_state = find_current_arm_state()
rospy.loginfo('arm is currently in %s state' % current_state)
if current_state not in ['READY', 'TUCK']:
# check if the gripper is empty
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied:
rospy.loginfo('arm is not in any of the following states %s, opening gripper' % str(['READY', 'TUCK']))
self.open_gripper()
if current_state != 'READY' and not self.ready_arm(ordered_collision_operations): return False
self.gentle_close_gripper()
return True
def ready_arm(self, collision_operations=OrderedCollisionOperations()):
"""
Moves the arm to the side out of the view of all sensors. In this
position the arm is ready to perform a grasp action.
"""
goal = ReadyArmGoal()
goal.collision_operations = collision_operations
state = self.ready_arm_client.send_goal_and_wait(goal)
if state == GoalStatus.SUCCEEDED: return True
else: return False
def execute_action(self, action, tabletop_collision_map_processing_result):
goal = self.ACTION_INFO[action]['goal']
goal.category_id = self.category_id
closest_index = self.info[0][0]
goal.graspable_object = tabletop_collision_map_processing_result.graspable_objects[closest_index]
goal.collision_object_name = tabletop_collision_map_processing_result.collision_object_names[closest_index]
goal.collision_support_surface_name = tabletop_collision_map_processing_result.collision_support_surface_name
state = self.ACTION_INFO[action]['client'].send_goal_and_wait(goal)
if state == GoalStatus.SUCCEEDED:
result = self.ACTION_INFO[action]['client'].get_result()
return result.recorded_sound
else:
return None
# receive an InfoMax service request containing object ID and desired action
def process_infomax_request(self, req):
self.object_names = req.objectNames
self.action_names = req.actionNames
self.num_categories = req.numCats
self.category_id = req.catID
if not self.reset_robot(): return None
# find a graspable object on the floor
tcmpr = self.segment_objects()
if tcmpr is None: return None
# initialize as uniform distribution
beliefs = [1.0/self.num_categories] * self.num_categories
actions = self.ACTION_INFO[req.actionID.val]['prereqs']
for act in actions:
sound = self.execute_action(act, tcmpr)
if not sound:
self.reset_robot(tcmpr)
return None
else:
resp = self.classification_srv(sound)
beliefs = resp.beliefs
if not self.reset_robot(tcmpr): return None
res = InfoMaxResponse()
res.beliefs = beliefs
res.location = self.category_id
return res
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.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, angle)
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 = -1
# 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 = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
class SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState,
self.__joint_state_cb, queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
class PR2MoveArm(object):
def __init__(self, ik, joint_mover):
self.move_right_arm_client = SimpleActionClient("move_right_arm", MoveArmAction)
self.move_right_arm_client.wait_for_server()
self.move_left_arm_client = SimpleActionClient("move_left_arm", MoveArmAction)
self.move_left_arm_client.wait_for_server()
self.ik = ik
self.joint_mover = joint_mover
def move_right_arm(self, position, orientation, frame_id, waiting_time):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 10
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(waiting_time / 10.);
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
position_constraint.link_name = "r_wrist_roll_link"
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 2 * 0.02
position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame_id
orientation_constraint.link_name = "r_wrist_roll_link"
# orientation_constraint.type = dunno!
orientation_constraint.orientation.x = orientation[0]
orientation_constraint.orientation.y = orientation[1]
orientation_constraint.orientation.z = orientation[2]
orientation_constraint.orientation.w = orientation[3]
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
goal.disable_collision_monitoring = True
# rospy.loginfo("Goal: " + str(goal))
state = self.move_right_arm_client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
if state == actionlib.GoalStatus.SUCCEEDED:
return True
else:
return False
def move_right_arm_non_collision(self, position, orientation, frame_id, ignore_errors = False):
current_pose_stamped = self.ik.run_fk(self.joint_mover.robot_state.right_arm_positions,
"r_wrist_roll_link")
end_pose = self.ik.lists_to_pose_stamped(position,
orientation,
frame_id,
frame_id)
trajectory, error_codes = self.ik.check_cartesian_path(current_pose_stamped,
end_pose,
self.joint_mover.robot_state.right_arm_positions,
collision_aware = 0,
num_steps=10)
if (not ignore_errors) and any( (e == 3 for e in error_codes) ):
rospy.logerr("IK returns error codes: %s"%str(error_codes))
return False
self.joint_mover.execute_trajectory(trajectory, "right", True)
return True
class ForceTorquePlay(object):
def __init__(self):
self.patient_id = '2019001'
self.last_msg = None
self.start_ac = None
self.gripper_joints = None
self.plan_compute = False
self.moveit_status = 0
self.torso_joints = []
self.pos = []
self.person = False
self.arm_joints= None
self.action = 'None'
self.first_look = False
self.plan_feedback = None
self.heightType = '03'
self.marker_xtion = []
self.force_torque_sub = rospy.Subscriber('/wrist_ft',
WrenchStamped,
self.force_torque_cb,
queue_size=1)
self.pub = rospy.Publisher('/trigger', String, queue_size=1)
self.plan_done= rospy.Subscriber('/done', String, self.plan_state)
self.scene = moveit_commander.PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.interpreter = moveit_commander.MoveGroupCommandInterpreter()
self.interpreter.execute("use arm")
self.group = moveit_commander.MoveGroupCommander("arm")
self.eef_link=self.group.get_end_effector_link()
self.scene.remove_attached_object(self.eef_link, name="pringles")
self.scene.remove_world_object("pringles")
self.scene.remove_world_object("table")
self.torso = SimpleActionClient(
'/safe_torso_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
self.arm_torso = SimpleActionClient(
'/safe_arm_torso_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
self.cam_info = rospy.wait_for_message("/xtion/rgb/camera_info", CameraInfo, timeout=None)
self.client = SimpleActionClient('/safe_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
self.arm_subscribe= rospy.Subscriber('/arm_controller/state', JointTrajectoryControllerState, self.arm_state)
self.face = rospy.Subscriber("/pal_face/faces", FaceDetections, self.callback_face)
self.client.wait_for_server()
self.torso_subscribe= rospy.Subscriber('/torso_controller/state', JointTrajectoryControllerState, self.torso_state)
self.action_begin = rospy.Subscriber('/action', String, self.action_feedback)
self.action_begin_pub = rospy.Publisher('/action', String, queue_size = 1)
self.aruco_tf = rospy.Subscriber('/xtion_aruco', PoseStamped, self.aruco_xtion_tf)
self.send_waypoints = rospy.Publisher('/my_waypoints', Path, queue_size = 1)
self.gripper_cmd = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=1)
self.head_cmd = rospy.Publisher(
'/head_controller/command', JointTrajectory, queue_size=1)
self.arm_controller = rospy.Publisher(
'/arm_controller/command', JointTrajectory, queue_size=1)
self.message = " "
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
self.error = rospy.Subscriber('/move_group/feedback', MoveGroupActionFeedback, self.moveit_state)
rospy.loginfo("Connected!")
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path_josh', moveit_msgs.msg.DisplayTrajectory)
self.say_client = SimpleActionClient('/tts', TtsAction)
self.say_client.wait_for_server()
self.ac = SimpleActionClient(
'/head_controller/point_head_action', PointHeadAction)
self.hand_sub = rospy.Subscriber("/hand", PoseStamped, self.skeleton)
self.plan_done= rospy.Publisher('/plan_request', String, queue_size = 1)
self.plan_feedback = rospy.Subscriber("/plan_done", String, self.plan_cb)
self.skip_intro = True
self.my_rosbag = rosbag.Bag('/home/pal/sara_ws/patient_data/patientriggers'+self.patient_id+'block1.bag', 'w')
self.release_time = 0
rate = rospy.Rate(1)
self.listener = tf.TransformListener()
rospy.loginfo("Looked down")
self.hand_position = []
self.prev_hand_position = []
self.velocity = []
self.time_hand = 0.1
self.movement_init = False
self.init_time = 0
self.finish_time = 0
self.init_position = []
self.counter = 0
self.track_time = 0
self.up_count = 0
self.forward_count = 0
self.object_grasped = 0
self.near_object = 0
self.move_start = 0
self.initial_pose = []
self.initial_or = []
self.hand_up = False
self.hand_forward = False
self.environment_set = False
self.grasped = False
self.rest_pose = []
self.first = True
self.initial_torque = []
self.torque_after_grasp = []
self.object_weight_x = 0
self.object = [0.25, 0.08, 0.04]
self.fl0 = 0
self.m = 0
self.fzlg = 0.2
self.fg0 = 0
self.fl = 0
self.threshold = 0
self.cout_actions = 0
self.force_torque_sub = rospy.Subscriber('/wrist_ft',
WrenchStamped,
self.force_torque_cb,
queue_size=1)
self.gripper_state = rospy.Subscriber('/gripper_controller/state', JointTrajectoryControllerState, self.gripper_state, queue_size=1)
self.gripper_cmd = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=1)
self.run()
def force_torque_cb(self, msg):
self.last_msg = msg
def plan_cb(self, msg):
self.plan_feedback = msg.data
def aruco_xtion_tf(self, msg):
self.marker_xtion = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
def plan_state(self, msg):
self.plan_computed= msg.data
def gripper_state(self, msg):
self.gripper_joints = msg.actual.positions
def arm_state(self,msg):
self.arm_joints = msg
def torso_state(self, msg):
self.torso_joints = msg
def moveit_state(self, msg):
self.moveit_status = msg.status.status
def goto_cartesian(self, trans, rot):
waypoints = []
wpose = self.group.get_current_pose().pose
wpose.position.x = trans[0]
wpose.position.y = trans[1]
wpose.position.z = trans[2]
wpose.orientation.x = rot[0]
wpose.orientation.y = rot[1]
wpose.orientation.z = rot[2]
wpose.orientation.w = rot[3]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0,True)
self.group.execute(plan, wait=True)
def run(self):
marker = False
while not rospy.is_shutdown() and self.last_msg is None:
rospy.sleep(0.2)
r = rospy.Rate(2)
while not rospy.is_shutdown():
self.trajectory_test()
r.sleep()
def trajectory_test(self):
self.scene.remove_attached_object(self.eef_link, name="object")
self.scene.remove_world_object("object")
#Define how to perform trajectory planning for robot-to-human grasping. Cartesian = True indicates cartesian path planning,
#smoothed = True includes post-processing to apply time parameterization with external node.
cartesian = True
smoothed = True
while (not self.environment_set):
#Prepare the collision environment with respect to the marker
self.scene.remove_world_object("table")
self.scene.remove_world_object("right_restrict")
self.scene.remove_world_object("left_restrict")
self.scene.remove_world_object("top_restrict")
try:
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.26] looks before
jtp.time_from_start = rospy.Duration(1.0) head
jt.points.append(jtp)
self.head_cmd.publish(jt) #Look up to see the marker
rospy.sleep(1)
(trans,rot) = self.listener.lookupTransform('/base_footprint', '/arm_goal', rospy.Time(0))
#Collision object: table
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = "aruco_marker_frame"
object_pose.pose.position.x=-self.object[0] - 0.05
object_pose.pose.position.y=-0.7
object_pose.pose.orientation.w=1.0
object_name = "table"
self.scene.add_box(object_name, object_pose, size=(0.1,2,2))
#Collision object: left block
obstacle_pose = geometry_msgs.msg.PoseStamped()
obstacle_pose.header.frame_id = "aruco_marker_frame"
obstacle_pose.pose.position.y=-0.5
obstacle_pose.pose.position.z = 1.7
obstacle_pose.pose.orientation.w=1.0
object_name = "left_restrict"
self.scene.add_box(object_name, obstacle_pose, size=(2,2,2))
#Collision object: top block
obstacle_pose = geometry_msgs.msg.PoseStamped()
obstacle_pose.header.frame_id = "aruco_marker_frame"
obstacle_pose.pose.position.x=1.5
obstacle_pose.pose.orientation.w=1.0
object_name = "top_restrict"
self.scene.add_box(object_name, obstacle_pose, size=(2,2,2))
#Collision object: right block
obstacle_pose = geometry_msgs.msg.PoseStamped()
obstacle_pose.header.frame_id = "aruco_marker_frame"
obstacle_pose.pose.position.y=-0.5
obstacle_pose.pose.position.z = -1.2
obstacle_pose.pose.orientation.w=1.0
object_name = "right_restrict"
self.scene.add_box(object_name, obstacle_pose, size=(2,2,2))
#Collision object: object
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = "aruco_marker_frame"
object_pose.pose.position.x= -0.1
object_pose.pose.orientation.w=1.0
object_name = "object"
self.scene.remove_attached_object(self.eef_link, name="object")
self.scene.remove_world_object("object")
#Set initial object position
(trans2,rot2) = self.listener.lookupTransform('/base_footprint', '/place_goal', rospy.Time(0))
self.initial_pose = [trans2[0], trans2[1], trans2[2]]
self.initial_or = [rot2[0], rot2[1], rot2[2], rot2[3]]
print ("initial pose of object",trans2, rot2)
goal = PlayMotionGoal()
goal.motion_name = 'open_gripper'
goal.skip_planning = False
self.play_m_as.send_goal_and_wait(goal)
self.environment_set = True
print("ENVIRONMENT READY")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.75]
jtp.time_from_start = rospy.Duration(1.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
marker = False
#Robot-to-human handover
if(self.action == 'robot'):
rospy.sleep(1)
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.56]
jtp.time_from_start = rospy.Duration(1.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.sleep(1.0)
touch_links=['gripper_link', 'gripper_left_finger_link', 'gripper_right_finger_link', 'gripper_grasping_frame']
#Detect marker
while(not marker):
print("Action started")
try:
marker = True
#Lookup transform between the base_footprint and arm_goal, which specifies the pregrasp position
(trans,rot) = self.listener.lookupTransform('/base_footprint', '/arm_goal', rospy.Time(0))
print("waiting")
self.write("Marker location " + str(trans))
self.write("Marker orientation " + str(rot))
#Add object as collision object
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = "aruco_marker_frame"
object_pose.pose.position.x= -0.1
object_pose.pose.orientation.w=1.0
object_name = "object"
self.scene.remove_attached_object(self.eef_link, name=object_name)
self.scene.remove_world_object(object_name)
self.scene.add_box(object_name, object_pose, size=(self.object[0], self.object[1], self.object[2]))
goal = PlayMotionGoal()
goal.motion_name = 'open_gripper'
goal.skip_planning = False
self.play_m_as.send_goal_and_wait(goal)
x = self.marker_xtion[0]
y = self.marker_xtion[1]
#Track object
self.look_at(x,y, block=False)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if not cartesian:
#Compute trajectory by specyfying end-effector goal directly
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = rot[0]
pose_target.orientation.y = rot[1]
pose_target.orientation.z = rot[2]
pose_target.orientation.w = rot[3]
pose_target.position.x = trans[0]
pose_target.position.y = trans[1]
pose_target.position.z = trans[2]
self.group.set_pose_target(pose_target)
self.message="moving"
self.pub.publish(self.message)
self.write("Starts moving")
init_movement = datetime.now()
plan_time_start = datetime.now()
self.group.set_planner_id("ESTkConfigDefault")
plan = self.group.plan()
plan_time_start = datetime.now()
plan_execute = datetime.now() - plan_time_start
print("Time for plan", plan_execute)
self.group.go()
rospy.sleep(20)
self.interpreter.execute("go forward 0.09")
print("completed path time", datetime.now() - plan_time_start)
else:
plan_time_start = datetime.now()
if smoothed: #Compute cartesian path planning with smoothing
send_data = Path()
waypoints = []
wpose = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = trans[0]
wpose.position.y = trans[1]
wpose.position.z = trans[2]
wpose.orientation.x = rot[0]
wpose.orientation.y = rot[1]
wpose.orientation.z = rot[2]
wpose.orientation.w = rot[3]
new_pose = PoseStamped()
new_pose.pose = wpose
send_data.poses.append(copy.deepcopy(new_pose))
wpose.position.x += 0.1
waypoints.append(copy.deepcopy(wpose))
new_pose = PoseStamped()
new_pose.pose = wpose
send_data.poses.append(copy.deepcopy(new_pose))
self.message="moving"
print self.message
self.pub.publish(self.message)
self.write("Starts moving")
init_movement = datetime.now()
while (self.plan_feedback == "Done"):
rospy.sleep(0.05)
msg = String()
msg.data = "request sent"
self.plan_done.publish(msg)
self.send_waypoints.publish(send_data)
print(self.plan_feedback)
while (self.plan_feedback!= "Done"):
rospy.sleep(0.05)
#Compute cartesian path planning without smoothing
else:
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.03, # eef_step
0.0,True) # jump_threshold
self.group.execute(plan, wait=True)
print("completed path time", datetime.now() - plan_time_start)
rospy.sleep(0.5)
class Kill():
REFRESH_RATE = 20
def __init__(self):
self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]
self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]
self.v = [0] * len(self.r1)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self._action_name = 'kill'
self._tf = tf.TransformListener()
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#Initialize the sound controller
self._sound_client = SoundClient()
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.pose = None
self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)
#initialize this client
self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def marker_callback(self, marker):
if (self.pose is not None):
self.pose = marker.pose
def run(self, goal):
rospy.loginfo('Begin kill')
self.pose = goal.pose
#pose = self._tf.transformPose('/base_link', goal.pose)
self._sound_client.say('Halt!')
# turn to face the marker before opening arms (code repeated later)
r = rospy.Rate(self.REFRESH_RATE)
while(True):
pose = self.pose
if abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / ( 3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
else:
break
# open arms
traj_goal_r = JointTrajectoryGoal()
traj_goal_r.trajectory.joint_names = self.r_joint_names
traj_goal_l = JointTrajectoryGoal()
traj_goal_l.trajectory.joint_names = self.l_joint_names
traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3))
traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r2
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l2
self.l_traj_action_client.send_goal(traj_goal_l)
self._sound_client.say('You have the right to remain silent.')
# Keep a local copy because it will update
#pose = self.pose
#num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#for i in range(num_move_x):
# self._base_publisher.publish(twist_msg)
# r.sleep()
# track the marker as much as we can
while True:
pose = self.pose
# too far away
if abs(pose.pose.position.x > 1.5):
rospy.loginfo('Target out of range: ' + str(pose.pose.position.x))
self._as.set_aborted()
return;
# too far to the side -> rotate
elif abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(0.0, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y)))
#self._base_publisher.publish(twist_msg)
# now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over)
elif pose.pose.position.x > .5:
num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(.1, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for i in range(num_move_x):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.x = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#self._base_publisher.publish(twist_msg)
# susupect is within hugging range!!!
else:
break
r.sleep()
# after exiting the loop, we should be ready to capture -> send close arms goal
self._sound_client.say("Anything you say do can and will be used against you in a court of law.")
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r3
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l3
self.l_traj_action_client.send_goal(traj_goal_l)
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
rospy.loginfo('End kill')
rospy.sleep(20)
self._as.set_succeeded()
#!/usr/bin/env python
import rospy
from actionlib import SimpleActionClient
from giskard_msgs.msg import MoveAction, MoveGoal, MoveCmd, Controller
if __name__ == '__main__':
rospy.init_node('init_chain')
client = SimpleActionClient('qp_controller/command', MoveAction)
client.wait_for_server()
roots = rospy.get_param('~roots')
tips = rospy.get_param('~tips')
typess = rospy.get_param('~types')
goal = MoveGoal()
move_cmd = MoveCmd()
if not (len(roots) == len(tips) and len(tips) == len(typess)):
raise Exception('number of roots, tips and types not equal')
for root, tip, types in zip(roots, tips, typess):
for type in types:
controller = Controller()
controller.root_link = root
controller.tip_link = tip
controller.type = type
controller.weight = 0
controller.goal_pose.pose.orientation.w = 1
controller.goal_pose.header.frame_id = tip
move_cmd.controllers.append(controller)
goal.type = MoveGoal.PLAN_ONLY
goal.cmd_seq.append(move_cmd)
print('sending goal')
client.send_goal_and_wait(goal)
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.head_cmd = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
# Place the arm safe above table
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_approach_pose'
self.play_m_as.send_goal_and_wait(pmg)
pmg.skip_planning = False
rospy.loginfo("Setting ball pose based on Perception")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the arm safe above table
pmg = PlayMotionGoal()
pmg.motion_name = 'home_approach_pose'
self.play_m_as.send_goal_and_wait(pmg)
pmg.skip_planning = False
# Place the object to safe navigation position
rospy.loginfo("Gonna place the ball near my heart..")
pmg = PlayMotionGoal()
pmg.motion_name = 'home_pose'
self.play_m_as.send_goal_and_wait(pmg)
pmg.skip_planning = False
rospy.loginfo("Ready to move !")
if string_operation == "place":
pass
def lift_torso(self):
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.34]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def lower_head(self):
rospy.loginfo("Moving head down")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -0.75]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def prepare_robot(self):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'pregrasp'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
self.lower_head()
rospy.loginfo("Robot prepared.")
client.wait_for_result()
pub.publish('not moving')
elif confirmcheck == '2':
message = 'stop'
stop_pub.publish(message)
elif confirmcheck == '3':
message = 'go'
stop_pub.publish(message)
elif confirmcheck == '4':
goal = PlayMotionGoal()
goal.skip_planning = False
goal.motion_name = 'open_gripper'
play_m_as.send_goal_and_wait(goal)
elif confirmcheck == '5':
scene.remove_world_object("table")
scene.remove_world_object("right_restrict")
scene.remove_world_object("left_restrict")
scene.remove_world_object("top_restrict")
scene.remove_attached_object(eef_link, name="pringles")
scene.remove_world_object("pringles")
goal = PlayMotionGoal()
goal.skip_planning = False
goal.motion_name = 'open_gripper'
play_m_as.send_goal_and_wait(goal)
rospy.sleep(0.5)
goal = FollowJointTrajectoryGoal()
class ManipulateAruco(object):
def __init__(self):
rospy.loginfo("Initalizing ManipulateAruco...")
self.aruco_pose_top = rospy.get_param(rospy.get_name() + '/marker_pose_topic')
self.pickup_pose_top = rospy.get_param(rospy.get_name() + '/pickup_marker_pose')
self.place_pose_top = rospy.get_param(rospy.get_name() + '/place_marker_pose')
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient(self.pickup_pose_top, PickUpPoseAction)
self.pick_as.wait_for_server()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient(self.place_pose_top, PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(300)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher(
'/torso_controller/command', JointTrajectory, queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
self.aruco_pose_rcv = False
self.aruco_pose_subs = rospy.Subscriber(self.aruco_pose_top, PoseStamped, self.aruco_pose_cb)
self.pick_g = PickUpPoseGoal()
rospy.loginfo("Done initializing ManipulateAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_and_place_aruco(self, string_operation):
success = False
if string_operation == "pick":
self.prepare_robot_pandp()
rospy.sleep(2.0)
while not rospy.is_shutdown() and self.aruco_pose_rcv == False:
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection...")
rospy.sleep(1.0)
aruco_pose = self.aruco_pose
aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform("base_footprint",
ps.header.frame_id,
rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
rospy.loginfo("Setting cube pose based on Aruco detection")
self.pick_g.object_pose.pose.position = aruco_ps.pose.position
self.pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(self.pick_g))
self.pick_g.object_pose.header.frame_id = 'base_footprint'
self.pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(self.pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(self.pick_g))
self.pick_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
success = False
else:
success = True
self.prepare_robot_nav()
return success
if string_operation == "place":
# Place the object on table in front
rospy.loginfo("Placing aruco marker")
self.place_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place, not trying further")
success = False
else:
success = True
return success
def move_torso(self, string_operation):
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
if string_operation == "lift":
jtp.positions = [0.34]
elif string_operation == "lower":
jtp.positions = [0.15]
else:
return
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
rospy.loginfo("Moving torso " + string_operation)
self.torso_cmd.publish(jt)
def prepare_robot_pandp(self):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'pregrasp'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
rospy.loginfo("Robot prepared.")
def prepare_robot_nav(self):
# Move torso to its maximum height
self.move_torso("lift")
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
def aruco_pose_cb(self, aruco_pose_msg):
self.aruco_pose = aruco_pose_msg
self.aruco_pose_rcv = True
class Arm(object):
"""
A single arm can be either left or right, extends Arms:
Use left or right to get arm while running from the python console
Examples:
>>> left.send_goal(0.265, 1, 0.816, 0, 0, 0, 60)
or Equivalently:
>>> left.send_goal(px=0.265, py=1, pz=0.816, yaw=0, pitch=0, roll=0, time_out=60, pre_grasp=False, frame_id='/amigo/base_link')
#To open left gripper
>>> left.send_gripper_goal_open(10)
"""
def __init__(self, robot_name, side, tf_listener):
self.robot_name = robot_name
self.side = side
if (self.side is "left") or (self.side is "right"):
pass
else:
raise Exception("Side should be either: left or right")
self.tf_listener = tf_listener
self._occupied_by = None
self._operational = True # In simulation, there will be no hardware cb
# Get stuff from the parameter server
self.offset = self.load_param('skills/arm/offset/' + self.side)
self.marker_to_grippoint_offset = self.load_param('skills/arm/offset/marker_to_grippoint')
self.joint_names = self.load_param('skills/arm/joint_names')
self.joint_names = [name + "_" + self.side for name in self.joint_names]
self.torso_joint_names = rospy.get_param('/'+self.robot_name+'/skills/torso/joint_names')
self.default_configurations = self.load_param('skills/arm/default_configurations')
self.default_trajectories = self.load_param('skills/arm/default_trajectories')
# listen to the hardware status to determine if the arm is available
rospy.Subscriber("/amigo/hardware_status", DiagnosticArray, self.cb_hardware_status)
# Init gripper actionlib
self._ac_gripper = SimpleActionClient(
"/" + robot_name + "/" + self.side + "_arm/gripper/action", GripperCommandAction)
# Init graps precompute actionlib
self._ac_grasp_precompute = SimpleActionClient(
"/" + robot_name + "/" + self.side + "_arm/grasp_precompute", GraspPrecomputeAction)
# Init joint trajectory action server
self._ac_joint_traj = SimpleActionClient(
"/" + robot_name + "/body/joint_trajectory_action", FollowJointTrajectoryAction)
# ToDo: don't hardcode? // Comment from @reinzor, this always fails on
# startup of action server, why do we need this, who uses this info?
server_timeout = 0.25
self._ac_gripper_present = self._ac_gripper.wait_for_server(timeout=rospy.Duration(server_timeout))
if not self._ac_gripper_present:
rospy.loginfo("Cannot find gripper {0} server".format(self.side))
self._ac_grasp_precompute_present = self._ac_grasp_precompute.wait_for_server(timeout=rospy.Duration(server_timeout))
if not self._ac_grasp_precompute_present:
rospy.loginfo("Cannot find grasp precompute {0} server".format(self.side))
self._ac_joint_traj_present = self._ac_joint_traj.wait_for_server(timeout=rospy.Duration(server_timeout))
if not self._ac_joint_traj_present:
rospy.loginfo("Cannot find joint trajectory action server {0}".format(self.side))
# Init marker publisher
self._marker_publisher = rospy.Publisher(
"/" + robot_name + "/" + self.side + "_arm/grasp_target",
visualization_msgs.msg.Marker, queue_size=10)
def load_param(self, param_name):
'''
Loads a parameter from the parameter server, namespaced by robot name
'''
return rospy.get_param('/' + self.robot_name + '/' + param_name)
def cancel_goals(self):
""" Cancels the currently active grasp-precompute and joint-trajectory-action goals
"""
self._ac_grasp_precompute.cancel_all_goals()
self._ac_joint_traj.cancel_all_goals()
def close(self):
try:
rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.side))
except AttributeError:
print "Arm cancelling all goals on all arm-related ACs on close. Rospy is already deleted."
self._ac_gripper.cancel_all_goals()
self._ac_grasp_precompute.cancel_all_goals()
self._ac_joint_traj.cancel_all_goals()
@property
def operational(self):
'''
The 'operational' property reflects the current hardware status of the arm.
'''
return self._operational
def cb_hardware_status(self, msg):
'''
hardware_status callback to determine if the arms are operational
'''
diags = [diag for diag in msg.status if diag.name == self.side + '_arm']
if len(diags) == 0:
rospy.logwarn('no diagnostic msg received for the %s arm' % self.side)
elif len(diags) != 1:
rospy.logwarn('multiple diagnostic msgs received for the %s arm' % self.side)
else:
level = diags[0].level
# 0. Stale
# 1. Idle
# 2. Operational
# 3. Homing
# 4. Error
if level != 2:
self._operational = False
else:
self._operational = True
def send_goal(self, px, py, pz, roll, pitch, yaw,
timeout=30,
pre_grasp=False,
frame_id='/base_link',
first_joint_pos_only=False,
allowed_touch_objects=[]):
"""
Send a arm to a goal:
Using a position px, py, pz and orientation roll, pitch, yaw. A time
out time_out. pre_grasp means go to an offset that is normally needed
for things such as grasping. You can also specify the frame_id which
defaults to base_link """
# save the arguments for debugging later
myargs = locals()
# If necessary, prefix frame_id
if frame_id.find(self.robot_name) < 0:
frame_id = "/"+self.robot_name+frame_id
rospy.loginfo("Grasp precompute frame id = {0}".format(frame_id))
# Create goal:
grasp_precompute_goal = GraspPrecomputeGoal()
grasp_precompute_goal.goal.header.frame_id = frame_id
grasp_precompute_goal.goal.header.stamp = rospy.Time.now()
grasp_precompute_goal.PERFORM_PRE_GRASP = pre_grasp
grasp_precompute_goal.FIRST_JOINT_POS_ONLY = first_joint_pos_only
grasp_precompute_goal.allowed_touch_objects = allowed_touch_objects
grasp_precompute_goal.goal.x = px
grasp_precompute_goal.goal.y = py
grasp_precompute_goal.goal.z = pz
grasp_precompute_goal.goal.roll = roll
grasp_precompute_goal.goal.pitch = pitch
grasp_precompute_goal.goal.yaw = yaw
self._publish_marker(grasp_precompute_goal, [1, 0, 0], "grasp_point")
# Add tunable parameters
grasp_precompute_goal.goal.x += self.offset['x']
grasp_precompute_goal.goal.y += self.offset['y']
grasp_precompute_goal.goal.z += self.offset['z']
grasp_precompute_goal.goal.roll += self.offset['roll']
grasp_precompute_goal.goal.pitch += self.offset['pitch']
grasp_precompute_goal.goal.yaw += self.offset['yaw']
# rospy.loginfo("Arm goal: {0}".format(grasp_precompute_goal))
self._publish_marker(grasp_precompute_goal, [0, 1, 0], "grasp_point_corrected")
import time; time.sleep(0.001) # This is necessary: the rtt_actionlib in the hardware seems
# to only have a queue size of 1 and runs at 1000 hz. This
# means that if two goals are send approximately at the same
# time (e.g. an arm goal and a torso goal), one of the two
# goals probably won't make it. This sleep makes sure the
# goals will always arrive in different update hooks in the
# hardware TrajectoryActionLib server.
# Send goal:
if timeout == 0.0:
self._ac_grasp_precompute.send_goal(grasp_precompute_goal)
return True
else:
result = self._ac_grasp_precompute.send_goal_and_wait(
grasp_precompute_goal,
execute_timeout=rospy.Duration(timeout)
)
if result == GoalStatus.SUCCEEDED:
return True
else:
# failure
rospy.logerr('grasp precompute goal failed: \n%s', repr(myargs))
return False
def send_joint_goal(self, configuration, timeout=5.0):
'''
Send a named joint goal (pose) defined in the parameter default_configurations to the arm
'''
if configuration in self.default_configurations:
return self._send_joint_trajectory(
[self.default_configurations[configuration]]
, timeout=rospy.Duration(timeout))
else:
rospy.logwarn('Default configuration {0} does not exist'.format(configuration))
return False
def send_joint_trajectory(self, configuration, timeout=5.0):
'''
Send a named joint trajectory (sequence of poses) defined in the default_trajectories to
the arm
'''
if configuration in self.default_trajectories:
return self._send_joint_trajectory(self.default_trajectories[configuration], timeout=rospy.Duration(timeout))
else:
rospy.logwarn('Default trajectories {0} does not exist'.format(configuration))
return False
def reset(self, timeout=0.0):
'''
Put the arm into the 'reset' pose
'''
return self.send_joint_goal('reset', timeout=timeout)
@property
def occupied_by(self):
'''
The 'occupied_by' property will return the current entity that is in the gripper of this arm.
'''
return self._occupied_by
@occupied_by.setter
def occupied_by(self, value):
self._occupied_by = value
def send_gripper_goal(self, state, timeout=5.0):
'''
Send a GripperCommand to the gripper of this arm and wait for finishing
'''
goal = GripperCommandGoal()
if state == 'open':
goal.command.direction = GripperCommand.OPEN
elif state == 'close':
goal.command.direction = GripperCommand.CLOSE
else:
rospy.logerr('State shoulde be open or close, now it is {0}'.format(state))
return False
self._ac_gripper.send_goal(goal)
if state == 'open':
if self.occupied_by is not None:
rospy.logerr("send_gripper_goal open is called but there is still an entity with id '%s' occupying the gripper, please update the world model and remove this entity" % self.occupied_by.id)
self.occupied_by = None
goal_status = GoalStatus.SUCCEEDED
if timeout != 0.0:
self._ac_gripper.wait_for_result(rospy.Duration(timeout))
goal_status = self._ac_gripper.get_state()
return goal_status == GoalStatus.SUCCEEDED
def handover_to_human(self, timeout=10):
'''
Handover an item from the gripper to a human.
Feels if user slightly pulls or pushes the (item in the) arm. On timeout, it will return False.
'''
succeeded = False
pub = rospy.Publisher('/'+self.robot_name+'/handoverdetector_'+self.side+'/toggle_robot2human', std_msgs.msg.Bool, queue_size=1, latch = True)
pub.publish(std_msgs.msg.Bool(True))
try:
rospy.wait_for_message('/'+self.robot_name+'/handoverdetector_'+self.side+'/result', std_msgs.msg.Bool, timeout)
print '/'+self.robot_name+'/handoverdetector_'+self.side+'/result'
return True
except rospy.ROSException, e:
rospy.logerr(e)
return False
# - section: ''
# key: ''
# expanded: ''
# rawtext:
# text: 'I like talking to people'
# lang_id: 'en_GB'
# speakerName: ''
# wait_before_speaking: 0.0"
if __name__ == '__main__':
rospy.init_node('say_something')
# If the user adds some input, say what he wrote
if len(sys.argv) > 1:
text = ""
for arg in sys.argv[1:]:
text += arg + " "
# If not, just say a sentence
else:
text = "I like talking to people"
rospy.loginfo("I'll say: " + text)
# Connect to the text-to-speech action server
client = SimpleActionClient('/tts', TtsAction)
client.wait_for_server()
# Create a goal to say our sentence
goal = TtsGoal()
goal.rawtext.text = text
goal.rawtext.lang_id = "en_GB"
# Send the goal and wait
client.send_goal_and_wait(goal)
def plate_main():
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
wi = WorldInterface()
psi = get_planning_scene_interface()
detector = TablewareDetection()
plate = wi.collision_object('plate')
good_detection = False
goal = DARRTGoal()
if plate:
rospy.loginfo('Use last detection?')
good_detection = (raw_input() == 'y')
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name='plate',
collision_support_surface_name='serving_table')
while not good_detection:
det = detector.detect_objects(add_table_to_map=False,
add_objects_as_mesh=False, table_name='serving_table')
goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi)
psi.reset()
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
rospy.loginfo('Good detection?')
good_detection = (raw_input() == 'y')
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
return
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.pickup_goal.arm_name = 'right_arm'
goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped,
goal.pickup_goal.target.reference_frame_id)
goal.pickup_goal.lift.desired_distance = 0.3
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
# place_pose_stamped.pose.position.x = 1.3
# place_pose_stamped.pose.position.y = -0.3
# place_pose_stamped.pose.position.z = 1.01
place_pose_stamped.pose.position.x = 0.0
place_pose_stamped.pose.position.y = 0.0
place_pose_stamped.pose.position.z = 0.76
place_pose_stamped.pose.orientation.w = 1.0
# place_pose_stamped.pose.orientation.x = 0.1
# place_pose_stamped.pose.orientation.y = 0.1
# place_pose_stamped.pose.orientation.w = np.sqrt(0.98)
#place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
#place_pose_stamped.pose.position.x -= 0.2
#place_pose_stamped.pose.position.y -= 0.2
#place_pose_stamped.pose.position.z += 0.2
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'dirty_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.place_goal.approach.direction.header.frame_id = wi.world_frame
goal.place_goal.approach.direction.vector.x = np.sqrt(0.18)
goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18)
goal.place_goal.approach.direction.vector.z = -0.8
goal.place_goal.approach.desired_distance = 0.2
goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.19
goal.object_sampling_fraction = 0.7
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
marray = MarkerArray()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'trajectory_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
rospy.loginfo('Successful write')
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
segment = SegmentScene()
res = segment.execute()
if not res: exit(1)
closest_index = res[1][0][0]
grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
grasp_client.wait_for_server()
grasp_goal = GraspObjectGoal()
grasp_goal.category_id = -1
grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
grasp_goal.collision_object_name = res[0].collision_object_names[closest_index]
grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name
result = grasp_client.send_goal_and_wait(grasp_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to grasp')
reset.execute()
exit(1)
lift_client = SimpleActionClient('lift_object', LiftObjectAction)
lift_client.wait_for_server()
lift_goal = LiftObjectGoal()
lift_goal.category_id = 0
lift_goal.graspable_object = res[0].graspable_objects[closest_index]
lift_goal.collision_object_name = res[0].collision_object_names[closest_index]
lift_goal.collision_support_surface_name = res[0].collision_support_surface_name
class AStarSearch(object):
def __init__(self, name):
rospy.loginfo("Starting {}".format(name))
self._as = SimpleActionServer(name,
TopoNavAction,
self.execute_cb,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient("/waypoint_nav_node",
WayPointNavAction)
rospy.loginfo("Waiting for nav server")
self.client.wait_for_server()
self.nodes = set()
self.edges = defaultdict(list)
self.distances = {}
with open(rospy.get_param("~waypoint_yaml"), 'r') as f:
self.waypointInfo = yaml.load(f)
for waypoint, info in self.waypointInfo.items():
self.add_node(waypoint)
for edge in info["edges"]:
self.add_edge(waypoint, edge, 1.0)
self.waypointInfo[waypoint]["position"]["x"] *= 0.555
self.waypointInfo[waypoint]["position"]["y"] *= 0.555
self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped,
self.pose_cb)
self._as.start()
rospy.loginfo("Started {}".format(name))
def preempt_cb(self, *args):
self.client.cancel_all_goals()
def add_node(self, value):
self.nodes.add(value)
def add_edge(self, from_node, to_node, distance):
self.edges[from_node].append(to_node)
# self.edges[to_node].append(from_node)
self.distances[(from_node, to_node)] = distance
def pose_cb(self, msg):
closest = (None, None)
for k, v in self.waypointInfo.items():
dist = self.get_dist(msg, v["position"])
if closest[0] is None or dist < closest[0]:
closest = (dist, k)
self.closest = closest[1]
def get_dist(self, pose1, position):
return np.sqrt(
np.power(pose1.pose.position.x - position["x"], 2) +
np.power(pose1.pose.position.y - position["y"], 2))
def execute_cb(self, goal):
path = self.shortest_path(self.closest, goal.waypoint)[1]
print path
for p in path:
if not self._as.is_preempt_requested():
rospy.loginfo("Going to waypoint: {}".format(p))
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = "map"
target.pose.position.x = self.waypointInfo[p]["position"]["x"]
target.pose.position.y = self.waypointInfo[p]["position"]["y"]
target.pose.orientation.x = self.waypointInfo[p][
"orientation"]["x"]
target.pose.orientation.y = self.waypointInfo[p][
"orientation"]["y"]
target.pose.orientation.z = self.waypointInfo[p][
"orientation"]["z"]
target.pose.orientation.w = self.waypointInfo[p][
"orientation"]["w"]
self.client.send_goal_and_wait(WayPointNavGoal(target))
if not self._as.is_preempt_requested():
self._as.set_succeeded(TopoNavResult())
else:
self._as.set_preempted()
def dijkstra(self, initial):
visited = {initial: 0}
path = {}
nodes = set(self.nodes)
while nodes:
min_node = None
for node in nodes:
if node in visited:
if min_node is None:
min_node = node
elif visited[node] < visited[min_node]:
min_node = node
if min_node is None:
break
nodes.remove(min_node)
current_weight = visited[min_node]
for edge in self.edges[min_node]:
weight = current_weight + self.distances[(min_node, edge)]
if edge not in visited or weight < visited[edge]:
visited[edge] = weight
path[edge] = min_node
return visited, path
def shortest_path(self, origin, destination):
visited, paths = self.dijkstra(origin)
full_path = deque()
_destination = paths[destination]
while _destination != origin:
full_path.appendleft(_destination)
_destination = paths[_destination]
full_path.appendleft(origin)
full_path.append(destination)
return visited[destination], list(full_path)
class PigeonPickAndPlace:
#Class constructor (parecido com java, inicializa o que foi instanciado)
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
def __del__(self):
# Clean the scene
self._scene.remove_world_object(self._grasp_object_name)
def _add_object_grasp(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame() #pose
p.header.stamp = rospy.Time.now()
rospy.loginfo(self._robot.get_planning_frame())
p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
#p.pose.orientation.x = 0.0028925
#p.pose.orientation.y = -0.0019311
#p.pose.orientation.z = -0.71058
#p.pose.orientation.w = 0.70361
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/,
# using the measure tape tool from meshlab.
# The box is the bounding box of the lego cylinder.
# The values are taken from the cylinder base diameter and height.
# the size is length (x),thickness(y),height(z)
# I don't know if the detector return this values of object
self._scene.add_box(name, p, (0.032, 0.016, 0.020))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
#rospy.logerr('Generated: %f grasps:' % grasps.size)
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
#goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_object_grasp, 0.016)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
class MoveBase(smach.State):
serviceProxiesDict = {
"hpp": {
"target": {
"get_base_pose_at_param": [GetBasePoseAtParam],
}
},
}
def __init__(self, status):
super(MoveBase, self).__init__(
outcomes=["succeeded", "preempted"],
input_keys=[
"pathId",
"currentSection",
"times",
],
)
self.status = status
self.serviceProxies = ros_tools.createServiceProxies(
"", MoveBase.serviceProxiesDict)
self.axclient = SimpleActionClient("/move_base", MoveBaseAction)
self.move_base_methods = {
"sequential": self.sequential,
"full_path": self.full_path,
}
for name, default in (
("move_base_method", "sequential"),
("move_base_sequential/step", 0.1),
("move_base_sequential/distance_thr", 0.3),
):
if not rospy.has_param(name):
rospy.set_param(name, default)
def execute(self, userdata):
method = rospy.get_param('move_base_method', 'sequential')
if method not in self.move_base_methods:
rospy.logwarn(
"Method {} not available. Available methods are {}. Falling back to sequential"
.format(method, self.move_base_methods.keys()))
method = sequential
return self.move_base_methods[method](userdata)
def full_path(self, userdata):
pathId = userdata.pathId
start = userdata.times[userdata.currentSection]
end = userdata.times[userdata.currentSection + 1]
rsp = self.serviceProxies["hpp"]["target"]["get_base_pose_at_param"](
pathId, end)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "world"
goal.target_pose.pose = rsp.pose
result = self.axclient.send_goal_and_wait(goal)
if result == GoalStatus.SUCCEEDED:
return "succeeded"
rospy.logerr("Failed to move base. Result is {}".format(
_demangle_status(result)))
return "preempted"
def sequential(self, userdata):
pathId = userdata.pathId
start = userdata.times[userdata.currentSection]
end = userdata.times[userdata.currentSection + 1]
client = _Feedback(
self.axclient,
lambda t: self.serviceProxies["hpp"]["target"][
"get_base_pose_at_param"](pathId, t),
start,
end,
step=rospy.get_param("move_base_sequential/step", 0.1),
distance_thr=rospy.get_param("move_base_sequential/distance_thr",
0.03),
)
rate = rospy.Rate(10)
while not client.path_completed:
if client.abort: return "preempted"
rate.sleep()
return "succeeded"
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.head_cmd = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
# /arm_controller/command (trajectory_msgs/JointTrajectory)
self.trajectory_cmd = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=1)
self.gripper_cmd = rospy.Publisher('/gripper_controller/command',
JointTrajectory,
queue_size=1)
self.hey5_cmd = rospy.Publisher('/hand_controller/command',
JointTrajectory,
queue_size=1)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# get aruco pose here: from marker
aruco_pose = rospy.wait_for_message('/cylinder_detector/cylinder_pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
print("aruco pose: ", aruco_pose)
# # manually input aruco pose
# aruco_pose = PoseStamped()
# aruco_pose.header.frame_id = 'base_footprint'
# # # original aruco pose
# # aruco_pose.pose.position.x = 0.528450879403
# # aruco_pose.pose.position.y = -0.0486955713869
# # aruco_pose.pose.position.z = 0.922035729832
# # 0.545863 0.049354 0.724881
# aruco_pose.pose.position.x = 0.545863
# aruco_pose.pose.position.y = 0.049354
# aruco_pose.pose.position.z = 0.724881 + 0.05
# aruco_pose.pose.orientation.x = 0.5
# aruco_pose.pose.orientation.y = 0.5
# aruco_pose.pose.orientation.z = 0.5
# aruco_pose.pose.orientation.w = 0.5
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
# save the original pose ##########
original_pose = deepcopy(aruco_pose)
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
rospy.sleep(2.0)
# Move torso to its maximum height
self.lift_torso()
rospy.sleep(2.0)
# aruco_pose.pose.position.x = 0.733477 - 0.2
# aruco_pose.pose.position.y = 0.057126 + 0.07
# aruco_pose.pose.position.z += 0.2 # 0.2 is the allowance for pouring
pour_pose = deepcopy(aruco_pose)
pour_pose.pose.position.x = 0.733477 - 0.2
pour_pose.pose.position.y = 0.057126 + 0.07
pour_pose.pose.position.z += 0.2
"""
The following are good good_candidates for pose selection (in eular angles):
[[0, 0, 0], [0, 0, 270], [0, 180, 0], [0, 180, 180] - no, [0, 180, 270], [0, 270, 0], [0, 270, 180], [0, 270, 270]]
"""
# some more successful grasp candidates
# x, y, z, w = eular_to_q(0,270,180)
# x, y, z, w = eular_to_q(0,270,270)
# get pick_pose:
# x, y, z, w = get_pose(args)
pour_pose.pose.orientation.x = -0.5
pour_pose.pose.orientation.y = 0
pour_pose.pose.orientation.z = 0
pour_pose.pose.orientation.w = 1.2
success = False
while success == False:
rospy.sleep(2.0)
# x_e, y_e, z_e = random.choice([(0,270,180), (0,270,270)])
# x, y, z, w = eular_to_q(x_e, y_e, z_e)
# pour_pose.pose.orientation.x = x
# pour_pose.pose.orientation.y = y
# pour_pose.pose.orientation.z = z
# pour_pose.pose.orientation.w = w
result = cartesian_move_to(pour_pose, True)
rospy.loginfo("Success of trajectory: " + str(result))
# define a goal tolerance for replanning and manipulation
x_arm, y_arm, z_arm = arm_pose()
x_aim = pour_pose.pose.position.x
y_aim = pour_pose.pose.position.y
z_aim = pour_pose.pose.position.z
goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim,
z_aim)
rospy.loginfo("Deviation from target pose: " +
str(goal_deviation))
# pour_pose.pose.position.x -= 0.0001
# pour_pose.pose.position.y -= 0.0001
if result > 0.9 and goal_deviation < 0.05:
success = True
# x, y, z = q_to_eular(-0.5, 0, 0, 1.2)
# rospy.loginfo("End effector eular angles: " + str([x,y,z]))
rospy.sleep(3.0)
# pour liquid
self.turn_wrist()
rospy.sleep(5.0)
# turn wrist back to original pose
self.turn_wrist(pour=False)
rospy.sleep(3.0)
pour_pose.pose.position.x += 0.2
pour_pose.pose.position.y += 0.2
success = False
while success == False:
# x_e, y_e, z_e = random.choice([(0,270,180), (0,270,270)])
# x, y, z, w = eular_to_q(x_e, y_e, z_e)
# pour_pose.pose.orientation.x = x
# pour_pose.pose.orientation.y = y
# pour_pose.pose.orientation.z = z
# pour_pose.pose.orientation.w = w
result = cartesian_move_to(pour_pose, True)
rospy.loginfo("Success of trajectory: " + str(result))
# define a goal tolerance for replanning and manipulation
x_arm, y_arm, z_arm = arm_pose()
x_aim = pour_pose.pose.position.x
y_aim = pour_pose.pose.position.y
z_aim = pour_pose.pose.position.z
goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim,
z_aim)
rospy.loginfo("Deviation from target pose: " +
str(goal_deviation))
pour_pose.pose.position.x -= 0.0001
pour_pose.pose.position.y -= 0.0001
if result > 0.9 and goal_deviation < 0.05:
success = True
rospy.sleep(3.0)
# # return bottle to original pose
# x, y, z, w = eular_to_q(0,270,180)
# # x, y, z, w = eular_to_q(0,270,270)
# original_pose.pose.position.z = pour_pose.pose.position.z
# original_pose.pose.orientation.x = -0.5
# original_pose.pose.orientation.y = 0
# original_pose.pose.orientation.z = 0
# original_pose.pose.orientation.w = 1.2
# success = False
# while success==False:
# result = cartesian_move_to(original_pose, True)
# rospy.loginfo("success of trajectory: "+str(result))
# # define a goal tolerance for replanning and manipulation
# x_arm, y_arm, z_arm = arm_pose()
# x_aim = original_pose.pose.position.x
# y_aim = original_pose.pose.position.y
# z_aim = original_pose.pose.position.z
# goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim, z_aim)
# rospy.loginfo("Deviation from target pose: "+str(goal_deviation))
# original_pose.pose.position.x -= 0.01
# original_pose.pose.position.y -= 0.01
# if result > 0.9 and goal_deviation < 0.1:
# success = True
# rospy.loginfo("Gonna place near where it was")
# ps = PoseStamped()
# ps.pose.position = original_pose.pose.position
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", original_pose.header.frame_id)
# ps.header.frame_id = original_pose.header.frame_id
# transform_ok = False
# while not transform_ok and not rospy.is_shutdown():
# try:
# transform = self.tfBuffer.lookup_transform("base_footprint",
# ps.header.frame_id,
# rospy.Time(0))
# aruco_ps = do_transform_pose(ps, transform)
# transform_ok = True
# except tf2_ros.ExtrapolationException as e:
# rospy.logwarn(
# "Exception on transforming point... trying again \n(" +
# str(e) + ")")
# rospy.sleep(0.01)
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", original_pose.header.frame_id)
# pick_g = PickUpPoseGoal()
# arm=MoveGroupCommander('arm')
# arm.allow_replanning(True)
# end_effector_link=arm.get_end_effector_link()
# arm.set_goal_position_tolerance(0.03)
# arm.set_goal_orientation_tolerance(0.025)
# arm.allow_replanning(True)
# reference_frame='base_footprint'
# arm.set_pose_reference_frame(reference_frame)
# arm.set_planning_time(5)
# rospy.sleep(2)
# start_pose=arm.get_current_pose(end_effector_link).pose
# rospy.loginfo("End effector start pose: " + str(start_pose))
# x = start_pose.orientation.x
# y = start_pose.orientation.y
# z = start_pose.orientation.z
# w = start_pose.orientation.w
# x, y, z = q_to_eular(x, y, z, w)
# rospy.loginfo("End effector eular angles: " + str([x,y,z]))
# # Raise arm
# rospy.loginfo("Moving arm to a safe pose")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Raise object done.")
# Place the object back to its position
rospy.loginfo("Gonna place near where it was ...........")
# pick_g.object_pose.pose.position.z += 0.05
pick_g.object_pose.pose.position.z += 0.05
pick_g.object_pose.pose.position.y += 0.2
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
def lift_torso(self):
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.34]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def turn_wrist(self, pour=True):
wrist_state = -2.0
joint_state = rospy.wait_for_message('/joint_states', JointState)
j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7]
rospy.loginfo("Starting wrist state: " + str(j7))
max_try = 0
turn_target = 1.5
if pour:
rospy.loginfo("Turning Arm Forward")
max_wrist_state = j7 + turn_target
while j7 < max_wrist_state and max_try < 20:
jt = JointTrajectory()
jt.joint_names = [
'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
'arm_5_joint', 'arm_6_joint', 'arm_7_joint'
]
jtp = JointTrajectoryPoint()
jtp.positions = [j1, j2, j3, j4, j5, j6, j7 + 0.5]
jtp.time_from_start = rospy.Duration(2)
jt.points.append(jtp)
self.trajectory_cmd.publish(jt)
rospy.sleep(0.25)
# check the wrist joint state
joint_state = rospy.wait_for_message('/joint_states',
JointState)
j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7]
rospy.loginfo("Current wrist state: " + str(j7))
max_try += 1
rospy.loginfo("Done.")
else:
rospy.loginfo("Turning Arm Backward")
max_wrist_state = j7 - turn_target
# 2.09462737056
# while j7 > max_wrist_state and max_try < 20:
while j7 > max_wrist_state and max_try < 20:
jt = JointTrajectory()
jt.joint_names = [
'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
'arm_5_joint', 'arm_6_joint', 'arm_7_joint'
]
jtp = JointTrajectoryPoint()
jtp.positions = [j1, j2, j3, j4, j5, j6, j7 - 0.5]
jtp.time_from_start = rospy.Duration(2)
jt.points.append(jtp)
self.trajectory_cmd.publish(jt)
rospy.sleep(0.25)
# check the wrist joint state
joint_state = rospy.wait_for_message('/joint_states',
JointState)
j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7]
rospy.loginfo("Current wrist state: " + str(j7))
max_try += 1
rospy.loginfo("Done.")
def lower_head(self):
rospy.loginfo("Moving head down")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -1.0]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def prepare_robot(self):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'arm_raise'
# pmg.motion_name = 'pregrasp'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
self.lower_head()
rospy.loginfo("Robot prepared.")
def open_gripper(self):
rospy.loginfo("Opening Gripper")
jt = JointTrajectory()
jt.joint_names = [
'gripper_left_finger_joint', 'gripper_right_finger_joint'
]
jtp = JointTrajectoryPoint()
jtp.positions = [0.044, 0.044]
jtp.time_from_start = rospy.Duration(0.5)
jt.points.append(jtp)
self.gripper_cmd.publish(jt)
rospy.loginfo("Done.")
def open_hey5(self):
rospy.loginfo("Opening Gripper")
jt = JointTrajectory()
jt.joint_names = [
'hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint'
]
jtp = JointTrajectoryPoint()
jtp.positions = [-1.0, -1.0, -1.0]
jtp.time_from_start = rospy.Duration(0.1)
jt.points.append(jtp)
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.hey5_cmd.publish(jt)
rospy.loginfo("Done.")
def close_hey5(self):
rospy.loginfo("Closing Gripper")
jt = JointTrajectory()
jt.joint_names = [
'hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint'
]
jtp = JointTrajectoryPoint()
jtp.positions = [2.37, 0.0, 0.0]
jtp.time_from_start = rospy.Duration(0.1)
jt.points.append(jtp)
jtp = JointTrajectoryPoint()
jtp.positions = [6.2, 6.8, 9.2]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.hey5_cmd.publish(jt)
rospy.loginfo("Done.")
segment = SegmentScene()
res = segment.execute()
if not res: exit(1)
rospy.loginfo('Scene segmented')
closest_index = res[1][0][0]
grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
grasp_client.wait_for_server()
grasp_goal = GraspObjectGoal()
grasp_goal.category_id = -1
grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
grasp_goal.collision_object_name = res[0].collision_object_names[closest_index]
grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name
result = grasp_client.send_goal_and_wait(grasp_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to grasp')
reset.execute()
exit(1)
rospy.loginfo('Grasped an object')
lift_client = SimpleActionClient('lift_object', LiftObjectAction)
lift_client.wait_for_server()
lift_goal = LiftObjectGoal()
lift_goal.category_id = 0
lift_goal.graspable_object = res[0].graspable_objects[closest_index]
lift_goal.collision_object_name = res[0].collision_object_names[closest_index]
lift_goal.collision_support_surface_name = res[0].collision_support_surface_name
#!/usr/bin/env python
import roslib; roslib.load_manifest('w2_object_manipulation_launch')
import rospy
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from w2_object_manipulation_launch.msg import DoInfomaxAction
from w2_object_manipulation_launch.msg import DoInfomaxGoal
if __name__ == '__main__':
rospy.init_node('infomax_action_sm_tester')
client = SimpleActionClient('do_infomax', DoInfomaxAction)
client.wait_for_server()
goal = DoInfomaxGoal()
goal.graspable_object_name = 'graspable_object_1'
result = client.send_goal_and_wait(goal)
if result == GoalStatus.SUCCEEDED:
rospy.loginfo('infomax action state machine successfully completed execution')
else:
rospy.loginfo('failed to infomax an object')
class Gripper():
""" Wrapper for all the action client stuff """
def __init__(self):
""" Instantiate action clients and wait for communication with servers """
self.client_gripper = SimpleActionClient("r_gripper_sensor_controller/gripper_action", Pr2GripperCommandAction)
self.client_contact = SimpleActionClient("r_gripper_sensor_controller/find_contact", PR2GripperFindContactAction)
self.client_force = SimpleActionClient("r_gripper_sensor_controller/force_servo", PR2GripperForceServoAction)
self.client_gripper.wait_for_server(rospy.Duration(10.0))
self.client_contact.wait_for_server(rospy.Duration(10.0))
self.client_force.wait_for_server(rospy.Duration(10.0))
rospy.loginfo('All servers ready!')
rospy.Subscriber('r_gripper_pressure_balance', String, self.store_balance)
def store_balance(self, balance):
""" Stores gripper balance. """
self.balance = balance.data
def open(self):
""" Open the gripper """
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = 0.09
goal_open.command.max_effort = -1.0
rospy.loginfo("Sending goal: OPEN")
self.client_gripper.send_goal_and_wait(goal_open)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper opened!')
else:
rospy.loginfo('FAILURE. The gripper failed to open.')
def open_manual(self, position):
""" Open the gripper """
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = position
goal_open.command.max_effort = -1.0
rospy.loginfo("Sending goal: OPEN")
self.client_gripper.send_goal_and_wait(goal_open)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper opened!')
else:
rospy.loginfo('FAILURE. The gripper failed to open.')
def close(self):
""" Close the gripper """
goal_close = Pr2GripperCommandGoal()
goal_close.command.position = 0.00
goal_close.command.max_effort = 20.0
rospy.loginfo("Sending goal: CLOSE")
self.client_gripper.send_goal_and_wait(goal_close)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper closed!')
else:
rospy.loginfo('FAILURE. The gripper failed to close.')
def hold(self, holdForce):
""" Hold something with a specified grip force """
goal_squeeze = PR2GripperForceServoGoal()
goal_squeeze.command.fingertip_force = holdForce
rospy.loginfo("Sending goal: HOLD with {0}N of force.".format(holdForce))
self.client_force.send_goal_and_wait( goal_squeeze )
if (self.client_force.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. Stable force achieved!')
else:
rospy.loginfo('FAILURE. Stable force was NOT achieved.')
def findTwoContacts(self):
""" Find two contacts and go into force control mode """
goal_contact = PR2GripperFindContactGoal()
goal_contact.command.contact_conditions = goal_contact.command.BOTH # close until both fingers contact
goal_contact.command.zero_fingertip_sensors = True # zero fingertip sensors before moving
rospy.loginfo("Sending goal: FIND TWO CONTACTS")
self.client_contact.send_goal_and_wait(goal_contact)
if (self.client_contact.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. Contact found at left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_contact,
self.client_contact.get_result().data.right_fingertip_pad_contact))
rospy.loginfo('Contact force for left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_force,
self.client_contact.get_result().data.right_fingertip_pad_force))
else:
rospy.loginfo('FAILURE. No contact found or force not able to be maintained')
if __name__ == '__main__':
rospy.init_node('test_play_motion_from_tts_feedback_node')
ac = SimpleActionClient(TTS_AS, TtsAction)
rospy.loginfo("Trying to connect to: " + TTS_AS)
connecting_tries = 0
while not ac.wait_for_server(rospy.Duration(2.0)) and connecting_tries < 5:
rospy.logwarn("Retrying connecting to " + TTS_AS)
connecting_tries += 1
if connecting_tries >= 5:
rospy.logerr("Couldn't connect to " + TTS_AS)
exit(0)
tg = TtsGoal()
tg.rawtext.text = 'I say yes <mark name="doTrick trickName=yes"/> and move my head'
tg.rawtext.lang_id = "en_US"
rospy.loginfo("Sending goal: " + str(tg))
ac.send_goal_and_wait(tg)
rospy.loginfo("Goal sent!")
# text:
# section: ''
# key: ''
# lang_id: ''
# arguments: []
# rawtext:
# text: 'I say yes <mark name="doTrick trickName=yes"/> and move my head'
# lang_id: 'en_US'
# speakerName: ''
# wait_before_speaking: 0.0
class GiskardWrapper(object):
def __init__(self, node_name=u'giskard'):
giskard_topic = u'{}/command'.format(node_name)
if giskard_topic is not None:
self._client = SimpleActionClient(giskard_topic, MoveAction)
self._update_world_srv = rospy.ServiceProxy(
u'{}/update_world'.format(node_name), UpdateWorld)
self._get_object_names_srv = rospy.ServiceProxy(
u'{}/get_object_names'.format(node_name), GetObjectNames)
self._get_object_info_srv = rospy.ServiceProxy(
u'{}/get_object_info'.format(node_name), GetObjectInfo)
self._update_rviz_markers_srv = rospy.ServiceProxy(
u'{}/update_rviz_markers'.format(node_name), UpdateRvizMarkers)
self._get_attached_objects_srv = rospy.ServiceProxy(
u'{}/get_attached_objects'.format(node_name),
GetAttachedObjects)
self._marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(node_name))
self._client.wait_for_server()
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self._object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
"""
:rtype: str
"""
return self.robot_urdf.get_name()
def get_root(self):
"""
Returns the name of the robot's root link
:rtype: str
"""
return self.robot_urdf.get_root()
def get_robot_links(self):
"""
Returns a list of the robots links
:rtype: dict
"""
return self.robot_urdf.get_link_names()
def get_joint_states(self, topic=u'joint_states', timeout=1):
"""
Returns a dictionary of all joints (key) and their position (value)
:param topic: joint_state topic
:param timeout: duration to wait for JointState msg
:return: OrderedDict[str, float]
"""
try:
msg = rospy.wait_for_message(topic, JointState,
rospy.Duration(timeout))
return to_joint_state_position_dict(msg)
except rospy.ROSException:
rospy.logwarn("get_joint_states: wait_for_message timeout")
return {}
def set_cart_goal(self,
root_link,
tip_link,
goal_pose,
max_linear_velocity=None,
max_angular_velocity=None,
weight=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal pose
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose
:type goal_pose: PoseStamped
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_translation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_linear_velocity,
weight=weight)
self.set_rotation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_angular_velocity,
weight=weight)
def set_translation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal position
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, orientation will be ignored
:type goal_pose: PoseStamped
:param max_velocity: m/s, default 0.1
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal orientation
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, position will be ignored
:type goal_pose: PoseStamped
:param max_velocity: rad/s, default 0.5
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, goal_state, weight=None, max_velocity=None):
"""
This goal will move the robots joint to the desired configuration.
:param goal_state: Can either be a joint state messages or a dict mapping joint name to position.
:type goal_state: Union[JointState, dict]
:param weight: default WEIGHT_BELOW_CA
:type weight: float
:param max_velocity: default is the default of the added joint goals
:type max_velocity: float
"""
if weight is None and max_velocity is None:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
constraint.goal_state = goal_state
else:
for joint_name, joint_position in goal_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
self.cmd_seq[-1].joint_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
goal_state = goal_state
else:
goal_state2 = JointState()
for joint_name, joint_position in goal_state.items():
goal_state2.name.append(joint_name)
goal_state2.position.append(joint_position)
goal_state = goal_state2
params = {}
params[u'goal_state'] = convert_ros_message_to_dictionary(
goal_state)
if weight is not None:
params[u'weight'] = weight
if max_velocity is not None:
params[u'max_velocity'] = max_velocity
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self,
tip_link,
tip_normal,
root_link=None,
root_normal=None,
max_angular_velocity=None,
weight=WEIGHT_ABOVE_CA):
"""
This Goal will use the kinematic chain between tip and root normal to align both
:param root_link: name of the root link for the kinematic chain, default robot root link
:type root_link: str
:param tip_link: name of the tip link for the kinematic chain
:type tip_link: str
:param tip_normal: normal at the tip of the kin chain, default is z axis of robot root link
:type tip_normal: Vector3Stamped
:param root_normal: normal at the root of the kin chain
:type root_normal: Vector3Stamped
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
if root_link is None:
root_link = self.get_root()
if root_normal is None:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
params = {
u'tip_link': tip_link,
u'tip_normal': tip_normal,
u'root_link': root_link,
u'root_normal': root_normal
}
if weight is not None:
params[u'weight'] = weight
if max_angular_velocity is not None:
params[u'max_angular_velocity'] = max_angular_velocity
self.set_json_goal(u'AlignPlanes', **params)
def avoid_joint_limits(self, percentage=15, weight=WEIGHT_BELOW_CA):
"""
This goal will push joints away from their position limits
:param percentage: default 15, if limits are 0-100, the constraint will push into the 15-85 range
:type percentage: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
self.set_json_goal(u'AvoidJointLimits',
percentage=percentage,
weight=weight)
def limit_cartesian_velocity(self,
root_link,
tip_link,
weight=WEIGHT_ABOVE_CA,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
hard=True):
"""
This goal will limit the cartesian velocity of the tip link relative to root link
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param hard: default True, will turn this into a hard constraint, that will always be satisfied, can could
make some goal combination infeasible
:type hard: bool
"""
self.set_json_goal(u'CartesianVelocityLimit',
root_link=root_link,
tip_link=tip_link,
weight=weight,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
hard=hard)
def grasp_bar(self,
root_link,
tip_link,
tip_grasp_axis,
bar_center,
bar_axis,
bar_length,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
weight=WEIGHT_ABOVE_CA):
"""
This goal can be used to grasp bars. It's like a cartesian goal with some freedom along one axis.
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param tip_grasp_axis: this axis of the tip will be aligned with bar_axis
:type tip_grasp_axis: Vector3Stamped
:param bar_center: center of the bar
:type bar_center: PointStamped
:param bar_axis: tip_grasp_axis will be aligned with this vector
:type bar_axis: Vector3Stamped
:param bar_length: length of the bar
:type bar_length: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_json_goal(u'GraspBar',
root_link=root_link,
tip_link=tip_link,
tip_grasp_axis=tip_grasp_axis,
bar_center=bar_center,
bar_axis=bar_axis,
bar_length=bar_length,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
weight=weight)
def set_pull_door_goal(self,
tip_link,
object_name_prefix,
object_link_name,
angle_goal,
weight=WEIGHT_ABOVE_CA):
"""
:type tip_link: str
:param tip_link: tip of manipulator (gripper) which is used
:type object_name_prefix: object name link prefix
:param object_name_prefix: string
:type object_link_name str
:param object_link_name name of the object link name
:type object_link_name str
:param object_link_name handle to grasp
:type angle_goal: float
:param angle_goal: how far to open
:type weight float
:param weight Default = WEIGHT_ABOVE_CA
"""
self.set_json_goal(u'OpenDoor',
tip_link=tip_link,
object_name=object_name_prefix,
object_link_name=object_link_name,
angle_goal=angle_goal,
weight=weight)
def update_god_map(self, updates):
"""
don't use, it's only for hacks :)
"""
self.set_json_goal(u'UpdateGodMap', updates=updates)
def pointing(self,
tip_link,
goal_point,
root_link=None,
pointing_axis=None,
weight=None):
"""
Uses the kinematic chain from root_link to tip_link to move the pointing axis, such that it points to the goal point.
:param tip_link: name of the tip of the kin chain
:type tip_link: str
:param goal_point: where the pointing_axis will point towards
:type goal_point: PointStamped
:param root_link: name of the root of the kin chain
:type root_link: str
:param pointing_axis: default is z axis, this axis will point towards the goal_point
:type pointing_axis: Vector3Stamped
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
kwargs = {u'tip_link': tip_link, u'goal_point': goal_point}
if root_link is not None:
kwargs[u'root_link'] = root_link
if pointing_axis is not None:
kwargs[u'pointing_axis'] = pointing_axis
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = goal_point
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
"""
Set a goal for any of the goals defined in Constraints.py
:param constraint_type: Name of the Goal
:type constraint_type: str
:param kwargs: maps constraint parameter names to values. Values should be float, str or ros messages.
:type kwargs: dict
"""
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
"""
Adds collision entries to the current goal
:param collisions: list of CollisionEntry
:type collisions: list
"""
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self,
min_dist,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
"""
Allows the collision with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_self_collision(self):
"""
Avoid collisions with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal. The distance will override anything from the config file.
If you don't want to override the distance, don't call this function. Avoid all is the default, if you don't
add any collision entries.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self):
"""
Adds another command to the goal sequence. Any set goal calls will be added the this new goal.
This is used, if you want Giskard to plan multiple goals in succession.
"""
move_cmd = MoveCmd()
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_AND_EXECUTE, wait)
def check_reachability(self, wait=True):
"""
Not implemented
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.CHECK_REACHABILITY, wait)
def plan(self, wait=True):
"""
Plans, but doesn't execute the goal. Useful, if you just want to look at the planning ghost.
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_ONLY, wait)
def send_goal(self, goal_type, wait=True):
goal = self._get_goal()
goal.type = goal_type
if wait:
self._client.send_goal_and_wait(goal)
return self._client.get_result()
else:
self._client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
"""
Stops any goal that Giskard is processing.
"""
self._client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self._client.wait_for_result(timeout)
return self._client.get_result()
def clear_world(self):
"""
Removes any objects and attached objects from Giskard's world and reverts the robots urdf to what it got from
the parameter server.
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(),
False, PoseStamped())
return self._update_world_srv.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False,
PoseStamped())
return self._update_world_srv.call(req)
def add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: (x length, y length, z length) in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self._update_world_srv.call(req)
def add_sphere(self,
name=u'sphere',
size=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: radius in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_mesh(self,
name=u'mesh',
mesh=u'',
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param mesh: path to the meshes location. e.g. package://giskardpy/test/urdfs/meshes/bowl_21.obj
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param height: in m
:type height: float
:param radius: in m
:type radius: float
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions = [0, 0]
object.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = height
object.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS] = radius
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
pose=None):
"""
Add a box to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:param pose: pose of the box
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self._update_world_srv.call(req)
def attach_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=None,
position=None,
orientation=None):
"""
Add a cylinder to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type height: int
:param height: height of the cylinder. Default = 1
:type radius: int
:param radius: radius of the cylinder. Default = 1
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self._update_world_srv.call(req)
def attach_object(self, name, link_frame_id):
"""
Attach an already existing object at link_frame_id of the robot.
:type name: str
:param link_frame_id: name of a robot link
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self._update_world_srv.call(req)
def detach_object(self, object_name):
"""
Detach an object from the robot and add it back to the world.
Careful though, you could amputate an arm be accident!
:type object_name: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self._update_world_srv.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
"""
Adds a urdf to the world
:param name: name it will have in the world
:type name: str
:param urdf: urdf as string, no path
:type urdf: str
:type pose: PoseStamped
:param js_topic: Giskard will listen on that topic for joint states and update the urdf accordingly
:type js_topic: str
:param set_js_topic: A topic that the python wrapper will use to set the urdf joint state.
If None, set_js_topic == js_topic
:type set_js_topic: str
:return: UpdateWorldResponse
"""
if set_js_topic is None:
set_js_topic = js_topic
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self._object_js_topics[name] = rospy.Publisher(set_js_topic,
JointState,
queue_size=10)
return self._update_world_srv.call(req)
def set_object_joint_state(self, object_name, joint_states):
"""
:type object_name: str
:param joint_states: joint state message or a dict that maps joint name to position
:type joint_states: Union[JointState, dict]
:return: UpdateWorldResponse
"""
if isinstance(joint_states, dict):
joint_states = position_dict_to_joint_states(joint_states)
self._object_js_topics[object_name].publish(joint_states)
def get_object_names(self):
"""
returns the names of every object in the world
:rtype: GetObjectNamesResponse
"""
return self._get_object_names_srv()
def get_object_info(self, name):
"""
returns the joint state, joint state topic and pose of the object with the given name
:type name: str
:rtype: GetObjectInfoResponse
"""
return self._get_object_info_srv(name)
def update_rviz_markers(self, object_names):
"""
republishes visualization markers for rviz
:type object_names: list
:rtype: UpdateRvizMarkersResponse
"""
return self._update_rviz_markers_srv(object_names)
def get_attached_objects(self):
"""
returns a list of all objects that are attached to the robot and the respective attachement points
:rtype: GetAttachedObjectsResponse
"""
return self._get_attached_objects_srv()
class ChipBehaviour(object):
def __init__(self):
rospy.loginfo("Initializing ChipBehaviour")
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm_torso")
self.pose_pub = rospy.Publisher('/arm_pointing_pose',
geometry_msgs.msg.PoseStamped,
queue_size=1)
self.tts_ac = SimpleActionClient('/tts', TtsAction)
self.tts_ac.wait_for_server()
self.point_head_ac = SimpleActionClient('/head_controller/point_head_action',
PointHeadAction)
self.point_head_ac.wait_for_server(rospy.Duration(10.0))
self.faces_sub = rospy.Subscriber('/pal_face/faces',
FaceDetections,
self.faces_cb,
queue_size=1)
def point_at(self, x, y, z):
# Get shoulder pose
shoulder_pose = geometry_msgs.msg.Pose()
shoulder_pose.position.x = -0.21
shoulder_pose.position.y = -0.22
shoulder_pose.position.z = 1.28
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.y = x - 0.4
max_z = 3.0
if z > max_z:
normalized_z = 1.0
elif z < 0.0:
normalized_z = 0.0
else:
normalized_z = z / 3.0
normalized_z = normalized_z / 3.0
pose_target.position.x = normalized_z - 0.05
pose_target.position.z = 1.3
rospy.loginfo("Sending arm to: " + str(pose_target))
ps = geometry_msgs.msg.PoseStamped()
ps.header.frame_id = '/odom'
ps.pose = pose_target
self.pose_pub.publish(ps)
self.group.set_pose_target(pose_target)
plan1 = self.group.plan()
self.group.go(wait=True)
def faces_cb(self, msg):
# msg = FaceDetections()
if len(msg.faces) > 0:
#rospy.loginfo("Face msg: " + str(msg.faces))
self.say("Oh, face")
f = msg.faces[0]
# f = FaceDetection()
# self.look_at(f.position.x, f.position.y, f.position.z)
self.point_at(f.position.x, f.position.y, f.position.z)
def look_at(self, x, y, z):
phg = PointHeadGoal()
phg.target.header.frame_id = '/stereo_optical_frame'
phg.target.point.x = x
phg.target.point.y = y
phg.target.point.z = z
phg.pointing_axis.z = 1.0
phg.pointing_frame = phg.target.header.frame_id
phg.min_duration = rospy.Duration(1.0)
phg.max_velocity = 1.0
self.point_head_ac.send_goal_and_wait(phg)
def say(self, text):
rospy.loginfo("Creating goal with text: " + text)
goal = TtsGoal()
goal.rawtext.text = text
goal.rawtext.lang_id = 'en_GB'
rospy.loginfo("Sending goal!")
self.tts_ac.send_goal_and_wait(goal)
rospy.loginfo("We are done!")
class Gripper(object):
def __init__(self, active=True):
self.active = active
self.home_action = SimpleActionClient('/wsg50/home_gripper_action', HomeGripperAction)
self.slipping_control_action = SimpleActionClient('/wsg50/slipping_control_action', SlippingControlAction)
self.grasp_action = SimpleActionClient('/wsg50/grasp_action', GraspAction)
self.get_state_service = rospy.ServiceProxy('/wsg50/slipping_control/get_state', GetState)
self.ch_params0_service = rospy.ServiceProxy('/wsg50/ls_0/change_params', ChLSParams)
self.ch_params1_service = rospy.ServiceProxy('/wsg50/ls_1/change_params', ChLSParams)
def home(self):
if self.active:
self.home_action.send_goal_and_wait(HomeGripperGoal())
def grasp(self, force):
if self.active:
goal = GraspGoal()
goal.desired_force = force
self.grasp_action.send_goal_and_wait(goal)
def gripper_pivoting(self):
if self.active:
goal = SlippingControlGoal()
goal.mode = SlippingControlGoal.MODE_GRIPPER_PIVOTING
self.slipping_control_action.send_goal_and_wait(goal)
def slipping_avoidance(self):
if self.active:
goal = SlippingControlGoal()
goal.mode = SlippingControlGoal.MODE_SLIPPING_AVOIDANCE
self.slipping_control_action.send_goal_and_wait(goal)
def get_state(self):
if self.active:
return self.get_state_service().state
else:
return 4
# define STATE_UNDEFINED -1
# define STATE_HOME 0
# define STATE_HOMING 1
# define STATE_COMPUTING_BIAS 2
# define STATE_GRASPING 3
# define STATE_GRASPED 4
# define STATE_TO_GRIPPER_PIVOTING 5
# define STATE_GRIPPER_PIVOTING 6
# define STATE_TO_SLIPPING_AVOIDANCE 7
# define STATE_SLIPPING_AVOIDANCE 8
# define STATE_TO_DYN_SLIPPING_AVOIDANCE 9
# define STATE_DYN_SLIPPING_AVOIDANCE 10
# define STATE_OBJECT_PIVOTING 11
def is_grasped(self):
state = self.get_state()
if state == 4 or state == 5 or state == 6 or state == 7 or state == 8 or state == 9 or state == 10:
return True
return False
def change_params(self, mu):
ch_msg = ChLSParamsRequest()
ch_msg.delta = -1
ch_msg.gamma = -1
ch_msg.mu = mu
ch_msg.k = -1
self.ch_params0_service(ch_msg)
self.ch_params1_service(ch_msg)
def do_it(msg):
rospy.loginfo('We are DOING IT!')
# connect to cluster bounding box finding service
rospy.loginfo('waiting for find_cluster_bounding_box service')
rospy.wait_for_service('/find_cluster_bounding_box')
find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox)
rospy.loginfo('connected to find_cluster_bounding_box')
target_location = msg.target_location
t_p1 = msg.object_bbox.points[0]
t_p2 = msg.object_bbox.points[1]
t_p1.y, t_p2.y = t_p2.y, t_p1.y
reset = ResetRobot()
reset.execute()
rospy.loginfo('Robot reset')
segment = SegmentScene()
res = segment.execute()
if not res: exit(1)
rospy.loginfo('Scene segmented')
# find a cluster that intersects with a provided bounding box
for idx,cluster in enumerate(res[1]['segmentation_result'].clusters):
bbox_response = find_bounding_box_srv(cluster)
if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS:
rospy.logwarn('unable to find for cluster %d bounding box' % idx)
continue
o_center = bbox_response.pose.pose.position
o_p1 = Point32(o_center.x - bbox_response.box_dims.x, o_center.y - bbox_response.box_dims.y, o_center.z)
o_p2 = Point32(o_center.x + bbox_response.box_dims.x, o_center.y + bbox_response.box_dims.y, o_center.z)
print idx, t_p1, t_p2, o_p1, o_p2
if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y) and
(o_p1.x < t_p2.x and o_p1.y < t_p2.y)):
closest_index = idx
break
grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
grasp_client.wait_for_server()
grasp_goal = GraspObjectGoal()
grasp_goal.category_id = -1
grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
grasp_goal.collision_object_name = res[0].collision_object_names[closest_index]
grasp_goal.collision_support_surface_name = res[0].collision_support_surface_name
result = grasp_client.send_goal_and_wait(grasp_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to grasp')
reset.execute()
exit(1)
rospy.loginfo('Grasped an object')
lift_client = SimpleActionClient('lift_object', LiftObjectAction)
lift_client.wait_for_server()
lift_goal = LiftObjectGoal()
lift_goal.category_id = 0
lift_goal.graspable_object = res[0].graspable_objects[closest_index]
lift_goal.collision_object_name = res[0].collision_object_names[closest_index]
lift_goal.collision_support_surface_name = res[0].collision_support_surface_name
result = lift_client.send_goal_and_wait(lift_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to lift')
exit(1)
rospy.loginfo('Lifted an object')
pudown_client = SimpleActionClient('put_down_at', PutDownAtAction)
pudown_client.wait_for_server()
putdown_goal = PutDownAtGoal()
putdown_goal.category_id = -1
putdown_goal.graspable_object = res[0].graspable_objects[closest_index]
putdown_goal.collision_object_name = res[0].collision_object_names[closest_index]
putdown_goal.collision_support_surface_name = res[0].collision_support_surface_name
putdown_goal.target_location = Point(target_location.x, target_location.y, target_location.z) #Point(0.468, 0.226, 0.251)
result = pudown_client.send_goal_and_wait(putdown_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to put down')
reset.execute()
exit(1)
rospy.loginfo('Pu down the object')
reset.execute()
randoms = []
max_value = max - min
for i in range(n):
randoms.append((random.random() * max_value) + min) # this scales the value between min and max
return randoms
if __name__ == '__main__':
rospy.init_node('random_left_leg_goals')
ctl_as = SimpleActionClient(CONTROLLER_AS, FollowJointTrajectoryAction)
rospy.loginfo("Connecting to " + CONTROLLER_AS)
ctl_as.wait_for_server()
rospy.loginfo("Connected.")
while not rospy.is_shutdown():
fjtg = FollowJointTrajectoryGoal()
jt = JointTrajectory()
#jt.joint_names = ['leg_left_1_joint', 'leg_left_2_joint', 'leg_left_3_joint', 'leg_left_4_joint', 'leg_left_5_joint', 'leg_left_6_joint']
jt.joint_names = JOINT_NAMES
jtp = JointTrajectoryPoint()
jtp.positions = get_n_randoms(len(JOINT_NAMES), -4.0, 4.0)
jtp.velocities = [0.0] * len(JOINT_NAMES)
#jtp.accelerations = [0.0] * len(JOINT_NAMES)
#jtp.effort = [0.0] * len(JOINT_NAMES)
jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
jt.points.append(jtp)
fjtg.trajectory = jt
ctl_as.send_goal_and_wait(fjtg)
rospy.loginfo("Sent: " + str(jt))
rospy.sleep(jtp.time_from_start)
class ResetRobot():
def __init__(self):
self.action_index = 0
self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction)
self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction)
self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
result = self.posture_controller.send_goal_and_wait(pg)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to open gripper')
return False
return True
def gentle_close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = 0.0
goal.dynamic_torque_control = False
self.gripper_controller.send_goal(goal)
self.gripper_controller.wait_for_result()
def ready_arm(self, collision_operations=OrderedCollisionOperations()):
"""
Moves the arm to the side out of the view of all sensors. In this
position the arm is ready to perform a grasp action.
"""
goal = ReadyArmGoal()
goal.collision_operations = collision_operations
state = self.ready_arm_client.send_goal_and_wait(goal)
return state == GoalStatus.SUCCEEDED
def reset_collision_map(self):
req = TabletopCollisionMapProcessingRequest()
req.detection_result = TabletopDetectionResult()
req.reset_collision_models = True
req.reset_attached_models = True
self.collision_map_processing_srv(req)
rospy.loginfo('collision map reset')
def execute(self):
rospy.loginfo('resetting robot')
self.action_index = 0
self.reset_collision_map()
current_state = find_current_arm_state()
if current_state not in ['READY', 'TUCK']:
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied:
if not self.open_gripper(): return False
if current_state != 'READY' and not self.ready_arm(): return False
self.gentle_close_gripper()
return True
class Pick_Place:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
#
# self._pose_table = self._scene.get_object_poses(self._table_object_name)
# self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name)
# self.setup()
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
rospy.sleep(1.0)
self._scene.remove_world_object(self._grasp_object_name)
rospy.sleep(1.0)
# # Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
rospy.sleep(2.0)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(2.0)
# rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.06
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
# while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
# rospy.logwarn('Place failed! Retrying ...')
# rospy.sleep(1.0)
#
# rospy.loginfo('Place successfully')
def setup():
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_attached_object()
def _add_table(self, name):
rospy.loginfo("entered table")
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.2
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (9 , 9, 0.02))
return p.pose
def _add_grasp_block_(self, name):
rospy.loginfo("entered box grabber")
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.05
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.1, 0.1, 0.1))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.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, angle )
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
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def _add_objects(self):
"""
Here add all the objects that you want to add to the _scene
"""
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
class SonarHead(object):
def __init__(self):
rospy.loginfo("Initializing SonarHead")
self.torso_pub = rospy.Publisher('/torso_controller/command',
JointTrajectory, queue_size=5)
self.torso_ac = SimpleActionClient('/torso_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.torso_joints = [0.0, 0.0]
self.js_sub = rospy.Subscriber('/joint_states',
JointState,
self.js_cb,
queue_size=1)
# self.torso_ac.wait_for_server()
self.sonar_reads = [0.0, 0.0]
self.sonar_sub = rospy.Subscriber('/sonar_torso',
Range,
self.sonar_cb,
queue_size=10)
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
dif = self.sonar_reads[0] - self.sonar_reads[1]
if abs(dif) > 0.25:
self.move_torso_topic(
self.torso_joints[0] + dif / 10.0,
0.0,
time=0.2)
r.sleep()
def js_cb(self, msg):
# msg = JointState()
t1_idx = msg.name.index('torso_1_joint')
t2_idx = msg.name.index('torso_2_joint')
self.torso_joints[0] = msg.position[t1_idx]
self.torso_joints[1] = msg.position[t2_idx]
def sonar_cb_2(self, msg):
if "14" in msg.header.frame_id:
self.sonar_reads[0] = msg.range
elif "15" in msg.header.frame_id:
self.sonar_reads[1] = msg.range
def sonar_cb(self, msg):
# 15 is left 14 is right
# msg = Range()
if "14" in msg.header.frame_id:
# rospy.loginfo("Sonar right distance: " + str(msg.range))
if msg.range < 0.25:
#self.move_torso(-0.5, 0.0, time=2.0)
#self.move_torso_topic(-0.2, 0.0, time=1.0)
self.move_torso_topic(
self.torso_joints[0] - 0.05, 0.0, time=0.2)
elif "15" in msg.header.frame_id:
#rospy.loginfo("Sonar left distance: " + str(msg.range))
if msg.range < 0.25:
#self.move_torso(0.5, 0.0, time=2.0)
#self.move_torso_topic(0.2, 0.0, time=1.0)
self.move_torso_topic(
self.torso_joints[0] + 0.05, 0.0, time=0.2)
def move_torso(self, joint_1, joint_2, time=1.0):
# -0.24 0.61 torso_2_joint
# -1.28 1.28 torso_1_joint
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names = ['torso_1_joint', 'torso_2_joint']
p = JointTrajectoryPoint()
p.positions = [joint_1, joint_2]
rospy.loginfo("Moving torso to: " + str(p.positions))
p.time_from_start = rospy.Duration(time)
fjtg.trajectory.points.append(p)
self.torso_ac.send_goal_and_wait(fjtg)
def move_torso_topic(self, joint_1, joint_2, time=1.0):
jt = JointTrajectory()
jt.joint_names = ['torso_1_joint', 'torso_2_joint']
p = JointTrajectoryPoint()
p.positions = [joint_1, joint_2]
rospy.loginfo("Moving torso to: " + str(p.positions))
p.time_from_start = rospy.Duration(time)
jt.points.append(p)
self.torso_pub.publish(jt)
