def place(self,
current_episode: Episode,
action_id: int,
action: Action,
action_frame: Affine,
image_frame: Affine,
place_bin=None):
place_bin = place_bin if place_bin else self.current_bin
self.move_to_safety(self.md)
md_approach_down = MotionData().with_dynamics(
0.22).with_z_force_condition(7.0)
md_approach_up = MotionData().with_dynamics(
1.0).with_z_force_condition(20.0)
action_approch_affine = Affine(z=Config.approach_distance_from_pose)
action_approach_frame = action_frame * action_approch_affine
if Config.release_in_other_bin:
self.move_to_release(self.md, target=action_approach_frame)
else:
self.robot.move_cartesian(Frames.gripper, action_approach_frame,
self.md)
self.robot.move_relative_cartesian(Frames.gripper,
action_approch_affine.inverse(),
md_approach_down)
if md_approach_down.did_break:
self.robot.recover_from_errors()
action.collision = True
self.robot.move_relative_cartesian(Frames.gripper, Affine(z=0.001),
md_approach_up)
action.final_pose = RobotPose(
affine=(image_frame.inverse() *
self.robot.current_pose(Frames.gripper)))
action.pose.d = self.gripper.width()
self.gripper.release(action.pose.d + 0.01) # [m]
if Config.mode is not Mode.Perform:
self.move_to_safety(md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images:
self.robot.move_cartesian(Frames.camera, image_frame, self.md)
self.last_after_images = self.take_images(current_bin=place_bin)
self.saver.save_image(self.last_after_images,
current_episode.id,
action_id,
'after',
action=action)
def get_pose_in_image(cls,
action_pose: RobotPose,
image_pose: Affine,
reference_pose: Affine = None) -> RobotPose:
if np.isnan(action_pose.z):
action_pose.z = -0.35 # [m]
z_offset = -0.022 # [m]
image_translation = Affine(image_pose.x, image_pose.y,
image_pose.z + z_offset)
image_rotation = Affine(a=image_pose.a, b=image_pose.b, c=image_pose.c)
action_translation = Affine(action_pose.x, action_pose.y,
action_pose.z)
action_rotation = Affine(b=action_pose.b,
c=action_pose.c) * Affine(a=action_pose.a)
affine = image_rotation * image_translation.inverse(
) * action_translation * action_rotation
if reference_pose:
reference_rotation = Affine(b=reference_pose.b,
c=reference_pose.c).inverse()
affine = reference_rotation * affine
return RobotPose(affine=affine, d=action_pose.d)
def take_images(current_bin: Bin, camera: Camera, robot: Robot, image_frame: Affine = None):
images = camera.take_images()
if not image_frame:
image_frame = robot.current_pose(Frames.camera)
pose = RobotPose(affine=(image_frame.inverse() * Frames.get_frame(current_bin)))
for image in images:
image.pose = pose
return images
def place(
robot: Robot,
gripper: Gripper,
current_episode: Episode,
current_bin: Bin,
action: Action,
action_frame: Affine,
grasp_action: Action,
image_frame: Affine,
camera: Camera,
saver: Saver,
md: MotionData
) -> None:
move_to_safety(robot, md)
md_approach_down = MotionData().with_dynamics(0.3).with_z_force_condition(7.0)
md_approach_up = MotionData().with_dynamics(1.0).with_z_force_condition(20.0)
action_approch_affine = Affine(z=Config.approach_distance_from_pose)
action_approach_frame = action_frame * action_approch_affine
robot.move_cartesian(Frames.gripper, action_approach_frame, md)
robot.move_relative_cartesian(Frames.gripper, action_approch_affine.inverse(), md_approach_down)
if md_approach_down.did_break:
robot.recover_from_errors()
action.collision = True
robot.move_relative_cartesian(Frames.gripper, Affine(z=0.001), md_approach_up)
action.final_pose = RobotPose(affine=(image_frame.inverse() * robot.current_pose(Frames.gripper)))
gripper.release(grasp_action.final_pose.d + 0.006) # [m]
if Config.mode is not Mode.Perform:
move_to_safety(robot, md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images and Config.release_in_other_bin:
robot.move_cartesian(Frames.camera, image_frame, md)
saver.save_image(take_images(current_bin, camera, robot), current_episode.id, 'after', action=action)
def take_images(self,
image_frame: Affine = None,
current_bin=None) -> List[OrthographicImage]:
current_bin = current_bin if current_bin else self.current_bin
images = self.camera.take_images()
if not image_frame:
image_frame = self.robot.current_pose(Frames.camera)
pose = RobotPose(affine=(image_frame.inverse() *
Frames.get_frame(current_bin)))
for image in images:
image.pose = pose.to_array()
return images
def shift(self, current_episode: Episode, action_id: int, action: Action,
action_frame: Affine, image_frame: Affine):
md_approach_down = MotionData().with_dynamics(
0.15).with_z_force_condition(6.0)
md_approach_up = MotionData().with_dynamics(
0.6).with_z_force_condition(20.0)
md_shift = MotionData().with_dynamics(0.1).with_xy_force_condition(
10.0)
action_approch_affine = Affine(z=Config.approach_distance_from_pose)
action_approach_frame = action_approch_affine * action_frame
try:
process_gripper = Process(target=self.gripper.move,
args=(action.pose.d, ))
process_gripper.start()
self.robot.move_cartesian(Frames.gripper, action_approach_frame,
self.md)
process_gripper.join()
except OSError:
self.gripper.move(0.08)
self.robot.move_cartesian(Frames.gripper, action_approach_frame,
self.md)
self.robot.move_relative_cartesian(Frames.gripper,
action_approch_affine.inverse(),
md_approach_down)
if md_approach_down.did_break:
self.robot.recover_from_errors()
action.collision = True
self.robot.move_relative_cartesian(Frames.gripper, Affine(z=0.001),
md_approach_up)
self.robot.move_relative_cartesian(
Frames.gripper,
Affine(x=action.shift_motion[0], y=action.shift_motion[1]),
md_shift)
self.robot.move_relative_cartesian(Frames.gripper,
action_approch_affine,
md_approach_up)
def shift(
robot: Robot,
gripper: Gripper,
current_episode: Episode,
current_bin: Bin,
action: Action,
action_frame: Affine,
image_frame: Affine,
camera: Camera,
saver: Saver,
md: MotionData
) -> None:
md_approach_down = MotionData().with_dynamics(0.15).with_z_force_condition(6.0)
md_approach_up = MotionData().with_dynamics(0.6).with_z_force_condition(20.0)
md_shift = MotionData().with_dynamics(0.1).with_xy_force_condition(10.0)
action_approch_affine = Affine(z=Config.approach_distance_from_pose)
action_approach_frame = action_approch_affine * action_frame
try:
process_gripper = Process(target=gripper.move, args=(action.pose.d, ))
process_gripper.start()
robot.move_cartesian(Frames.gripper, action_approach_frame, md)
process_gripper.join()
except OSError:
gripper.move(0.08)
robot.move_cartesian(Frames.gripper, action_approach_frame, md)
robot.move_relative_cartesian(Frames.gripper, action_approch_affine.inverse(), md_approach_down)
if md_approach_down.did_break:
robot.recover_from_errors()
action.collision = True
robot.move_relative_cartesian(Frames.gripper, Affine(z=0.001), md_approach_up)
robot.move_relative_cartesian(Frames.gripper, Affine(x=action.shift_motion[0], y=action.shift_motion[1]), md_shift)
robot.move_relative_cartesian(Frames.gripper, action_approch_affine, md_approach_up)
import numpy as np
from argparse import ArgumentParser
from cfrankr import Robot, Affine, MotionData
if __name__ == '__main__':
parser = ArgumentParser()
parser.add_argument('--host',
default='panda_arm',
help='name of the robot')
args = parser.parse_args()
# Setup Robot
general_dynamics_rel = 0.32
robot = Robot(args.host, general_dynamics_rel)
#robot.recover_from_errors()
# Move down
md_approach_down = MotionData().with_dynamics(0.3).with_z_force_condition(
7.0) # dynamtics is both velocity and acceleration of robot (= 0.3)
tcp = Affine(0.0, 0.0, 0.18, -np.pi / 4, 0.0, -np.pi)
approach_distance_from_pose = 0.120
action_approch_affine = Affine(z=approach_distance_from_pose)
robot.move_relative_cartesian(tcp, action_approch_affine.inverse(),
md_approach_down)
def grasp(
robot: Robot,
gripper: Gripper,
current_episode: Episode,
current_bin: Bin,
action: Action,
action_frame: Affine,
image_frame: Affine,
camera: Camera,
saver: Saver,
md: MotionData
) -> None:
# md_approach_down = MotionData().with_dynamics(0.12).with_z_force_condition(7.0)
# md_approach_up = MotionData().with_dynamics(0.6).with_z_force_condition(20.0)
md_approach_down = MotionData().with_dynamics(0.3).with_z_force_condition(7.0)
md_approach_up = MotionData().with_dynamics(1.0).with_z_force_condition(20.0)
action_approch_affine = Affine(z=Config.approach_distance_from_pose)
action_approach_frame = action_frame * action_approch_affine
try:
process_gripper = Process(target=gripper.move, args=(action.pose.d, ))
process_gripper.start()
robot.move_cartesian(Frames.gripper, action_approach_frame, md)
process_gripper.join()
except OSError:
gripper.move(0.08)
robot.move_cartesian(Frames.gripper, action_approach_frame, md)
robot.move_relative_cartesian(Frames.gripper, action_approch_affine.inverse(), md_approach_down)
if md_approach_down.did_break:
robot.recover_from_errors()
action.collision = True
robot.move_relative_cartesian(Frames.gripper, Affine(z=0.001), md_approach_up)
action.final_pose = RobotPose(affine=(image_frame.inverse() * robot.current_pose(Frames.gripper)))
first_grasp_successful = gripper.clamp()
if first_grasp_successful:
logger.info('Grasp successful at first.')
robot.recover_from_errors()
# Change here
action_approch_affine = Affine(z=1.5*Config.approach_distance_from_pose)
move_up_success = robot.move_relative_cartesian(Frames.gripper, action_approch_affine, md_approach_up)
if move_up_success and not md_approach_up.did_break:
if Config.mode is Mode.Measure and Config.take_after_images and not Config.release_in_other_bin:
robot.move_cartesian(Frames.camera, image_frame, md)
saver.save_image(take_images(current_bin, camera, robot), current_episode.id, 'after', action=action)
if Config.file_objects:
raise Exception('File objects not implemented!')
if Config.release_during_grasp_action:
move_to_release_success = move_to_release(robot, current_bin, md)
if move_to_release_success:
if gripper.is_grasping():
action.reward = 1.0
action.final_pose.d = gripper.width()
if Config.mode is Mode.Perform:
gripper.release(action.final_pose.d + 0.002) # [m]
else:
gripper.release(action.pose.d + 0.002) # [m]
move_to_safety(robot, md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images and Config.release_in_other_bin:
robot.move_cartesian(Frames.camera, image_frame, md)
saver.save_image(take_images(current_bin, camera, robot), current_episode.id, 'after', action=action)
else:
if Config.mode is not Mode.Perform:
move_to_safety(robot, md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images and Config.release_in_other_bin:
robot.move_cartesian(Frames.camera, image_frame, md)
saver.save_image(take_images(current_bin, camera, robot), current_episode.id, 'after', action=action)
else:
gripper.release(action.pose.d + 0.002) # [m]
robot.recover_from_errors()
robot.move_relative_cartesian(Frames.gripper, action_approch_affine, md_approach_up)
move_to_safety_success = move_to_safety(robot, md_approach_up)
if not move_to_safety_success:
robot.recover_from_errors()
robot.recover_from_errors()
move_to_safety(robot, md_approach_up)
gripper.move(gripper.max_width)
move_to_safety(robot, md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images:
robot.move_cartesian(Frames.camera, image_frame, md)
saver.save_image(take_images(current_bin, camera, robot), current_episode.id, 'after', action=action)
else:
logger.info('Grasp not successful.')
gripper.release(gripper.width() + 0.002) # [m]
robot.recover_from_errors()
move_up_successful = robot.move_relative_cartesian(Frames.gripper, action_approch_affine, md_approach_up)
if md_approach_up.did_break or not move_up_successful:
gripper.release(action.pose.d) # [m]
robot.recover_from_errors()
robot.move_relative_cartesian(Frames.gripper, action_approch_affine, md_approach_up)
move_to_safety(robot, md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images:
robot.move_cartesian(Frames.camera, image_frame, md)
saver.save_image(take_images(current_bin, camera, robot), current_episode.id, 'after', action=action)
def grasp(self, current_episode: Episode, action_id: int, action: Action,
action_frame: Affine, image_frame: Affine):
md_approach_down = MotionData().with_dynamics(
0.3).with_z_force_condition(7.0)
md_approach_up = MotionData().with_dynamics(
1.0).with_z_force_condition(20.0)
action_approch_affine = Affine(z=Config.approach_distance_from_pose)
action_approach_frame = action_frame * action_approch_affine
try:
process_gripper = Process(target=self.gripper.move,
args=(action.pose.d, ))
process_gripper.start()
self.robot.move_cartesian(Frames.gripper, action_approach_frame,
self.md)
process_gripper.join()
except OSError:
self.gripper.move(0.08)
self.robot.move_cartesian(Frames.gripper, action_approach_frame,
self.md)
self.robot.move_relative_cartesian(Frames.gripper,
action_approch_affine.inverse(),
md_approach_down)
if md_approach_down.did_break:
self.robot.recover_from_errors()
action.collision = True
self.robot.move_relative_cartesian(Frames.gripper, Affine(z=0.001),
md_approach_up)
action.final_pose = RobotPose(
affine=(image_frame.inverse() *
self.robot.current_pose(Frames.gripper)))
first_grasp_successful = self.gripper.clamp()
if first_grasp_successful:
logger.info('Grasp successful at first.')
self.robot.recover_from_errors()
action_approch_affine = Affine(
z=Config.approach_distance_from_pose)
move_up_success = self.robot.move_relative_cartesian(
Frames.gripper, action_approch_affine, md_approach_up)
if move_up_success and not md_approach_up.did_break:
if Config.mode is Mode.Measure and Config.take_after_images and not Config.release_in_other_bin:
self.robot.move_cartesian(Frames.camera, image_frame,
self.md)
self.last_after_images = self.take_images()
self.saver.save_image(self.last_after_images,
current_episode.id,
action_id,
'after',
action=action)
if Config.release_during_grasp_action:
move_to_release_success = self.move_to_release(self.md)
if move_to_release_success:
if self.gripper.is_grasping():
action.reward = 1.0
action.final_pose.d = self.gripper.width()
if Config.mode is Mode.Perform:
self.gripper.release(action.final_pose.d +
0.005) # [m]
else:
self.gripper.release(action.pose.d + 0.005) # [m]
self.move_to_safety(md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images and Config.release_in_other_bin:
self.robot.move_cartesian(Frames.camera,
image_frame, self.md)
self.last_after_images = self.take_images()
self.saver.save_image(self.last_after_images,
current_episode.id,
action_id,
'after',
action=action)
else:
if Config.mode is not Mode.Perform:
self.move_to_safety(md_approach_up)
if self.gripper.is_grasping():
action.reward = 1.0
action.final_pose.d = self.gripper.width()
if Config.mode is Mode.Measure and Config.take_after_images:
self.robot.move_cartesian(Frames.camera, image_frame,
self.md)
self.last_after_images = self.take_images()
self.saver.save_image(self.last_after_images,
current_episode.id,
action_id,
'after',
action=action)
else:
self.gripper.release(action.pose.d + 0.002) # [m]
self.robot.recover_from_errors()
self.robot.move_relative_cartesian(Frames.gripper,
action_approch_affine,
md_approach_up)
move_to_safety_success = self.move_to_safety(md_approach_up)
if not move_to_safety_success:
self.robot.recover_from_errors()
self.robot.recover_from_errors()
self.move_to_safety(md_approach_up)
self.gripper.move(self.gripper.max_width)
self.move_to_safety(md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images:
self.robot.move_cartesian(Frames.camera, image_frame,
self.md)
self.last_after_images = self.take_images()
self.saver.save_image(self.last_after_images,
current_episode.id,
action_id,
'after',
action=action)
else:
logger.info('Grasp not successful.')
self.gripper.release(self.gripper.width() + 0.002) # [m]
self.robot.recover_from_errors()
move_up_successful = self.robot.move_relative_cartesian(
Frames.gripper, action_approch_affine, md_approach_up)
if md_approach_up.did_break or not move_up_successful:
self.gripper.release(action.pose.d) # [m]
self.robot.recover_from_errors()
self.robot.move_relative_cartesian(Frames.gripper,
action_approch_affine,
md_approach_up)
self.move_to_safety(md_approach_up)
if Config.mode is Mode.Measure and Config.take_after_images:
self.robot.move_cartesian(Frames.camera, image_frame, self.md)
self.last_after_images = self.take_images()
self.saver.save_image(self.last_after_images,
current_episode.id,
action_id,
'after',
action=action)
