def predict(self): rot, trans = RigidTransform.rotation_and_translation_from_matrix( self.datapoint['obj_pose']) dataset_pose = RigidTransform(rot, trans, 'obj', 'world') dataset_final_poses = self.datapoint['final_poses'] num_vertices = self.datapoint['num_vertices'] final_poses = [] for i in range(20): final_pose_mat = dataset_final_poses[i * 4:(i + 1) * 4] if np.array_equal(final_pose_mat, np.zeros((4, 4))): break rot, trans = RigidTransform.rotation_and_translation_from_matrix( final_pose_mat) final_poses.append(RigidTransform(rot, trans, 'obj', 'world')) vertices = self.datapoint['vertices'][:num_vertices] normals = self.datapoint['normals'][:num_vertices] vertices = (self.obj.T_obj_world * dataset_pose.inverse() * PointCloud(vertices.T, 'world')).data.T normals = (self.obj.T_obj_world * dataset_pose.inverse() * PointCloud(normals.T, 'world')).data.T vertex_probs = self.datapoint[ 'vertex_probs'][:num_vertices, :len(final_poses)] required_forces = self.datapoint['min_required_forces'][:num_vertices] return vertices, normals, final_poses, vertex_probs, required_forces
def object_to_camera_pose(self, radius, elev, az, roll): """ Convert spherical coords to an object-camera pose. """ # generate camera center from spherical coords camera_center_obj = np.array([sph2cart(radius, az, elev)]).squeeze() camera_z_obj = -camera_center_obj / np.linalg.norm(camera_center_obj) # find the canonical camera x and y axes camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0]) if np.linalg.norm(camera_x_par_obj) == 0: camera_x_par_obj = np.array([1, 0, 0]) camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj) camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) if camera_y_par_obj[2] > 0: camera_x_par_obj = -camera_x_par_obj camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) camera_y_par_obj = camera_y_par_obj / np.linalg.norm( camera_y_par_obj) # rotate by the roll R_obj_camera_par = np.c_[camera_x_par_obj, camera_y_par_obj, camera_z_obj] R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0], [np.sin(roll), np.cos(roll), 0], [0, 0, 1]]) R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera) t_obj_camera = camera_center_obj # create final transform T_obj_camera = RigidTransform(R_obj_camera, t_obj_camera, from_frame=self.frame, to_frame='obj') return T_obj_camera.inverse()
def visualize_scene(mesh, T_world_ar, T_ar_cam, T_cam_obj): T_cam_cam = RigidTransform() T_obj_cam = T_cam_obj.inverse() T_cam_ar = T_ar_cam.inverse() Twc = np.matmul(T_world_ar.matrix, T_ar_cam.matrix) T_world_cam = RigidTransform(Twc[:3, :3], Twc[:3, 3]) T_cam_world = T_world_cam.inverse() o = np.array([0., 0., 0.]) centroid_cam = apply_transform(o, T_cam_obj) tag_cam = apply_transform(o, T_cam_ar) robot_cam = apply_transform(o, T_cam_world) # camera vis3d.points(o, color=(0, 1, 0), scale=0.01) vis3d.pose(T_cam_cam, alpha=0.05, tube_radius=0.005, center_scale=0.002) # object vis3d.mesh(mesh) vis3d.points(centroid_cam, color=(0, 0, 0), scale=0.01) vis3d.pose(T_cam_obj, alpha=0.05, tube_radius=0.005, center_scale=0.002) # AR tag vis3d.points(tag_cam, color=(1, 0, 1), scale=0.01) vis3d.pose(T_cam_ar, alpha=0.05, tube_radius=0.005, center_scale=0.002) vis3d.table(T_cam_ar, dim=0.074) # robot vis3d.points(robot_cam, color=(1, 0, 0), scale=0.01) vis3d.pose(T_cam_world, alpha=0.05, tube_radius=0.005, center_scale=0.002) vis3d.show()
def T_obj_camera(self): """Returns the transformation from camera to object when the object is in the given stable pose. Returns ------- :obj:`autolab_core.RigidTransform` The desired transform. """ if self.stable_pose is None: T_obj_world = RigidTransform(from_frame='obj', to_frame='world') else: T_obj_world = self.stable_pose.T_obj_table.as_frames( 'obj', 'world') T_camera_obj = T_obj_world.inverse() * self.T_camera_world return T_camera_obj
def test_inverse(self): R_a_b = RigidTransform.random_rotation() t_a_b = RigidTransform.random_translation() T_a_b = RigidTransform(R_a_b, t_a_b, 'a', 'b') T_b_a = T_a_b.inverse() # multiple with numpy arrays M_a_b = np.r_[np.c_[R_a_b, t_a_b], [[0, 0, 0, 1]]] M_b_a = np.linalg.inv(M_a_b) self.assertTrue(np.sum(np.abs(T_b_a.matrix - M_b_a)) < 1e-5, msg='Inverse gave incorrect transformation') # check frames self.assertEqual(T_b_a.from_frame, 'b', msg='Inverse has incorrect input frame') self.assertEqual(T_b_a.to_frame, 'a', msg='Inverse has incorrect output frame')
def execute_grasp(T_gripper_world, robot, arm, subscriber, config): """ Executes a single grasp for the hand pose T_gripper_world up to the point of lifting the object """ # snap gripper to valid depth if T_gripper_world.translation[2] < config['grasping']['min_gripper_depth']: T_gripper_world.translation[2] = config['grasping'][ 'min_gripper_depth'] # get cur pose T_cur_world = arm.get_pose() # compute approach pose t_approach_target = np.array([0, 0, config['grasping']['approach_dist']]) T_gripper_approach = RigidTransform(translation=t_approach_target, from_frame='gripper', to_frame='gripper') T_approach_world = T_gripper_world * T_gripper_approach.inverse() t_lift_target = np.array([0, 0, config['grasping']['lift_height']]) T_gripper_lift = RigidTransform(translation=t_lift_target, from_frame='gripper', to_frame='gripper') T_lift_world = T_gripper_world * T_gripper_lift.inverse() # compute lift pose t_delta_approach = T_approach_world.translation - T_cur_world.translation # perform grasp on the robot, up until the point of lifting for _ in range(10): _, torques = subscriber.left.get_torque() resting_torque = torques[3] arm.open_gripper(wait_for_res=True) robot.set_z(config['control']['approach_zoning']) arm.goto_pose(YMC.L_KINEMATIC_AVOIDANCE_POSE) arm.goto_pose(T_approach_world) # grasp robot.set_v(config['control']['approach_velocity']) robot.set_z(config['control']['standard_zoning']) if config['control']['test_collision']: robot.set_z('z200') T_gripper_world.translation[2] = 0.0 arm.goto_pose(T_gripper_world, wait_for_res=True) _, T_cur_gripper_world = subscriber.left.get_pose() dist_from_goal = np.linalg.norm(T_cur_gripper_world.translation - T_gripper_world.translation) collision = False for i in range(10): _, torques = subscriber.left.get_torque() while dist_from_goal > 1e-3: _, T_cur_gripper_world = subscriber.left.get_pose() dist_from_goal = np.linalg.norm(T_cur_gripper_world.translation - T_gripper_world.translation) _, torques = subscriber.left.get_torque() print torques if torques[1] > 0.001: logging.info('Detected collision!!!!!!') robot.set_z('fine') arm.goto_pose(T_approach_world, wait_for_res=True) logging.info('Commanded!!!!!!') collision = True break arm.goto_pose(T_gripper_world, wait_for_res=False) else: arm.goto_pose(T_gripper_world, wait_for_res=True) # pick up object arm.close_gripper(force=config['control']['gripper_close_force'], wait_for_res=True) pickup_gripper_width = arm.get_gripper_width() robot.set_v(config['control']['standard_velocity']) robot.set_z(config['control']['standard_zoning']) arm.goto_pose(T_lift_world) arm.goto_pose(YMC.L_KINEMATIC_AVOIDANCE_POSE, wait_for_res=True) arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=True) # shake test if config['control']['shake_test']: # compute shake poses radius = config['control']['shake_radius'] angle = config['control']['shake_angle'] * np.pi delta_T = RigidTransform(translation=[0, 0, radius], from_frame='gripper', to_frame='gripper') R_shake = np.array([[1, 0, 0], [0, np.cos(angle), -np.sin(angle)], [0, np.sin(angle), np.cos(angle)]]) delta_T_up = RigidTransform(rotation=R_shake, translation=[0, 0, -radius], from_frame='gripper', to_frame='gripper') delta_T_down = RigidTransform(rotation=R_shake.T, translation=[0, 0, -radius], from_frame='gripper', to_frame='gripper') T_shake_up = YMC.L_PREGRASP_POSE.as_frames( 'gripper', 'world') * delta_T_up * delta_T T_shake_down = YMC.L_PREGRASP_POSE.as_frames( 'gripper', 'world') * delta_T_down * delta_T robot.set_v(config['control']['shake_velocity']) robot.set_z(config['control']['shake_zoning']) for i in range(config['control']['num_shakes']): arm.goto_pose(T_shake_up, wait_for_res=False) arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=False) arm.goto_pose(T_shake_down, wait_for_res=False) arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=False) robot.set_v(config['control']['standard_velocity']) robot.set_z(config['control']['standard_zoning']) # check gripper width for _ in range(10): _, torques = subscriber.left.get_torque() lift_torque = torques[3] lift_gripper_width = arm.get_gripper_width() # check drops lifted_object = False if np.abs(lift_gripper_width) > config['grasping']['pickup_min_width']: lifted_object = True # check lifts # table_clear = False # for i in range(3): # _, depth_im, _ = sensor.frames() return lifted_object, lift_gripper_width, lift_torque
[ 0.00913315, -0.80663693, -0.59097669]]) tra = np.array( [-0.14501467, -1.161184 , 0.3797466 ]) T_ar_cam = RigidTransform(rot, tra, 'ar_marker_2_0', 'base') T_ar_cam.from_frame = 'ar_marker_0' else: print('Getting transform from robot to AR tag...\n') T_world_ar = lookup_transform('ar_marker_0', 'base') print(T_world_ar) print('Getting transform from camera to AR tag...\n') T_ar_cam = lookup_transform('camera_depth_optical_frame', 'ar_marker_2_0') print(T_ar_cam) T_ar_cam.from_frame = 'ar_marker_0' T_ar_world = T_world_ar.inverse() T_cam_ar = T_ar_cam.inverse() ## Load and pre-process mesh ## filename = args.obj print('Loading mesh {}...\n'.format(filename)) mesh = trimesh.load_mesh(filename) mesh.fix_normals() T_cam_obj = RigidTransform(T_cam_ar.rotation, mesh.centroid) T_obj_cam = T_cam_obj.inverse() ## Visualize the mesh ## print('Visualizing mesh (close visualizer window to continue)...\n') utils.visualize_scene(mesh, T_world_ar, T_ar_cam, T_cam_obj)
1022.3043212890625, 777.7421875, height=1536, width=2048) T_tag_camera = april.detect(sensor, intr, vis=cfg['vis_detect'])[0] T_tag_world = T_camera_world * T_tag_camera logging.info('Tag has translation {}'.format(T_tag_world.translation)) import ipdb ipdb.set_trace() T_tag_tool = RigidTransform(rotation=np.eye(3), translation=[0, 0, 0.04], from_frame=T_tag_world.from_frame, to_frame="franka_tool") T_tool_world = T_tag_world * T_tag_tool.inverse() fa.goto_pose_with_cartesian_control( T_tool_world, cartesian_impedances=[2000, 2000, 1000, 300, 300, 300]) logging.info('Finding closest orthogonal grasp') T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world) T_lift = RigidTransform(translation=[0, 0, 0.2], from_frame=T_ready_world.to_frame, to_frame=T_ready_world.to_frame) T_lift_world = T_lift * T_grasp_world logging.info('Visualizing poses') _, depth_im, _ = sensor.frames() points_world = T_camera_world * intr.deproject(depth_im) if cfg['vis_detect']:
def register_ensenso(config): # set parameters average = True add_to_buffer = True clear_buffer = False decode = True grid_spacing = -1 # read parameters num_patterns = config["num_patterns"] max_tries = config["max_tries"] # load tf pattern to world T_pattern_world = RigidTransform.load(config["pattern_tf"]) # initialize sensor sensor_frame = config["sensor_frame"] sensor_config = {"frame": sensor_frame} logging.info("Creating sensor") sensor = RgbdSensorFactory.sensor("ensenso", sensor_config) # initialize node rospy.init_node("register_ensenso", anonymous=True) rospy.wait_for_service("/%s/collect_pattern" % (sensor_frame)) rospy.wait_for_service("/%s/estimate_pattern_pose" % (sensor_frame)) # start sensor print("Starting sensor") sensor.start() time.sleep(1) print("Sensor initialized") # perform registration try: print("Collecting patterns") num_detected = 0 i = 0 while num_detected < num_patterns and i < max_tries: collect_pattern = rospy.ServiceProxy( "/%s/collect_pattern" % (sensor_frame), CollectPattern) resp = collect_pattern(add_to_buffer, clear_buffer, decode, grid_spacing) if resp.success: print("Detected pattern %d" % (num_detected)) num_detected += 1 i += 1 if i == max_tries: raise ValueError("Failed to detect calibration pattern!") print("Estimating pattern pose") estimate_pattern_pose = rospy.ServiceProxy( "/%s/estimate_pattern_pose" % (sensor_frame), EstimatePatternPose) resp = estimate_pattern_pose(average) q_pattern_camera = np.array([ resp.pose.orientation.w, resp.pose.orientation.x, resp.pose.orientation.y, resp.pose.orientation.z, ]) t_pattern_camera = np.array( [resp.pose.position.x, resp.pose.position.y, resp.pose.position.z]) T_pattern_camera = RigidTransform( rotation=q_pattern_camera, translation=t_pattern_camera, from_frame="pattern", to_frame=sensor_frame, ) except rospy.ServiceException as e: print("Service call failed: %s" % (str(e))) # compute final transformation T_camera_world = T_pattern_world * T_pattern_camera.inverse() # save final transformation output_dir = os.path.join(config["calib_dir"], sensor_frame) if not os.path.exists(output_dir): os.makedirs(output_dir) pose_filename = os.path.join(output_dir, "%s_to_world.tf" % (sensor_frame)) T_camera_world.save(pose_filename) intr_filename = os.path.join(output_dir, "%s.intr" % (sensor_frame)) sensor.ir_intrinsics.save(intr_filename) # log final transformation print("Final Result for sensor %s" % (sensor_frame)) print("Rotation: ") print(T_camera_world.rotation) print("Quaternion: ") print(T_camera_world.quaternion) print("Translation: ") print(T_camera_world.translation) # stop sensor sensor.stop() # move robot to calib pattern center if config["use_robot"]: # create robot pose relative to target point target_pt_camera = Point(T_pattern_camera.translation, sensor_frame) target_pt_world = T_camera_world * target_pt_camera R_gripper_world = np.array([[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]]) t_gripper_world = np.array([ target_pt_world.x + config["gripper_offset_x"], target_pt_world.y + config["gripper_offset_y"], target_pt_world.z + config["gripper_offset_z"], ]) T_gripper_world = RigidTransform( rotation=R_gripper_world, translation=t_gripper_world, from_frame="gripper", to_frame="world", ) # move robot to pose y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF) y.reset_home() time.sleep(1) T_lift = RigidTransform(translation=(0, 0, 0.05), from_frame="world", to_frame="world") T_gripper_world_lift = T_lift * T_gripper_world y.right.goto_pose(T_gripper_world_lift) y.right.goto_pose(T_gripper_world) # wait for human measurement keyboard_input("Take measurement. Hit [ENTER] when done") y.right.goto_pose(T_gripper_world_lift) y.reset_home() y.stop()
xend = sx * (nx / 2 - ((nx + 1) % 2) / 2.0 + 1) ystart = -sy * (ny / 2 - ((ny + 1) % 2) / 2.0) yend = sy * (ny / 2 - ((ny + 1) % 2) / 2.0 + 1) filler = np.mgrid[ystart:yend:sy, xstart:xend:sx] filler = filler.reshape( (filler.shape[0], filler.shape[1] * filler.shape[2])).T objp[:, :2] = filler ret, rvec, tvec = cv2.solvePnP(objp, webcam_corner_px, intrinsics.K, None) mat, _ = cv2.Rodrigues(rvec) T_cb_cam = RigidTransform(mat, tvec, from_frame='cb', to_frame=sensor_frame) T_cam_cb = T_cb_cam.inverse() T_camera_world = T_cb_world.dot(T_cam_cb) logging.info('Final Result for sensor %s' % (sensor_frame)) logging.info('Translation: ') logging.info(T_camera_world.translation) logging.info('Rotation: ') logging.info(T_camera_world.rotation) except Exception as e: logging.error('Failed to register sensor {}'.format(sensor_frame)) traceback.print_exc() continue # save tranformation arrays based on setup output_dir = os.path.join(config['calib_dir'], sensor_frame)
from_frame='ycb_model', to_frame='source_model') fmt = {} for j in target_model_json['exported_objects']: if j['class'] == MODEL_NAME: fmt = j['fixed_model_transform'] break target_fixed_model_transform_mat = np.transpose(np.array(fmt)) target_fixed_model_transform = RigidTransform( rotation=target_fixed_model_transform_mat[:3, :3], translation=target_fixed_model_transform_mat[:3, 3], from_frame='ycb_model', to_frame='target_model') model_transform = fixed_model_transform.dot( target_fixed_model_transform.inverse()) (translation, rotation) = object_pose_from_json(reference_source_json['objects'][0]) reference_source_pose = RigidTransform(rotation=rotation, translation=translation, from_frame='target_model', to_frame='camera') (translation, rotation) = object_pose_from_json(reference_target_json['objects'][0]) reference_target_pose = RigidTransform(rotation=rotation, translation=translation, from_frame='target_model_fixed', to_frame='camera')
class FrankaArm: def __init__(self, rosnode_name='franka_arm_client'): self._connected = False self._in_skill = False # set signal handler to handle ctrl+c and kill sigs signal.signal(signal.SIGINT, self._sigint_handler_gen()) # init ROS rospy.init_node(rosnode_name, disable_signals=True) self._sub = FrankaArmSubscriber(new_ros_node=False) self._client = actionlib.SimpleActionClient('/execute_skill_action_server_node/execute_skill', ExecuteSkillAction) self._client.wait_for_server() self._current_goal_handle = None self._connected = True # set default identity tool delta pose self._tool_delta_pose = RigidTransform(from_frame='franka_tool', to_frame='franka_tool_base') def _sigint_handler_gen(self): def sigint_handler(sig, frame): if self._connected and self._in_skill: self._client.cancel_goal() sys.exit(0) return sigint_handler def _send_goal(self, goal, cb): ''' Raises: FrankaArmCommException if a timeout is reached ''' # TODO(jacky): institute a timeout check self._client.send_goal(goal, feedback_cb=cb) self._in_skill = True self._client.wait_for_result() self._in_skill = False return self._client.get_result() ''' Controls ''' def goto_pose(self, tool_pose, duration=3): '''Commands Arm to the given pose via linear interpolation Args: tool_pose (RigidTransform) : End-effector pose in tool frame duration (float) : How much time this robot motion should take Raises: FrankaArmCollisionException if a collision is detected ''' if pose.from_frame != 'franka_tool' or pose.to_frame != 'world': raise ValueError('pose has invalid frame names! Make sure pose has from_frame=franka_tool and to_frame=world') tool_base_pose = tool_pose * self._tool_delta_pose.inverse() skill = ArmMoveToGoalWithDefaultSensorSkill() skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_feedback_controller_params(FC.DEFAULT_TORQUE_CONTROLLER_PARAMS) skill.add_termination_params(FC.DEFAULT_TERM_PARAMS) skill.add_trajectory_params([duration] + tool_base_pose.matrix.T.flatten().tolist()) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x)) def goto_pose_delta(self, delta_tool_pose, duration=3): '''Commands Arm to the given delta pose via linear interpolation Args: delta_tool_pose (RigidTransform) : Delta pose in tool frame duration (float) : How much time this robot motion should take Raises: FrankaArmCollisionException if a collision is detected ''' if delta_tool_pose.from_frame != 'franka_tool' or delta_tool_pose.to_frame != 'franka_tool': raise ValueError('delta_pose has invalid frame names! Make sure delta_pose has from_frame=franka_tool and to_frame=franka_tool') delta_tool_base_pose = self._tool_delta_pose * delta_tool_pose * self._tool_delta_pose.inverse() skill = ArmRelativeMotionWithDefaultSensorSkill() skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_feedback_controller_params(FC.DEFAULT_TORQUE_CONTROLLER_PARAMS) skill.add_termination_params(FC.DEFAULT_TERM_PARAMS) skill.add_trajectory_params([duration] + delta_tool_base_pose.translation.tolist() + delta_tool_base_pose.quaternion.tolist()) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x)) def goto_joints(self, joints, duration=3): '''Commands Arm to the given joint configuration Args: joints (list): A list of 7 numbers that correspond to joint angles in radians duration (float) : How much time this robot motion should take Raises: ValueError: If is_joints_reachable(joints) returns False FrankaArmCollisionException if a collision is detected ''' if not self.is_joints_reachable(joints): raise ValueError('Joints not reachable!') skill = JointPoseWithDefaultSensorSkill() skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_termination_params(FC.DEFAULT_TERM_PARAMS) skill.add_trajectory_params([duration] + np.array(joints).tolist()) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x)) def apply_joint_torques(self, torques, duration): '''Commands Arm to apply given joint torques for duration seconds Args: torques (list): A list of 7 numbers that correspond to torques in Nm duration (float): A float in the unit of seconds Raises: FrankaArmCollisionException if a collision is detected ''' pass def apply_effector_forces_torques(self, duration, forces=None, torques=None): '''Applies the given end-effector forces and torques in N and Nm Args: duration (float): A float in the unit of seconds forces (list): Optional (defaults to None). A list of 3 numbers that correspond to end-effector forces in 3 directions torques (list): Optional (defaults to None). A list of 3 numbers that correspond to end-effector torques in 3 axes Raises: FrankaArmCollisionException if a collision is detected ''' forces = [0, 0, 0] if forces is None else np.array(forces).tolist() torques = [0, 0, 0] if torques is None else np.array(torques).tolist() if np.linalg.norm(forces) > FC.MAX_FORCE_NORM: raise ValueError('Force magnitude exceeds safety threshold of {}'.format(FC.MAX_FORCE_NORM)) if np.linalg.norm(torques) > FC.MAX_TORQUE_NORM: raise ValueError('Torque magnitude exceeds safety threshold of {}'.format(FC.MAX_TORQUE_NORM)) skill = ForceTorqueSkill() skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_termination_params([duration]) skill.add_trajectory_params([0] + forces + torques) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x)) def goto_gripper(self, width, speed=0.04, force=None): '''Commands gripper to goto a certain width, applying up to the given (default is max) force if needed Args: width (float): A float in the unit of meters speed (float): Gripper operation speed in meters per sec force (float): Max gripper force to apply in N. Default to None, which gives acceptable force Raises: ValueError: If width is less than 0 or greater than TODO(jacky) the maximum gripper opening ''' skill = GripperWithDefaultSensorSkill() skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) # TODO(jacky): why is wait time needed? if force is not None: skill.add_trajectory_params([width, speed, force, FC.GRIPPER_WAIT_TIME]) # Gripper Width, Gripper Speed, Wait Time else: skill.add_trajectory_params([width, speed, FC.GRIPPER_WAIT_TIME]) # Gripper Width, Gripper Speed, Wait Time goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x)) def open_gripper(self): '''Opens gripper to maximum width ''' self.goto_gripper(FC.GRIPPER_WIDTH_MAX) def close_gripper(self, grasp=True): '''Closes the gripper as much as possible ''' self.goto_gripper(FC.GRIPPER_WIDTH_MIN, force=FC.GRIPPER_MAX_FORCE if grasp else None) ''' Reads ''' def get_robot_state(self): ''' Returns: dict of full robot state data ''' return self._sub.get_data() def get_pose(self): ''' Returns: pose (RigidTransform) of the current end-effector ''' tool_base_pose = self._sub.get_pose() tool_pose = tool_base_pose * self._tool_delta_pose return tool_pose def get_joints(self): ''' Returns: ndarray of shape (7,) of joint angles in radians ''' return self._sub.get_joints() def get_joint_torques(self): ''' Returns: ndarray of shape (7,) of joint torques in Nm ''' return self._sub.get_joint_torques() def get_joint_velocities(self): ''' Returns: ndarray of shape (7,) of joint velocities in rads/s ''' return self._sub.get_joint_velocities() def get_gripper_width(self): ''' Returns: float of gripper width in meters ''' return self._sub.get_gripper_width() def get_gripper_is_grasped(self): ''' Returns: True if gripper is grasping something. False otherwise ''' return self._sub.get_gripper_is_grasped() def get_speed(self, speed): ''' Returns: float of current target speed parameter ''' pass def get_tool_base_pose(self): ''' Returns: RigidTransform of current tool base pose ''' return self._tool_delta_pose.copy() ''' Sets ''' def set_tool_delta_pose(self, tool_delta_pose): '''Sets the tool pose relative to the end-effector pose Args: tool_delta_pose (RigidTransform) ''' if tool_delta_pose.from_frame != 'franka_tool' or tool_delta_pose.to_frame != 'franka_tool_base': raise ValueError('tool_delta_pose has invalid frame names! Make sure the has from_frame=franka_tool, and to_frame=franka_tool_base') self._tool_delta_pose = tool_delta_pose.copy() def set_speed(self, speed): '''Sets current target speed parameter Args: speed (float) ''' pass ''' Misc ''' def reset_joints(self): '''Commands Arm to goto hardcoded home joint configuration Raises: FrankaArmCollisionException if a collision is detected ''' self.goto_joints(FC.HOME_JOINTS, duration=5) def is_joints_reachable(self, joints): ''' Returns: True if all joints within joint limits ''' for i, val in enumerate(joints): if val <= FC.JOINT_LIMITS_MIN[i] or val >= FC.JOINT_LIMITS_MAX[i]: return False return True
def register_ensenso(config): # set parameters average = True add_to_buffer = True clear_buffer = False decode = True grid_spacing = -1 # read parameters num_patterns = config['num_patterns'] max_tries = config['max_tries'] # load tf pattern to world T_pattern_world = RigidTransform.load(config['pattern_tf']) # initialize sensor sensor_frame = config['sensor_frame'] sensor = EnsensoSensor(sensor_frame) # initialize node rospy.init_node('register_ensenso', anonymous=True) rospy.wait_for_service('/%s/collect_pattern' %(sensor_frame)) rospy.wait_for_service('/%s/estimate_pattern_pose' %(sensor_frame)) # start sensor print('Starting sensor') sensor.start() time.sleep(1) print('Sensor initialized') # perform registration try: print('Collecting patterns') num_detected = 0 i = 0 while num_detected < num_patterns and i < max_tries: collect_pattern = rospy.ServiceProxy('/%s/collect_pattern' %(sensor_frame), CollectPattern) resp = collect_pattern(add_to_buffer, clear_buffer, decode, grid_spacing) if resp.success: print('Detected pattern %d' %(num_detected)) num_detected += 1 i += 1 if i == max_tries: raise ValueError('Failed to detect calibration pattern!') print('Estimating pattern pose') estimate_pattern_pose = rospy.ServiceProxy('/%s/estimate_pattern_pose' %(sensor_frame), EstimatePatternPose) resp = estimate_pattern_pose(average) q_pattern_camera = np.array([resp.pose.orientation.w, resp.pose.orientation.x, resp.pose.orientation.y, resp.pose.orientation.z]) t_pattern_camera = np.array([resp.pose.position.x, resp.pose.position.y, resp.pose.position.z]) T_pattern_camera = RigidTransform(rotation=q_pattern_camera, translation=t_pattern_camera, from_frame='pattern', to_frame=sensor_frame) except rospy.ServiceException as e: print('Service call failed: %s' %(str(e))) # compute final transformation T_camera_world = T_pattern_world * T_pattern_camera.inverse() # save final transformation output_dir = os.path.join(config['calib_dir'], sensor_frame) if not os.path.exists(output_dir): os.makedirs(output_dir) pose_filename = os.path.join(output_dir, '%s_to_world.tf' %(sensor_frame)) T_camera_world.save(pose_filename) intr_filename = os.path.join(output_dir, '%s.intr' %(sensor_frame)) sensor.ir_intrinsics.save(intr_filename) # log final transformation print('Final Result for sensor %s' %(sensor_frame)) print('Rotation: ') print(T_camera_world.rotation) print('Quaternion: ') print(T_camera_world.quaternion) print('Translation: ') print(T_camera_world.translation) # stop sensor sensor.stop() # move robot to calib pattern center if config['use_robot']: # create robot pose relative to target point target_pt_camera = Point(T_pattern_camera.translation, sensor_frame) target_pt_world = T_camera_world * target_pt_camera R_gripper_world = np.array([[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]]) t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'], target_pt_world.y + config['gripper_offset_y'], target_pt_world.z + config['gripper_offset_z']]) T_gripper_world = RigidTransform(rotation=R_gripper_world, translation=t_gripper_world, from_frame='gripper', to_frame='world') # move robot to pose y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF) y.reset_home() time.sleep(1) T_lift = RigidTransform(translation=(0,0,0.05), from_frame='world', to_frame='world') T_gripper_world_lift = T_lift * T_gripper_world y.right.goto_pose(T_gripper_world_lift) y.right.goto_pose(T_gripper_world) # wait for human measurement yesno = raw_input('Take measurement. Hit [ENTER] when done') y.right.goto_pose(T_gripper_world_lift) y.reset_home() y.stop()
def object_to_camera_poses(self): """Turn the params into a set of object to camera transformations. Returns ------- :obj:`list` of :obj:`RigidTransform` A list of rigid transformations that transform from object space to camera space. """ # compute increments in radial coordinates if self.max_radius == self.min_radius: radius_inc = 1 elif self.num_radii == 1: radius_inc = self.max_radius - self.min_radius + 1 else: radius_inc = (self.max_radius - self.min_radius) / (self.num_radii - 1) az_inc = (self.max_az - self.min_az) / self.num_az if self.max_elev == self.min_elev: elev_inc = 1 elif self.num_elev == 1: elev_inc = self.max_elev - self.min_elev + 1 else: elev_inc = (self.max_elev - self.min_elev) / (self.num_elev - 1) roll_inc = (self.max_roll - self.min_roll) / self.num_roll # create a pose for each set of spherical coords object_to_camera_poses = [] radius = self.min_radius while radius <= self.max_radius: elev = self.min_elev while elev <= self.max_elev: az = self.min_az while az < self.max_az: #not inclusive due to topology (simplifies things) roll = self.min_roll while roll < self.max_roll: # generate camera center from spherical coords camera_center_obj = np.array([sph2cart(radius, az, elev)]).squeeze() camera_z_obj = -camera_center_obj / np.linalg.norm(camera_center_obj) # find the canonical camera x and y axes camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0]) if np.linalg.norm(camera_x_par_obj) == 0: camera_x_par_obj = np.array([1, 0, 0]) camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj) camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) if camera_y_par_obj[2] > 0: camera_x_par_obj = -camera_x_par_obj camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) # rotate by the roll R_obj_camera_par = np.c_[camera_x_par_obj, camera_y_par_obj, camera_z_obj] R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0], [np.sin(roll), np.cos(roll), 0], [0, 0, 1]]) R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera) t_obj_camera = camera_center_obj # create final transform T_obj_camera = RigidTransform(R_obj_camera, t_obj_camera, from_frame='camera', to_frame='obj') object_to_camera_poses.append(T_obj_camera.inverse()) roll += roll_inc az += az_inc elev += elev_inc radius += radius_inc return object_to_camera_poses
def get_camera_chessboard(sensor_frame, sensor_type, device_num, config): # load cfg num_transform_avg = config['num_transform_avg'] num_images = config['num_images'] sx = config['corners_x'] sy = config['corners_y'] color_image_upsample_rate = config['color_image_upsample_rate'] vis = config['vis'] # open sensor if sensor_type.lower() in CameraChessboardRegister._SENSOR_TYPES: sensor = Kinect2Sensor( device_num=device_num, frame=sensor_frame, packet_pipeline_mode=Kinect2PacketPipelineMode.CPU) else: logging.warning( 'Could not register device at %s. Sensor type %s not supported' % (sensor_frame, sensor_type)) logging.info('Registering camera %s' % (sensor_frame)) sensor.start() # repeat registration multiple times and average results R = np.zeros([3, 3]) t = np.zeros([3, 1]) points_3d_plane = PointCloud(np.zeros([3, sx * sy]), frame=sensor.ir_frame) k = 0 while k < num_transform_avg: # average a bunch of depth images together depth_ims = np.zeros([ Kinect2Sensor.DEPTH_IM_HEIGHT, Kinect2Sensor.DEPTH_IM_WIDTH, num_images ]) for i in range(num_images): small_color_im, new_depth_im, _ = sensor.frames() depth_ims[:, :, i] = new_depth_im.data med_depth_im = np.median(depth_ims, axis=2) depth_im = DepthImage(med_depth_im, sensor.ir_frame) # find the corner pixels in an upsampled version of the color image big_color_im = small_color_im.resize(color_image_upsample_rate) corner_px = CameraChessboardRegister._find_chessboard(big_color_im, sx=sx, sy=sy, vis=vis) if corner_px is None: logging.error('No chessboard detected') continue # convert back to original image small_corner_px = corner_px / color_image_upsample_rate if vis: plt.figure() plt.imshow(small_color_im.data) for i in range(sx): plt.scatter(small_corner_px[i, 0], small_corner_px[i, 1], s=25, c='b') plt.axis('off') plt.show() # project points into 3D camera_intr = sensor.ir_intrinsics points_3d = camera_intr.deproject(depth_im) # get round chessboard ind corner_px_round = np.round(small_corner_px).astype(np.uint16) corner_ind = depth_im.ij_to_linear(corner_px_round[:, 0], corner_px_round[:, 1]) if corner_ind.shape[0] != sx * sy: print 'Did not find all corners. Discarding...' continue # average 3d points points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1) logging.info('Registration iteration %d of %d' % (k + 1, config['num_transform_avg'])) k += 1 # fit a plane to the chessboard corners X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)] y = points_3d_plane.z_coords A = X.T.dot(X) b = X.T.dot(y) w = np.linalg.inv(A).dot(b) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) mean_point_plane = points_3d_plane.mean() # find x-axis of the chessboard coordinates on the fitted plane T_camera_table = RigidTransform( translation=-points_3d_plane.mean().data, from_frame=points_3d_plane.frame, to_frame='table') points_3d_centered = T_camera_table * points_3d_plane # get points along y coord_pos_y = int(math.floor(sx * (sy - 1) / 2.0)) coord_neg_y = int(math.ceil(sx * (sy + 1) / 2.0)) points_pos_y = points_3d_centered[:coord_pos_y] points_neg_y = points_3d_centered[coord_neg_y:] y_axis = np.mean(points_pos_y.data, axis=1) - np.mean( points_neg_y.data, axis=1) y_axis = y_axis - np.vdot(y_axis, n) * n y_axis = y_axis / np.linalg.norm(y_axis) x_axis = np.cross(-n, y_axis) # WARNING! May need symmetry breaking but it appears the points are ordered consistently # produce translation and rotation from plane center and chessboard basis rotation_cb_camera = rotation_from_axes(x_axis, y_axis, n) translation_cb_camera = mean_point_plane.data T_cb_camera = RigidTransform(rotation=rotation_cb_camera, translation=translation_cb_camera, from_frame='cb', to_frame=sensor.frame) T_camera_cb = T_cb_camera.inverse() cb_points_cam = points_3d[corner_ind] # optionally display cb corners with detected pose in 3d space if config['debug']: # display image with axes overlayed cb_center_im = camera_intr.project( Point(T_cb_camera.translation, frame=sensor.ir_frame)) cb_x_im = camera_intr.project( Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_y_im = camera_intr.project( Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_z_im = camera_intr.project( Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame)) x_line = np.array([cb_center_im.data, cb_x_im.data]) y_line = np.array([cb_center_im.data, cb_y_im.data]) z_line = np.array([cb_center_im.data, cb_z_im.data]) plt.figure() plt.imshow(small_color_im.data) plt.scatter(cb_center_im.data[0], cb_center_im.data[1]) plt.plot(x_line[:, 0], x_line[:, 1], c='r', linewidth=3) plt.plot(y_line[:, 0], y_line[:, 1], c='g', linewidth=3) plt.plot(z_line[:, 0], z_line[:, 1], c='b', linewidth=3) plt.axis('off') plt.title('Chessboard frame in camera %s' % (sensor.frame)) plt.show() return T_camera_cb, cb_points_cam, points_3d_plane
def object_to_camera_poses(self, camera_intr): """Turn the params into a set of object to camera transformations. Returns ------- :obj:`list` of :obj:`RigidTransform` A list of rigid transformations that transform from object space to camera space. :obj:`list` of :obj:`RigidTransform` A list of rigid transformations that transform from object space to camera space without the translation in the plane :obj:`list` of :obj:`CameraIntrinsics` A list of camera intrinsics that project the translated object into the center pixel of the camera, simulating cropping """ # compute increments in radial coordinates if self.max_radius == self.min_radius: radius_inc = 1 elif self.num_radii == 1: radius_inc = self.max_radius - self.min_radius + 1 else: radius_inc = (self.max_radius - self.min_radius) / (self.num_radii - 1) az_inc = (self.max_az - self.min_az) / self.num_az if self.max_elev == self.min_elev: elev_inc = 1 elif self.num_elev == 1: elev_inc = self.max_elev - self.min_elev + 1 else: elev_inc = (self.max_elev - self.min_elev) / (self.num_elev - 1) roll_inc = (self.max_roll - self.min_roll) / self.num_roll if self.max_x == self.min_x: x_inc = 1 elif self.num_x == 1: x_inc = self.max_x - self.min_x + 1 else: x_inc = (self.max_x - self.min_x) / (self.num_x - 1) if self.max_y == self.min_y: y_inc = 1 elif self.num_y == 1: y_inc = self.max_y - self.min_y + 1 else: y_inc = (self.max_y - self.min_y) / (self.num_y - 1) # create a pose for each set of spherical coords object_to_camera_poses = [] object_to_camera_normalized_poses = [] camera_shifted_intrinsics = [] radius = self.min_radius while radius <= self.max_radius: elev = self.min_elev while elev <= self.max_elev: az = self.min_az while az < self.max_az: #not inclusive due to topology (simplifies things) roll = self.min_roll while roll < self.max_roll: x = self.min_x while x <= self.max_x: y = self.min_y while y <= self.max_y: num_poses = len(object_to_camera_poses) # generate camera center from spherical coords delta_t = np.array([x, y, 0]) camera_center_obj = np.array([sph2cart(radius, az, elev)]).squeeze() + delta_t camera_z_obj = -np.array([sph2cart(radius, az, elev)]).squeeze() camera_z_obj = camera_z_obj / np.linalg.norm(camera_z_obj) # find the canonical camera x and y axes camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0]) if np.linalg.norm(camera_x_par_obj) == 0: camera_x_par_obj = np.array([1, 0, 0]) camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj) camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) if camera_y_par_obj[2] > 0: print 'Flipping', num_poses camera_x_par_obj = -camera_x_par_obj camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj) camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj) # rotate by the roll R_obj_camera_par = np.c_[camera_x_par_obj, camera_y_par_obj, camera_z_obj] R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0], [np.sin(roll), np.cos(roll), 0], [0, 0, 1]]) R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera) t_obj_camera = camera_center_obj # create final transform T_obj_camera = RigidTransform(R_obj_camera, t_obj_camera, from_frame='camera', to_frame='obj') object_to_camera_poses.append(T_obj_camera.inverse()) # compute pose without the center offset because we can easily add in the object offset later t_obj_camera_normalized = camera_center_obj - delta_t T_obj_camera_normalized = RigidTransform(R_obj_camera, t_obj_camera_normalized, from_frame='camera', to_frame='obj') object_to_camera_normalized_poses.append(T_obj_camera_normalized.inverse()) # compute new camera center by projecting object 0,0,0 into the camera center_obj_obj = Point(np.zeros(3), frame='obj') center_obj_camera = T_obj_camera.inverse() * center_obj_obj u_center_obj = camera_intr.project(center_obj_camera) camera_shifted_intr = copy.deepcopy(camera_intr) camera_shifted_intr.cx = 2 * camera_intr.cx - float(u_center_obj.x) camera_shifted_intr.cy = 2 * camera_intr.cy - float(u_center_obj.y) camera_shifted_intrinsics.append(camera_shifted_intr) y += y_inc x += x_inc roll += roll_inc az += az_inc elev += elev_inc radius += radius_inc return object_to_camera_poses, object_to_camera_normalized_poses, camera_shifted_intrinsics
def T_obj_world(self): T_world_obj = RigidTransform(rotation=self.r.T, translation=self.x0, from_frame='world', to_frame='obj') return T_world_obj.inverse()
def register(sensor, config): """ Registers a camera to a chessboard. Parameters ---------- sensor : :obj:`perception.RgbdSensor` the sensor to register config : :obj:`autolab_core.YamlConfig` or :obj:`dict` configuration file for registration Returns ------- :obj:`ChessboardRegistrationResult` the result of registration Notes ----- The config must have the parameters specified in the Other Parameters section. Other Parameters ---------------- num_transform_avg : int the number of independent registrations to average together num_images : int the number of images to read for each independent registration corners_x : int the number of chessboard corners in the x-direction corners_y : int the number of chessboard corners in the y-direction color_image_rescale_factor : float amount to rescale the color image for detection (numbers around 4-8 are useful) vis : bool whether or not to visualize the registration """ # read config num_transform_avg = config['num_transform_avg'] num_images = config['num_images'] sx = config['corners_x'] sy = config['corners_y'] point_order = config['point_order'] color_image_rescale_factor = config['color_image_rescale_factor'] flip_normal = config['flip_normal'] y_points_left = False if 'y_points_left' in config.keys() and sx == sy: y_points_left = config['y_points_left'] num_images = 1 vis = config['vis'] # read params from sensor logging.info('Registering camera %s' %(sensor.frame)) ir_intrinsics = sensor.ir_intrinsics # repeat registration multiple times and average results R = np.zeros([3,3]) t = np.zeros([3,1]) points_3d_plane = PointCloud(np.zeros([3, sx*sy]), frame=sensor.ir_frame) k = 0 while k < num_transform_avg: # average a bunch of depth images together depth_ims = None for i in range(num_images): start = time.time() small_color_im, new_depth_im, _ = sensor.frames() end = time.time() logging.info('Frames Runtime: %.3f' %(end-start)) if depth_ims is None: depth_ims = np.zeros([new_depth_im.height, new_depth_im.width, num_images]) depth_ims[:,:,i] = new_depth_im.data med_depth_im = np.median(depth_ims, axis=2) depth_im = DepthImage(med_depth_im, sensor.ir_frame) # find the corner pixels in an upsampled version of the color image big_color_im = small_color_im.resize(color_image_rescale_factor) corner_px = big_color_im.find_chessboard(sx=sx, sy=sy) if vis: plt.figure() plt.imshow(big_color_im.data) for i in range(sx): plt.scatter(corner_px[i,0], corner_px[i,1], s=25, c='b') plt.show() if corner_px is None: logging.error('No chessboard detected! Check camera exposure settings') continue # convert back to original image small_corner_px = corner_px / color_image_rescale_factor if vis: plt.figure() plt.imshow(small_color_im.data) for i in range(sx): plt.scatter(small_corner_px[i,0], small_corner_px[i,1], s=25, c='b') plt.axis('off') plt.show() # project points into 3D camera_intr = sensor.ir_intrinsics points_3d = camera_intr.deproject(depth_im) # get round chessboard ind corner_px_round = np.round(small_corner_px).astype(np.uint16) corner_ind = depth_im.ij_to_linear(corner_px_round[:,0], corner_px_round[:,1]) if corner_ind.shape[0] != sx*sy: logging.warning('Did not find all corners. Discarding...') continue # average 3d points points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1) logging.info('Registration iteration %d of %d' %(k+1, config['num_transform_avg'])) k += 1 # fit a plane to the chessboard corners X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)] y = points_3d_plane.z_coords A = X.T.dot(X) b = X.T.dot(y) w = np.linalg.inv(A).dot(b) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) if flip_normal: n = -n mean_point_plane = points_3d_plane.mean() # find x-axis of the chessboard coordinates on the fitted plane T_camera_table = RigidTransform(translation = -points_3d_plane.mean().data, from_frame=points_3d_plane.frame, to_frame='table') points_3d_centered = T_camera_table * points_3d_plane # get points along y if point_order == 'row_major': coord_pos_x = int(math.floor(sx*sy/2.0)) coord_neg_x = int(math.ceil(sx*sy/2.0)) points_pos_x = points_3d_centered[coord_pos_x:] points_neg_x = points_3d_centered[:coord_neg_x] x_axis = np.mean(points_pos_x.data, axis=1) - np.mean(points_neg_x.data, axis=1) x_axis = x_axis - np.vdot(x_axis, n)*n x_axis = x_axis / np.linalg.norm(x_axis) y_axis = np.cross(n, x_axis) else: coord_pos_y = int(math.floor(sx*(sy-1)/2.0)) coord_neg_y = int(math.ceil(sx*(sy+1)/2.0)) points_pos_y = points_3d_centered[:coord_pos_y] points_neg_y = points_3d_centered[coord_neg_y:] y_axis = np.mean(points_pos_y.data, axis=1) - np.mean(points_neg_y.data, axis=1) y_axis = y_axis - np.vdot(y_axis, n)*n y_axis = y_axis / np.linalg.norm(y_axis) x_axis = np.cross(-n, y_axis) # produce translation and rotation from plane center and chessboard basis rotation_cb_camera = RigidTransform.rotation_from_axes(x_axis, y_axis, n) translation_cb_camera = mean_point_plane.data T_cb_camera = RigidTransform(rotation=rotation_cb_camera, translation=translation_cb_camera, from_frame='cb', to_frame=sensor.frame) if y_points_left and np.abs(T_cb_camera.y_axis[1]) > 0.1: if T_cb_camera.x_axis[0] > 0: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(-np.pi/2).T) else: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(np.pi/2).T) T_camera_cb = T_cb_camera.inverse() # optionally display cb corners with detected pose in 3d space if config['debug']: # display image with axes overlayed cb_center_im = camera_intr.project(Point(T_cb_camera.translation, frame=sensor.ir_frame)) cb_x_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_y_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_z_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame)) x_line = np.array([cb_center_im.data, cb_x_im.data]) y_line = np.array([cb_center_im.data, cb_y_im.data]) z_line = np.array([cb_center_im.data, cb_z_im.data]) plt.figure() plt.imshow(small_color_im.data) plt.scatter(cb_center_im.data[0], cb_center_im.data[1]) plt.plot(x_line[:,0], x_line[:,1], c='r', linewidth=3) plt.plot(y_line[:,0], y_line[:,1], c='g', linewidth=3) plt.plot(z_line[:,0], z_line[:,1], c='b', linewidth=3) plt.axis('off') plt.title('Chessboard frame in camera %s' %(sensor.frame)) plt.show() return ChessboardRegistrationResult(T_camera_cb, points_3d_plane)
class FrankaArm: def __init__(self, rosnode_name='franka_arm_client', ros_log_level=rospy.INFO, robot_num=1): self._execute_skill_action_server_name = \ '/execute_skill_action_server_node_'+str(robot_num)+'/execute_skill' self._robot_state_server_name = \ '/get_current_robot_state_server_node_'+str(robot_num)+'/get_current_robot_state_server' self._robolib_status_server_name = \ '/get_current_robolib_status_server_node_'+str(robot_num)+'/get_current_robolib_status_server' self._connected = False self._in_skill = False # set signal handler to handle ctrl+c and kill sigs signal.signal(signal.SIGINT, self._sigint_handler_gen()) # init ROS rospy.init_node(rosnode_name, disable_signals=True, log_level=ros_log_level) rospy.wait_for_service(self._robolib_status_server_name) self._get_current_robolib_status = rospy.ServiceProxy( self._robolib_status_server_name, GetCurrentRobolibStatusCmd) self._state_client = FrankaArmStateClient( new_ros_node=False, robot_state_server_name=self._robot_state_server_name) self._client = actionlib.SimpleActionClient( self._execute_skill_action_server_name, ExecuteSkillAction) self._client.wait_for_server() self.wait_for_robolib() # done init ROS self._connected = True # set default identity tool delta pose self._tool_delta_pose = RigidTransform(from_frame='franka_tool', to_frame='franka_tool_base') def wait_for_robolib(self, timeout=None): '''Blocks execution until robolib gives ready signal. ''' timeout = FC.DEFAULT_ROBOLIB_TIMEOUT if timeout is None else timeout t_start = time() while time() - t_start < timeout: robolib_status = self._get_current_robolib_status().robolib_status if robolib_status.is_ready: return sleep(1e-2) raise FrankaArmCommException('Robolib status not ready for {}s'.format( FC.DEFAULT_ROBOLIB_TIMEOUT)) def _sigint_handler_gen(self): def sigint_handler(sig, frame): if self._connected and self._in_skill: self._client.cancel_goal() sys.exit(0) return sigint_handler def _send_goal(self, goal, cb, ignore_errors=True): ''' Raises: FrankaArmCommException if a timeout is reached FrankaArmException if robolib gives an error FrankaArmRobolibNotReadyException if robolib is not ready ''' self._in_skill = True self._client.send_goal(goal, feedback_cb=cb) done = False while not done: robolib_status = self._get_current_robolib_status().robolib_status e = None if rospy.is_shutdown(): e = RuntimeError('rospy is down!') elif robolib_status.error_description: e = FrankaArmException(robolib_status.error_description) elif not robolib_status.is_ready: e = FrankaArmRobolibNotReadyException() if e is not None: if ignore_errors: self.wait_for_robolib() break else: raise e done = self._client.wait_for_result( rospy.Duration.from_sec(FC.ACTION_WAIT_LOOP_TIME)) self._in_skill = False return self._client.get_result() ''' Controls ''' def goto_pose(self, tool_pose, duration=3, stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc='', skill_type=SkillType.ImpedanceControlSkill): '''Commands Arm to the given pose via linear interpolation Args: tool_pose (RigidTransform) : End-effector pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. ignore_errors : function ignores errors by default. If False, errors and some exceptions can be thrown skill_desc (string) : Skill description to use for logging on control-pc. ''' return self._goto_pose(tool_pose, duration, stop_on_contact_forces, cartesian_impedances, ignore_errors, skill_desc, skill_type) def goto_pose_with_cartesian_control(self, tool_pose, duration=3., stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc=''): '''Commands Arm to the given pose via min-jerk interpolation. Use Franka's internal Cartesian Controller to execute this skill. Args: tool_pose (RigidTransform) : End-effector pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. ignore_errors : function ignores errors by default. If False, errors and some exceptions can be thrown skill_desc (string) : Skill description to use for logging on control-pc. ''' return self._goto_pose(tool_pose, duration, stop_on_contact_forces, cartesian_impedances, ignore_errors, skill_desc, skill_type=SkillType.CartesianPoseSkill) def goto_pose_with_impedance_control(self, tool_pose, duration=3., stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc=''): '''Commands Arm to the given pose via min-jerk interpolation. Use our own impedance controller (use jacobian to convert cartesian poses to joints.) Args: tool_pose (RigidTransform) : End-effector pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. ignore_errors : function ignores errors by default. If False, errors and some exceptions can be thrown skill_desc (string) : Skill description to use for logging on control-pc. ''' return self._goto_pose(tool_pose, duration, stop_on_contact_forces, cartesian_impedances, ignore_errors, skill_desc, skill_type=SkillType.ImpedanceControlSkill) def _goto_pose(self, tool_pose, duration=3, stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc='', skill_type=None): '''Commands Arm to the given pose via linear interpolation Args: tool_pose (RigidTransform) : End-effector pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. ''' if tool_pose.from_frame != 'franka_tool' or tool_pose.to_frame != 'world': raise ValueError( 'pose has invalid frame names! Make sure pose has \ from_frame=franka_tool and to_frame=world') tool_base_pose = tool_pose * self._tool_delta_pose.inverse() skill = GoToPoseSkill(skill_desc, skill_type) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if cartesian_impedances is not None: skill.add_cartesian_impedances(cartesian_impedances) else: skill.add_feedback_controller_params( FC.DEFAULT_TORQUE_CONTROLLER_PARAMS) if stop_on_contact_forces is not None: skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME, stop_on_contact_forces, stop_on_contact_forces) else: skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) skill.add_goal_pose_with_matrix( duration, tool_base_pose.matrix.T.flatten().tolist()) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def goto_pose_delta(self, delta_tool_pose, duration=3, stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc='', skill_type=SkillType.ImpedanceControlSkill): '''Commands Arm to the given delta pose via linear interpolation and uses impedance control. Args: delta_tool_pose (RigidTransform) : Delta pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. ignore_errors : function ignores errors by default. If False, errors and some exceptions can be thrown skill_desc (string) : Skill description to use for logging on control-pc. ''' return self._goto_pose_delta(delta_tool_pose, duration, stop_on_contact_forces, cartesian_impedances, ignore_errors, skill_desc, skill_type) def goto_pose_delta_with_impedance_control(self, delta_tool_pose, duration=3, stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc=''): return self._goto_pose_delta( delta_tool_pose, duration, stop_on_contact_forces, cartesian_impedances, ignore_errors, skill_desc, skill_type=SkillType.ImpedanceControlSkill) def goto_pose_delta_with_cartesian_control(self, delta_tool_pose, duration=3, stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc=''): '''Commands Arm to the given delta pose via linear interpolation using franka's internal cartesian control. Args: delta_tool_pose (RigidTransform) : Delta pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. cartesian_impedance (list): List of 6 floats. Used to set the cartesian impedance of Franka's internal cartesian controller. List of (x, y, z, roll, pitch, yaw) ignore_errors : function ignores errors by default. If False, errors and some exceptions can be thrown skill_desc (string) : Skill description to use for logging on control-pc. ''' return self._goto_pose_delta(delta_tool_pose, duration, stop_on_contact_forces, cartesian_impedances, ignore_errors, skill_desc, skill_type=SkillType.CartesianPoseSkill) def _goto_pose_delta(self, delta_tool_pose, duration=3, stop_on_contact_forces=None, cartesian_impedances=None, ignore_errors=True, skill_desc='', skill_type=SkillType.ImpedanceControlSkill): '''Commands Arm to the given delta pose via linear interpolation Args: delta_tool_pose (RigidTransform) : Delta pose in tool frame duration (float) : How much time this robot motion should take stop_on_contact_forces (list): List of 6 floats corresponding to force limits on translation (xyz) and rotation about (xyz) axes. Default is None. If None then will not stop on contact. ''' if delta_tool_pose.from_frame != 'franka_tool' \ or delta_tool_pose.to_frame != 'franka_tool': raise ValueError('delta_pose has invalid frame names! ' \ 'Make sure delta_pose has from_frame=franka_tool ' \ 'and to_frame=franka_tool') delta_tool_base_pose = self._tool_delta_pose \ * delta_tool_pose * self._tool_delta_pose.inverse() skill = GoToPoseDeltaSkill(skill_desc, skill_type) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if cartesian_impedances is not None: skill.add_cartesian_impedances(cartesian_impedances) else: skill.add_feedback_controller_params( FC.DEFAULT_TORQUE_CONTROLLER_PARAMS) if stop_on_contact_forces is not None: skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME, stop_on_contact_forces, stop_on_contact_forces) else: skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) skill.add_relative_motion_with_quaternion( duration, delta_tool_base_pose.translation.tolist(), delta_tool_base_pose.quaternion.tolist()) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def goto_joints(self, joints, duration=5, stop_on_contact_forces=None, joint_impedances=None, k_gains=None, d_gains=None, ignore_errors=True, skill_desc='', skill_type=SkillType.JointPositionSkill): '''Commands Arm to the given joint configuration Args: joints (list): A list of 7 numbers that correspond to joint angles in radians duration (float): How much time this robot motion should take joint_impedances (list): A list of 7 numbers that represent the desired joint impedances for the internal robot joint controller Raises: ValueError: If is_joints_reachable(joints) returns False ''' return self._goto_joints(joints, duration, stop_on_contact_forces, joint_impedances, k_gains, d_gains, ignore_errors, skill_desc, skill_type) def goto_joints_with_joint_control(self, joints, duration=5, stop_on_contact_forces=None, joint_impedances=None, ignore_errors=True, skill_desc=''): '''Commands Arm to the given joint configuration Args: joints (list): A list of 7 numbers that correspond to joint angles in radians duration (float): How much time this robot motion should take joint_impedances (list): A list of 7 numbers that represent the desired joint impedances for the internal robot joint controller Raises: ValueError: If is_joints_reachable(joints) returns False ''' return self._goto_joints(joints, duration, stop_on_contact_forces, joint_impedances, None, None, ignore_errors, skill_desc, skill_type=SkillType.JointPositionSkill) def goto_joints_with_impedance_control(self, joints, duration=5, stop_on_contact_forces=None, k_gains=None, d_gains=None, ignore_errors=True, skill_desc=''): '''Commands Arm to the given joint configuration Args: joints (list): A list of 7 numbers that correspond to joint angles in radians duration (float): How much time this robot motion should take joint_impedances (list): A list of 7 numbers that represent the desired joint impedances for the internal robot joint controller Raises: ValueError: If is_joints_reachable(joints) returns False ''' return self._goto_joints(joints, duration, stop_on_contact_forces, None, k_gains, d_gains, ignore_errors, skill_desc, skill_type=SkillType.ImpedanceControlSkill) def _goto_joints(self, joints, duration=5, stop_on_contact_forces=None, joint_impedances=None, k_gains=None, d_gains=None, ignore_errors=True, skill_desc='', skill_type=SkillType.JointPositionSkill): '''Commands Arm to the given joint configuration Args: joints (list): A list of 7 numbers that correspond to joint angles in radians duration (float): How much time this robot motion should take joint_impedances (list): A list of 7 numbers that represent the desired joint impedances for the internal robot joint controller Raises: ValueError: If is_joints_reachable(joints) returns False ''' if not self.is_joints_reachable(joints): raise ValueError('Joints not reachable!') skill = GoToJointsSkill(skill_desc, skill_type) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if joint_impedances is not None: skill.add_joint_impedances(joint_impedances) elif k_gains is not None and d_gains is not None: skill.add_joint_gains(k_gains, d_gains) else: if (skill_type == SkillType.ImpedanceControlSkill): skill.add_joint_gains(FC.DEFAULT_K_GAINS, FC.DEFAULT_D_GAINS) else: skill.add_feedback_controller_params([]) if stop_on_contact_forces is not None: skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME, stop_on_contact_forces, stop_on_contact_forces) else: skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) skill.add_goal_joints(duration, joints) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def apply_joint_torques(self, torques, duration, ignore_errors=True): '''Commands Arm to apply given joint torques for duration seconds Args: torques (list): A list of 7 numbers that correspond to torques in Nm. duration (float): A float in the unit of seconds ''' pass def execute_joint_dmp(self, dmp_info, duration, ignore_errors=True, skill_desc='', skill_type=SkillType.JointPositionSkill): '''Commands Arm to execute a given dmp for duration seconds Args: dmp_info (dict): Contains all the parameters of a DMP (phi_j, tau, alpha, beta, num_basis, num_sensors, mu, h, and weights) duration (float): A float in the unit of seconds ''' skill = JointDMPSkill(skill_desc, skill_type) skill.add_initial_sensor_values(dmp_info['phi_j']) # sensor values # Doesn't matter because we overwrite it with the initial joint positions anyway y0 = [-0.282, -0.189, 0.0668, -2.186, 0.0524, 1.916, -1.06273] # Run time, tau, alpha, beta, num_basis, num_sensor_value, mu, h, weight trajectory_params = [ duration, dmp_info['tau'], dmp_info['alpha'], dmp_info['beta'], float(dmp_info['num_basis']), float(dmp_info['num_sensors'])] \ + dmp_info['mu'] \ + dmp_info['h'] \ + y0 \ + np.array(dmp_info['weights']).reshape(-1).tolist() skill.add_trajectory_params(trajectory_params) skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def execute_pose_dmp(self, dmp_info, duration, ignore_errors=True, skill_desc='', skill_type=SkillType.CartesianPoseSkill): '''Commands Arm to execute a given dmp for duration seconds Args: dmp_info (dict): Contains all the parameters of a DMP (phi_j, tau, alpha, beta, num_basis, num_sensors, mu, h, and weights) duration (float): A float in the unit of seconds ''' skill = PoseDMPSkill(skill_desc, skill_type) skill.add_initial_sensor_values(dmp_info['phi_j']) # sensor values # Doesn't matter because we overwrite it with the initial position anyways y0 = [0.0, 0.0, 0.0] # Run time, tau, alpha, beta, num_basis, num_sensor_value, mu, h, weight trajectory_params = [ duration, dmp_info['tau'], dmp_info['alpha'], dmp_info['beta'], float(dmp_info['num_basis']), float(dmp_info['num_sensors'])] \ + dmp_info['mu'] \ + dmp_info['h'] \ + y0 \ + np.array(dmp_info['weights']).reshape(-1).tolist() skill.add_trajectory_params(trajectory_params) skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def execute_goal_pose_dmp(self, dmp_info, duration, ignore_errors=True, phi_j=None, skill_desc='', skill_type=SkillType.CartesianPoseSkill): '''Commands Arm to execute a given dmp for duration seconds Args: dmp_info (dict): Contains all the parameters of a DMP (phi_j, tau, alpha, beta, num_basis, num_sensors, mu, h, and weights) duration (float): A float in the unit of seconds ''' skill = GoalPoseDMPSkill(skill_desc, skill_type) skill.add_initial_sensor_values(dmp_info['phi_j']) # sensor values # Doesn't matter because we overwrite it with the initial position anyways y0 = [0.0, 0.0, 0.0] if phi_j is None: phi_j = np.array([[-0.025, 1.], [0, 0.], [-0.05, 1.0]]) # Run time, tau, alpha, beta, num_basis, num_sensor_value, mu, h, weight trajectory_params = [ duration, dmp_info['tau'], dmp_info['alpha'], dmp_info['beta'], float(dmp_info['num_basis']), float(dmp_info['num_sensors'])] \ + dmp_info['mu'] \ + dmp_info['h'] \ + y0 \ + np.array(dmp_info['weights']).reshape(-1).tolist() \ + np.array(phi_j).reshape(-1).tolist() skill.add_trajectory_params(trajectory_params) skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def apply_effector_forces_torques(self, run_duration, acc_duration, max_translation, max_rotation, forces=None, torques=None, ignore_errors=True, skill_desc=''): '''Applies the given end-effector forces and torques in N and Nm Args: run_duration (float): A float in the unit of seconds acc_duration (float): A float in the unit of seconds. How long to acc/de-acc to achieve desired force. forces (list): Optional (defaults to None). A list of 3 numbers that correspond to end-effector forces in 3 directions torques (list): Optional (defaults to None). A list of 3 numbers that correspond to end-effector torques in 3 axes Raises: ValueError if acc_duration > 0.5*run_duration, or if forces are too large ''' if acc_duration > 0.5 * run_duration: raise ValueError( 'acc_duration must be smaller than half of run_duration!') forces = [0, 0, 0] if forces is None else np.array(forces).tolist() torques = [0, 0, 0] if torques is None else np.array(torques).tolist() if np.linalg.norm(forces) * run_duration > FC.MAX_LIN_MOMENTUM: raise ValueError('Linear momentum magnitude exceeds safety ' 'threshold of {}'.format(FC.MAX_LIN_MOMENTUM)) if np.linalg.norm(torques) * run_duration > FC.MAX_ANG_MOMENTUM: raise ValueError('Angular momentum magnitude exceeds safety ' 'threshold of {}'.format(FC.MAX_ANG_MOMENTUM)) skill = ForceTorqueSkill(skill_desc=skill_desc) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_termination_params([0.1]) skill.add_trajectory_params( [run_duration, acc_duration, max_translation, max_rotation] + forces + torques) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def apply_effector_forces_along_axis(self, run_duration, acc_duration, max_translation, forces, ignore_errors=True, skill_desc=''): '''Applies the given end-effector forces and torques in N and Nm Args: run_duration (float): A float in the unit of seconds acc_duration (float): A float in the unit of seconds. How long to acc/de-acc to achieve desired force. max_translation (float): Max translation before the robot deaccelerates. forces (list): Optional (defaults to None). A list of 3 numbers that correspond to end-effector forces in 3 directions Raises: ValueError if acc_duration > 0.5*run_duration, or if forces are too large ''' if acc_duration > 0.5 * run_duration: raise ValueError( 'acc_duration must be smaller than half of run_duration!') if np.linalg.norm( forces) * run_duration > FC.MAX_LIN_MOMENTUM_CONSTRAINED: raise ValueError('Linear momentum magnitude exceeds safety ' \ 'threshold of {}'.format(FC.MAX_LIN_MOMENTUM_CONSTRAINED)) forces = np.array(forces) force_axis = forces / np.linalg.norm(forces) skill = ForceAlongAxisSkill(skill_desc=skill_desc) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_termination_params([0.1]) skill.add_feedback_controller_params( FC.DEFAULT_FORCE_AXIS_CONTROLLER_PARAMS + force_axis.tolist()) init_params = [run_duration, acc_duration, max_translation, 0] skill.add_trajectory_params(init_params + forces.tolist() + [0, 0, 0]) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def goto_gripper(self, width, speed=0.04, force=None, ignore_errors=True): '''Commands gripper to goto a certain width, applying up to the given (default is max) force if needed Args: width (float): A float in the unit of meters speed (float): Gripper operation speed in meters per sec force (float): Max gripper force to apply in N. Default to None, which gives acceptable force Raises: ValueError: If width is less than 0 or greater than TODO(jacky) the maximum gripper opening ''' skill = GripperSkill() skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if force is not None: skill.add_trajectory_params([width, speed, force]) else: skill.add_trajectory_params([width, speed]) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) # this is so the gripper state can be updated, which happens with a # small lag sleep(FC.GRIPPER_CMD_SLEEP_TIME) def stay_in_position(self, duration=3, translational_stiffness=600, rotational_stiffness=50, k_gains=None, d_gains=None, cartesian_impedances=None, joint_impedances=None, ignore_errors=True, skill_desc='', skill_type=SkillType.ImpedanceControlSkill, feedback_controller_type=FeedbackControllerType. CartesianImpedanceFeedbackController): '''Commands the Arm to stay in its current position with provided translation and rotation stiffnesses Args: duration (float) : How much time the robot should stay in place in seconds. translational_stiffness (float): Translational stiffness factor used in the torque controller. Default is 600. A value of 0 will allow free translational movement. rotational_stiffness (float): Rotational stiffness factor used in the torque controller. Default is 50. A value of 0 will allow free rotational movement. ''' skill = StayInInitialPoseSkill(skill_desc, skill_type, feedback_controller_type) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if skill_type == SkillType.ImpedanceControlSkill: if feedback_controller_type == FeedbackControllerType.CartesianImpedanceFeedbackController: if cartesian_impedances is not None: skill.add_cartesian_impedances(cartesian_impedances) else: skill.add_feedback_controller_params( [translational_stiffness] + [rotational_stiffness]) elif feedback_controller_type == FeedbackControllerType.JointImpedanceFeedbackController: if k_gains is not None and d_gains is not None: skill.add_joint_gains(k_gains, d_gains) else: skill.add_feedback_controller_params([]) else: skill.add_feedback_controller_params( [translational_stiffness] + [rotational_stiffness]) elif skill_type == SkillType.CartesianPoseSkill: if cartesian_impedances is not None: skill.add_cartesian_impedances(cartesian_impedances) else: skill.add_feedback_controller_params([]) elif skill_type == SkillType.JointPositionSkill: if joint_impedances is not None: skill.add_joint_impedances(joint_impedances) else: skill.add_feedback_controller_params([]) else: skill.add_feedback_controller_params([translational_stiffness] + [rotational_stiffness]) skill.add_run_time(duration) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def run_guide_mode_with_selective_joint_compliance( self, duration=3, joint_impedances=None, k_gains=FC.DEFAULT_K_GAINS, d_gains=FC.DEFAULT_D_GAINS, ignore_errors=True, skill_desc='', skill_type=SkillType.ImpedanceControlSkill): '''Run guide mode with selective joint compliance given k and d gains for each joint Args: duration (float) : How much time the robot should be in selective joint guide mode in seconds. k_gains (list): list of 7 k gains, one for each joint Default is 600., 600., 600., 600., 250., 150., 50.. d_gains (list): list of 7 d gains, one for each joint Default is 50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0. ''' skill = StayInInitialPoseSkill( skill_desc, skill_type, FeedbackControllerType.JointImpedanceFeedbackController) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if skill_type == SkillType.ImpedanceControlSkill: if k_gains is not None and d_gains is not None: skill.add_joint_gains(k_gains, d_gains) elif skill_type == SkillType.JointPositionSkill: if joint_impedances is not None: skill.add_joint_impedances(joint_impedances) else: skill.add_feedback_controller_params([]) skill.add_run_time(duration) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def run_guide_mode_with_selective_pose_compliance( self, duration=3, translational_stiffnesses=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES, rotational_stiffnesses=FC.DEFAULT_ROTATIONAL_STIFFNESSES, cartesian_impedances=None, ignore_errors=True, skill_desc='', skill_type=SkillType.ImpedanceControlSkill): '''Run guide mode with selective pose compliance given translational and rotational stiffnesses Args: duration (float) : How much time the robot should be in selective pose guide mode in seconds. translational_stiffnesses (list): list of 3 translational stiffnesses, one for each axis (x,y,z) Default is 600.0, 600.0, 600.0 rotational_stiffnesses (list): list of 3 rotational stiffnesses, one for axis (roll, pitch, yaw) Default is 50.0, 50.0, 50.0 ''' skill = StayInInitialPoseSkill( skill_desc, skill_type, FeedbackControllerType.CartesianImpedanceFeedbackController) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if skill_type == SkillType.ImpedanceControlSkill: if cartesian_impedances is not None: skill.add_cartesian_impedances(cartesian_impedances) else: skill.add_feedback_controller_params( [translational_stiffnesses] + [rotational_stiffnesses]) elif skill_type == SkillType.CartesianPoseSkill: if cartesian_impedances is not None: skill.add_cartesian_impedances(cartesian_impedances) else: skill.add_feedback_controller_params([]) skill.add_feedback_controller_params(translational_stiffnesses + rotational_stiffnesses) skill.add_run_time(duration) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def run_dynamic_joint_position_interpolation( self, joints, duration=5, stop_on_contact_forces=None, joint_impedances=None, k_gains=None, d_gains=None, ignore_errors=True, skill_desc='', skill_type=SkillType.JointPositionDynamicInterpolationSkill): '''Commands Arm to the given joint configuration Args: joints (list): A list of 7 numbers that correspond to joint angles in radians duration (float): How much time this robot motion should take joint_impedances (list): A list of 7 numbers that represent the desired joint impedances for the internal robot joint controller Raises: ValueError: If is_joints_reachable(joints) returns False ''' if not self.is_joints_reachable(joints): raise ValueError('Joints not reachable!') skill = GoToJointsSkill(skill_desc, skill_type) skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) if joint_impedances is not None: skill.add_joint_impedances(joint_impedances) elif k_gains is not None and d_gains is not None: skill.add_joint_gains(k_gains, d_gains) else: if (skill_type == SkillType.ImpedanceControlSkill): skill.add_joint_gains(FC.DEFAULT_K_GAINS, FC.DEFAULT_D_GAINS) else: skill.add_feedback_controller_params([]) if stop_on_contact_forces is not None: skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME, stop_on_contact_forces, stop_on_contact_forces) else: skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME]) skill.add_goal_joints(duration, joints) goal = skill.create_goal() self._send_goal(goal, cb=lambda x: skill.feedback_callback(x), ignore_errors=ignore_errors) def open_gripper(self): '''Opens gripper to maximum width ''' self.goto_gripper(FC.GRIPPER_WIDTH_MAX) def close_gripper(self, grasp=True): '''Closes the gripper as much as possible ''' self.goto_gripper(FC.GRIPPER_WIDTH_MIN, force=FC.GRIPPER_MAX_FORCE if grasp else None) def run_guide_mode(self, duration=100): self.apply_effector_forces_torques(duration, 0, 0, 0) ''' Reads ''' def get_robot_state(self): ''' Returns: dict of full robot state data ''' return self._state_client.get_data() def get_pose(self): ''' Returns: pose (RigidTransform) of the current end-effector ''' tool_base_pose = self._state_client.get_pose() tool_pose = tool_base_pose * self._tool_delta_pose return tool_pose def get_joints(self): ''' Returns: ndarray of shape (7,) of joint angles in radians ''' return self._state_client.get_joints() def get_joint_torques(self): ''' Returns: ndarray of shape (7,) of joint torques in Nm ''' return self._state_client.get_joint_torques() def get_joint_velocities(self): ''' Returns: ndarray of shape (7,) of joint velocities in rads/s ''' return self._state_client.get_joint_velocities() def get_gripper_width(self): ''' Returns: float of gripper width in meters ''' return self._state_client.get_gripper_width() def get_gripper_is_grasped(self): ''' Returns: True if gripper is grasping something. False otherwise ''' return self._state_client.get_gripper_is_grasped() def get_speed(self, speed): ''' Returns: float of current target speed parameter ''' pass def get_tool_base_pose(self): ''' Returns: RigidTransform of current tool base pose ''' return self._tool_delta_pose.copy() ''' Sets ''' def set_tool_delta_pose(self, tool_delta_pose): '''Sets the tool pose relative to the end-effector pose Args: tool_delta_pose (RigidTransform) ''' if tool_delta_pose.from_frame != 'franka_tool' \ or tool_delta_pose.to_frame != 'franka_tool_base': raise ValueError('tool_delta_pose has invalid frame names! ' \ 'Make sure it has from_frame=franka_tool, and ' \ 'to_frame=franka_tool_base') self._tool_delta_pose = tool_delta_pose.copy() def set_speed(self, speed): '''Sets current target speed parameter Args: speed (float) ''' pass ''' Misc ''' def reset_joints(self, skill_desc='', ignore_errors=True): '''Commands Arm to goto hardcoded home joint configuration ''' self.goto_joints(FC.HOME_JOINTS, duration=5, skill_desc=skill_desc, ignore_errors=ignore_errors) def reset_pose(self, skill_desc='', ignore_errors=True): '''Commands Arm to goto hardcoded home pose ''' self.goto_pose(FC.HOME_POSE, duration=5, skill_desc=skill_desc, ignore_errors=ignore_errors) def is_joints_reachable(self, joints): ''' Returns: True if all joints within joint limits ''' for i, val in enumerate(joints): if val <= FC.JOINT_LIMITS_MIN[i] or val >= FC.JOINT_LIMITS_MAX[i]: return False return True
def main(): # ==================================== # parse arguments # ==================================== parser = argparse.ArgumentParser( description='Generate segmentation images and updated json files.') parser.add_argument('-d', '--data-dir', default=os.getcwd(), help='directory containing images and json files') parser.add_argument( '-o', '--object-settings', help= 'object_settings file where the fixed_model_transform corresponds to the poses' 'in the frame annotation jsons.' 'default: <data_dir>/_object_settings.json') parser.add_argument( '-t', '--target-object-settings', help= 'if given, transform all poses into the fixed_model_transform from this file.' ) args = parser.parse_args() if not args.object_settings: args.object_settings = os.path.join(args.data_dir, '_object_settings.json') if not args.target_object_settings: args.target_object_settings = args.object_settings # ===================== # parse object_settings # ===================== with open(args.object_settings, 'r') as f: object_settings_json = json.load(f) with open(args.target_object_settings, 'r') as f: target_object_settings_json = json.load(f) if not len(object_settings_json['exported_objects']) == len( target_object_settings_json['exported_objects']): print "FATAL: object_settings and target_object_settings do not match!" sys.exit(-1) models = {} for model_json, target_model_json in zip( object_settings_json['exported_objects'], target_object_settings_json['exported_objects']): class_name = model_json['class'] if not class_name == target_model_json['class']: print "FATAL: object_settings and target_object_settings do not match!" sys.exit(-1) segmentation_class_id = model_json['segmentation_class_id'] cuboid_dimensions = np.array(model_json['cuboid_dimensions']) # calculate model_transform fixed_model_transform_mat = np.transpose( np.array(model_json['fixed_model_transform'])) fixed_model_transform = RigidTransform( rotation=fixed_model_transform_mat[:3, :3], translation=fixed_model_transform_mat[:3, 3], from_frame='ycb_model', to_frame='source_model') target_fixed_model_transform_mat = np.transpose( np.array(target_model_json['fixed_model_transform'])) target_fixed_model_transform = RigidTransform( rotation=target_fixed_model_transform_mat[:3, :3], translation=target_fixed_model_transform_mat[:3, 3], from_frame='ycb_model', to_frame='target_model_original') model_transform = fixed_model_transform.dot( target_fixed_model_transform.inverse()) models[class_name] = ModelConfig(class_name, segmentation_class_id, model_transform, cuboid_dimensions) # ================================================== # parse camera_settings and set up camera intrinsics # ================================================== with open(os.path.join(args.data_dir, '_camera_settings.json'), 'r') as f: camera_settings_json = json.load(f)['camera_settings'][0] camera_intrinsics = CameraIntrinsics( frame='camera', fx=camera_settings_json['intrinsic_settings']['fx'], fy=camera_settings_json['intrinsic_settings']['fy'], cx=camera_settings_json['intrinsic_settings']['cx'], cy=camera_settings_json['intrinsic_settings']['cy'], skew=camera_settings_json['intrinsic_settings']['s'], height=camera_settings_json['captured_image_size']['height'], width=camera_settings_json['captured_image_size']['width']) # ==================================== # process each frame # ==================================== pattern = re.compile(r'\d{3,}.json') json_files = sorted([ os.path.join(args.data_dir, f) for f in os.listdir(args.data_dir) if pattern.match(f) ]) for json_file in json_files: filename_prefix = json_file[:-len('json')] print '\n---------------------- {}*'.format(filename_prefix) with open(json_file, 'r') as f: frame_json = json.load(f) updated_frame_json = process_frame(frame_json, camera_intrinsics, models) with open(filename_prefix + 'flipped.json', 'w') as f: json.dump(updated_frame_json, f, indent=2, sort_keys=True)
T_obj_world.inverse(), mat_props=table_mat_props) } for name, scene_obj in scene_objs.iteritems(): virtual_camera.add_to_scene(name, scene_obj) # camera pose cam_dist = 0.3 T_camera_world = RigidTransform(rotation=np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]]), translation=[0, 0, cam_dist], from_frame=camera_intr.frame, to_frame='world') T_obj_camera = T_camera_world.inverse() * T_obj_world # show mesh if False: vis3d.figure() vis3d.mesh(mesh, T_obj_camera) vis3d.pose(RigidTransform(), alpha=0.1) vis3d.pose(T_obj_camera, alpha=0.1) vis3d.show() # render depth image render_start = time.time() IPython.embed() renders = virtual_camera.wrapped_images(mesh, [T_obj_camera], RenderMode.RGBD_SCENE, mat_props=mat_props,