Motion and Behavior Planning for Socially Assistive Robots