Move to a pose

Opening the gripper was one number. Moving the arm to "home" is six numbers with a name. That name is a skill's vocabulary, so it lives in the skill's config, not on the arm. The arm still just receives joint targets.

Install the skill

solarian install move-skill

This installs solarian-move-skill and copies conf/skills/move.yaml the first time. Open that file. It already has a home pose and a ground pose, both in the normalized −100 to 100 joint range, plus a gripper width in millimetres. Those numbers are a reasonable tuck for this arm. You will add one pose of your own in a moment.

MoveSkill reads poses[name], sends one MOVE_J, and finishes when a timeout elapses. The module drives the path. The skill does not interpolate.

Your arm module from Tutorial 2 already handles MOVE_J by writing the joint values. So the plugin skill works on the class you wrote, before any kinematics.

A key for home

so106/controller.py:

from solarian.core import Controller
from solarian.core.event import Event, EventType, event_handler
from solarian_move_skill import MoveSkill

from so106.skills import OpenGripperSkill


class So106Controller(Controller):
    skills = (OpenGripperSkill, MoveSkill)

    @event_handler(EventType.KEY_DOWN)
    def on_key_down(self, event: Event):
        key = event.data["key"]
        if key == "o":
            self.start_skill(OpenGripperSkill(params={"gripper": "right_gripper"}))
        elif key == "0":
            self.start_skill(MoveSkill(params={
                "pose": "home",
                "arm": "right_arm",
                "gripper": "right_gripper",
            }))

Restart the controller and the robot. Press 0. The arm should fold toward the tucked pose in move.yaml, and the gripper should open to the width in that pose. Press o and only the gripper moves; the arm stays. Press 0 again and the tuck runs again. Starting is not a toggle.

Name your own pose

In conf/skills/move.yaml, under poses:, add a pose you can recognize. These values are mid-travel on the shoulder and a bent elbow, so the hand sits out in front instead of tucked:

    reach:
        right_arm:
            shoulder_pan: 0.0
            shoulder_lift: 20.0
            elbow_flex: 40.0
            wrist_flex: -20.0
            wrist_yaw: 0.0
            wrist_roll: 0.0
        right_gripper:
            gripper: 40.0

Add a key:

elif key == "1":
    self.start_skill(MoveSkill(params={
        "pose": "reach",
        "arm": "right_arm",
        "gripper": "right_gripper",
    }))

Restart the controller. 0 tucks, 1 reaches. If 1 raises KeyError naming reach, the YAML key and the string in params disagree, or the controller was started from a directory that is not the project root.

Inverse kinematics

Joint poses were enough for home and reach. A policy does not speak in joint angles. It speaks in where the fingertips should be: x, y, z and three rotations. Inverse kinematics is the arm module turning that into joints.

Replace so106/modules/right_arm.py. The hand-written MOVE_J handler has to go. ArmModule already implements it, and a second handler on the subclass would hide that one. ArmModule also needs the joint limits from the kinematics model, which is why this arrives now and not in Tutorial 2.

from solarian.core.arm_module import ArmModule
from solarian.core.ik import CartesianIKActuator, MinkIK
from solarian_feetech.actuator import FeetechServo

JOINTS = [
    "shoulder_pan",
    "shoulder_lift",
    "elbow_flex",
    "wrist_flex",
    "wrist_yaw",
    "wrist_roll",
]


class RightArmModule(ArmModule):
    shoulder_pan = FeetechServo(model="sts3250")
    shoulder_lift = FeetechServo(model="sts3250")
    elbow_flex = FeetechServo(model="sts3250")
    wrist_flex = FeetechServo(model="sts3250")
    wrist_yaw = FeetechServo(model="sts3215")
    wrist_roll = FeetechServo(model="sts3215")

    x = CartesianIKActuator()
    y = CartesianIKActuator()
    z = CartesianIKActuator()
    rx = CartesianIKActuator()
    ry = CartesianIKActuator()
    rz = CartesianIKActuator()

    # The arm MJCF, not the scene. Joint names here are unprefixed.
    # sim_joint in the YAML stays prefixed, because that is the composed world.
    ik_backend = MinkIK(
        mjcf="assets/so106/so106.xml",
        joints=dict(zip(JOINTS, JOINTS)),
        frame="flange",
        position_cost=1.0,
        orientation_cost=1.0,
    )

The six CartesianIKActuators are the pose. frame="flange" is the wrist site in the arm model. The fingertips are not the flange. They sit a fixed offset beyond it, which the gripper module already measured. Pass that offset when you construct the arm, in so106/robot.py:

right_arm = RightArmModule(ik_tcp=UmiSlimFeetechModule.tcp_from_flange)

In conf/modules/right_arm.yaml, the module has to interpolate poses, not just hold a position. Add, next to frequency:

interpolator_type: pose
max_speed: [0.25, 0.6]    # metres/second, radians/second
max_accel: [0.5, 3.0]

And the action schema has to grow the six axes, or a Cartesian command has nowhere to land. In RightArmAction:

x = ActuatorField(So106Robot.right_arm.x.pos)
y = ActuatorField(So106Robot.right_arm.y.pos)
z = ActuatorField(So106Robot.right_arm.z.pos)
rx = ActuatorField(So106Robot.right_arm.rx.pos)
ry = ActuatorField(So106Robot.right_arm.ry.pos)
rz = ActuatorField(So106Robot.right_arm.rz.pos)

Do the same six lines on RightArmObs, with SensorField. The observation is how anything, including you in a log, reads the pose the kinematics produced.

Restart the robot and the controller. 0 and 1 should still move the arm. They are joint poses, and ArmModule handles MOVE_J itself now. If they don't, the old _handle_move_j is still on the class.

The contract you just implemented is the one in Adding a new arm: joint servos, six Cartesian actuators, a MinkIK backend, and interpolator_type: pose. The reference has the rest of the fields. You have enough for a policy to ask for a hand pose.

Next: what UMI is.

Was this page helpful?