class Task(object):
def __init__(self, limb, hover_distance = 0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._executor = Executor(limb, verbose)
trajsvc = "baxter_adapt/adaptation_server"
learningsvc = "baxter_adapt/learning_server"
rospy.wait_for_service(trajsvc, 5.0)
rospy.wait_for_service(learningsvc, 5.0)
self._trajsvc = rospy.ServiceProxy(trajsvc, Adaptation)
self._learningsvc = rospy.ServiceProxy(learningsvc, Learning)
def move_to_start(self, start_angles=None):
print self._executor.get_current_joints()
print("Moving the {0} arm to start pose...".format(self._limb_name))
if not start_angles:
start_angles = dict(zip(self._joint_names, [0]*7))
self._executor.move_to_joint(start_angles)
self._executor.gripper_open()
rospy.sleep(1.0)
def _approach(self, pose):
approach = copy.deepcopy(pose)
# approach with a pose the hover-distance above the requested pose
approach.position.z = approach.position.z + self._hover_distance
self._executor.move_to_pose(approach)
def _retract(self):
# retrieve current pose from endpoint
retract = copy.deepcopy(self._executor.get_current_pose())
retract.position.z = retract.position.z + self._hover_distance
self._executor.move_to_pose(retract)
def transfer(self, start_pose, end_pose, obstacles):
print("Picking up the object")
self.pick(start_pose)
joint_names = self._executor.joint_names()
joints = self._executor.ik_request(start_pose)
start_joints = [joints[i] for i in joint_names]
joints = self._executor.ik_request(end_pose)
end_joints = [joints[i] for i in joint_names]
print start_joints
print end_joints
print "Adapting Trajectory Learned from Demonstrations"
resp = self._trajsvc(start_joints, end_joints, obstacles, True)
# resp = AdaptationResponse()
# resp.filename = '/home/aecins/ros_ws/src/baxter_adapt/MPC/generated/JointsTrajectory'
# resp.response = True
print resp
if resp.response is True:
while os.path.isfile(resp.filename) is False:
print "Wait for trajectory generation"
rospy.sleep(2.0)
print("Perform trajectory")
traj = open(resp.filename, 'r').readlines()
self._executor.move_as_trajectory(traj)
def transfer_imitation(self, start_pose, end_pose):
print("Picking up the object")
self.pick(start_pose)
print("Computing imitation trajectory")
joint_names = self._executor.joint_names()
joints = self._executor.ik_request(start_pose)
start_joints = [joints[i] for i in joint_names]
joints = self._executor.ik_request(end_pose)
end_joints = [joints[i] for i in joint_names]
print start_joints
print end_joints
obstacles = []
resp = self._trajsvc(start_joints, end_joints, obstacles, False)
print resp
if resp.response is True:
print("Perform trajectory")
traj = open(resp.filename, 'r').readlines()
self._executor.move_as_trajectory(traj)
def get_current_joints(self):
return self._executor.get_current_joints()
def get_current_pose(self):
return self._executor.get_current_pose()
def pick(self, pose):
# open the gripper
self._executor.gripper_open()
# servo above pose
self._approach(pose)
# servo to pose
self._executor.move_to_pose(pose)
# close gripper
self._executor.gripper_close()
# retract to clear object
self._retract()
def place(self, pose):
# servo above pose
#self._approach(pose)
# servo to pose
self._executor.move_to_pose(pose)
# open the gripper
self._executor.gripper_open()
# retract to clear object
self._retract()
def learning(self):
print('Learning weights from feedback')
feedback = self.get_feedback()
resp = self._learningsvc(feedback)
print('Updated weights for movement adaptation')
def get_feedback(self):
filename = os.getcwd() + '/trajectory_improved'
print('Please help me improve the task.')
self._executor.record(filename)
print('Done')
return filename
def map_file(filename):
executor = Executor("left")
traj = open(filename, 'r').readlines()
#traj = executor.ik_trajectory(filename, filename + '_joints', executor.get_current_pose())
executor.move_as_trajectory(traj)
class Task(object):
def __init__(self, limb, hover_distance = 0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._executor = Executor(limb, verbose)
trajsvc = "baxter_adapt/imitation_server"
rospy.wait_for_service(trajsvc, 5.0)
self._trajsvc = rospy.ServiceProxy(trajsvc, Imitation)
def move_to_start(self, start_angles=None):
print("Moving the {0} arm to start pose...".format(self._limb_name))
if not start_angles:
start_angles = dict(zip(self._joint_names, [0]*7))
self._executor.move_to_joint(start_angles)
self._executor.gripper_open()
rospy.sleep(1.0)
print("Running. Ctrl-c to quit")
def _approach(self, pose):
approach = copy.deepcopy(pose)
# approach with a pose the hover-distance above the requested pose
approach.position.z = approach.position.z + self._hover_distance
self._executor.move_to_pose(approach)
def _retract(self):
# retrieve current pose from endpoint
retract = copy.deepcopy(self._executor.get_current_pose())
retract.position.z = retract.position.z + self._hover_distance
self._executor.move_to_pose(retract)
def transfer(self, pose):
# request Matlab to compute trajectory
pose_start = self._executor.get_current_pose()
y_start = pose_start.position
y_end = pose.position
resp = self._trajsvc(y_start, y_end)
print resp
if resp.response is True:
# perform the trajectory via Inverse Kinematics
self._executor.move_as_trajectory(resp.filename)
def pick(self, pose):
# open the gripper
self._executor.gripper_open()
# servo above pose
self._approach(pose)
# servo to pose
self._executor.move_to_pose(pose)
# close gripper
self._executor.gripper_close()
# retract to clear object
#self._retract()
def place(self, pose):
# servo above pose
self._approach(pose)
# servo to pose
self._executor.move_to_pose(pose)
# open the gripper
self._executor.gripper_open()
# retract to clear object
self._retract()
