Retrieving state

In order to collect our datasets, we also need to record, and therefore stream, our robot state back to the system. If you already have your robot setup running, you can skip to the dataset creation.

Streaming state

There are multiple ways to stream the robot state to the Incar Skill system, but the simplest way is to just send state back at the same frequency as we are handling our commands. Building on the example from the teleoperation tutorial, we can do the following:

from incar_networking.robot_interface import IncarRobotInterface

class MyRobot:
    def __init__(self): ...
    def cartesian_velocity_command(self, velocity: list[float]): ...
    def gripper_command(self, gripper: list[float]): ...
    def publish_state(self, interface: IncarRobotInterface):
        interface.set_robot_state(
            "arm", 
            ee_pose=self.ee_pose, # list of floats
            joint_pos=self.joint_pos # list of floats
        )
        interface.set_robot_state(
            "gripper",
            joint_pos=self.gripper_pos # list of floats
        )
        interface.publish_state()
        interface.publish_sensor(
            "force_torque", 
            self.force_torque_data # list of floats
        )

if __name__ == "__main__":
    dt = 0.01
    robot = MyRobot()
    interface = IncarRobotInterface(
        dt,
        command_hooks = {
            "right.commands.arm.ee.velocity": robot.cartesian_velocity_command,
            "right.commands.gripper.openclose": robot.gripper_command
        },
        loop_callbacks = [
            robot.publish_state
        ],
        sensor_names= [
            "force_torque"
        ]
    )
    interface.start()

Code Explained

The loop_callback in line 33 takes the IncarRobotInterface as an input, so we can set and publish state on it. We will send out the robot state per robot module. In this case, you could think of the arm and the gripper being separate robot modules, but for a dual arm system you could for instance have a left_arm module and a right_arm module, or for mobile robots you could have a base module. The module name will basically act with a key for how we can record the features.

It is possible to send various types of robot state, namely: end effector pose, end effector velocity, joint positions, joint velocities, and joint efforts. It is also possible to send multiple types of robot state at the same time. For the arm, we can send end effector pose and joint position state, and for the gripper, we can send the joint position. Note that the system expects all state to be in a list of floats.

After we have set the robot state, we can call the interface.publish_state() method to send it over to the Incar Skill System. Now, when we start our robot, and connect to it in the GUI, you can see that we are receiving the state as separate features in the system. The features are keyed by [module_name].[state_type], for example arm.joints.velocity. This is how we can key this feature for dataset recording.

Additional robot sensors

Additional sensors have to be declared in the sensor_names of the IncarRobotInterface. The sensor name is the key with which the feature can be recorded as well. Sensor data can then be published using interface.publish_sensor("[sensor name]", data) where data is a list of floats. The example above streams a feature keyed force_torque to the Incar System.

Camera streaming

Note

Cameras need to be connected to the device that the Incar Skill System is running on. Streaming cameras from a remote device is still in active development; a streamlined solution is planned on the roadmap.

In order to register a camera in the Incar Skill System, edit the workspace_config.json file of the workspace (you can find this in the root workspace path). You can add your cameras in the config based on type; the most straight-forward way to add a new camera is as opencv type, where you can specify the openCV index as port parameter.

Other specific camera implementation might have more settings for the camera. Currently, realsense and oak1 cameras are supported, however, it is possible to also make your own custom specific camera implementation if you need more control over the camera settings.

workspace_config.json

{
    "cameras": {
        "camera_one": {
            "type": "realsense",
            "serial_number": "218622277822",
            "exposure": 13500,
            "white_balance": 3200,
            "width": 640,
            "height": 480,
            "frame_type": "RGBD"
        },
        "camera_two": {
            "type": "oak1",
            "device_id": "1844301051E042F500"
        },
        "webcam": {
            "type": "opencv",
            "port": 0
        }
    },

    ...
}

When you reload the workspace, the camera streams will appear in the live data panel. You can click to inspect the camera stream.

Next Step: Creating and recording a dataset.