Koch 1.1
The lerobot docs keep changing, which is super annoying. Hopefully am calibrating this for the last time.
Issues:
- right follower: the elbow flex is fcked up, and the shoulder pan is fucked up
TODO: I need to pin the port to a particular arm, so that every time it unplugs, its the same.
Motors
The calibration was a little confusing, and how the motors are actually encoded.
The motors are in extended mode, which means that there’s no wrap around, if you turn the motor mutliple times, it will remember (-inf to inf range)
for motor in self.bus.motors:
self.bus.write("Operating_Mode", motor, OperatingMode.EXTENDED_POSITION.value)HOWEVER, Each motor has a reading from 0 to 4095 why you first boot it. Then, we will use the calibration file to apply this transformation:
Present_Position = Actual_Position + Homing_Offset
The Homing_offset gets stored to non-volatile memory so it remembers it. Hwoever, the actual_position is always measured beteween 0 and 4095. Now imagine your actual value is 50, and the next time its at -50 (+ 4095 = 4045). So the present position can have HUGE jumps if your actual position is close to 0/4095 boundary. This happened to me with the wrist roll :( The trick is to add a function to check this at init time, and you expect the calibrated value to be between 0 and 4095 (because that is what you set during the calibration).
This is an entry in the calibration file:
"wrist_roll": {
"id": 5,
"drive_mode": 0,
"homing_offset": -1975,
"range_min": 0,
"range_max": 4095
},
- The homing offset will be set such that the mid point is actually the midpoint, so you don’t suffer from underflow/overflow afterwards
The range_min and range_max are used to set the midpoint.
Ports
Finding the ports
lerobot-find-port
These are the ports when I plug the usb adapter into the top place.
Follower left
/dev/tty.usbmodem14201
Follower right
/dev/tty.usbmodem1301
Leader left
/dev/tty.usbmodem1201
Leader right
/dev/tty.usbmodem1101
Add these to your ~/.bashrc
export FOLLOWER_LEFT_PORT=/dev/tty.usbmodem14201
export FOLLOWER_RIGHT_PORT=/dev/tty.usbmodem1301
export LEADER_LEFT_PORT=/dev/tty.usbmodem1201
export LEADER_RIGHT_PORT=/dev/tty.usbmodem1101
export TOP_CAMERA_INDEX_OR_PATH=0
export FRONT_CAMERA_INDEX_OR_PATH=1
On steven-beast, setup udev rules like this https://chatgpt.com/share/68d1dd62-132c-8002-a314-b97bacf3e86c
/dev/ttyACM1
/dev/ttyACM0
/dev/ttyACM3
/dev/ttyACM2
Also for cameras https://chatgpt.com/share/68d2050a-30c0-8002-a55d-ee417181d3e6
ls -l /dev/serial/by-path/
total 0
lrwxrwxrwx 1 root root 13 Sep 22 19:25 pci-0000:0c:00.3-usb-0:1.1:1.0 -> ../../ttyACM2
lrwxrwxrwx 1 root root 13 Sep 22 19:25 pci-0000:0c:00.3-usb-0:1.2:1.0 -> ../../ttyACM3
lrwxrwxrwx 1 root root 13 Sep 22 19:24 pci-0000:0c:00.3-usb-0:1.3:1.0 -> ../../ttyACM0
lrwxrwxrwx 1 root root 13 Sep 22 19:24 pci-0000:0c:00.3-usb-0:1.4.2:1.0 -> ../../ttyACM1
lrwxrwxrwx 1 root root 13 Sep 22 19:25 pci-0000:0c:00.3-usbv2-0:1.1:1.0 -> ../../ttyACM2
lrwxrwxrwx 1 root root 13 Sep 22 19:25 pci-0000:0c:00.3-usbv2-0:1.2:1.0 -> ../../ttyACM3
lrwxrwxrwx 1 root root 13 Sep 22 19:24 pci-0000:0c:00.3-usbv2-0:1.3:1.0 -> ../../ttyACM0
lrwxrwxrwx 1 root root 13 Sep 22 19:24 pci-0000:0c:00.3-usbv2-0:1.4.2:1.0 -> ../../ttyACM1
export FOLLOWER_LEFT_PORT=/dev/follower-left
export FOLLOWER_RIGHT_PORT=/dev/follower-right
export LEADER_LEFT_PORT=/dev/leader-left
export LEADER_RIGHT_PORT=/dev/leader-right
export TOP_CAMERA_INDEX_OR_PATH=/dev/camera-top
export FRONT_CAMERA_INDEX_OR_PATH=/dev/camera-front
Calibrate
lerobot-calibrate \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower
lerobot-calibrate \
--teleop.type=bi_koch_leader \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--teleop.id=bimanual_leader
Teleop
http://steven-beast:9090/?url=rerun%2Bhttp%3A%2F%2Fsteven-beast%3A9876%2Fproxy
Single arm
lerobot-teleoperate \
--robot.type=koch_follower \
--robot.port=$FOLLOWER_LEFT_PORT \
--robot.id=follower \
--teleop.type=koch_leader \
--teleop.port=$LEADER_LEFT_PORT \
--teleop.id=leader \
--display_data=true
lerobot-teleoperate \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--robot.cameras="{ top: {type: opencv, index_or_path: $TOP_CAMERA_INDEX_OR_PATH, width: 640, height: 360, fps: 30}, left_wrist: {type: opencv, index_or_path: $LEFT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: $RIGHT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30} }" \
--teleop.type=bi_koch_leader \
--teleop.left_arm_port=$LEADER_LEFT_PORT \
--teleop.right_arm_port=$LEADER_RIGHT_PORT \
--teleop.id=bimanual_leader \
--display_data=true
Record a dataset
Keyboard controls:
escto stop data collection- right arrow to terminate episode early
- Left arrow to rerecord episode
lerobot-record \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--teleop.type=bi_koch_leader \
--teleop.left_arm_port=$LEADER_LEFT_PORT \
--teleop.right_arm_port=$LEADER_RIGHT_PORT \
--teleop.id=bimanual_leader \
--robot.cameras="{ top: {type: opencv, index_or_path: $TOP_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: $LEFT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: $RIGHT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30} }" \
--display_data=True \
--dataset.repo_id=${HF_USER}/koch-tshirt-pickplace \
--dataset.num_episodes=20 \
--dataset.reset_time_s=15 \
--dataset.episode_time_s=30 \
--dataset.single_task="Pick up the t-shirt and put it in the bin" \
--dataset.video_encoding_batch_size=20 \
--dataset.num_image_writer_processes=16
- TODO: there’s actually a flag to bulk render the videos, else its actually really time consuming to do this rendering on each video…
--dataset.single_task="Fold the t-shirt and put it in the bin" \
Visualize your dataset
You can go to this url and paste in the url, ex
Gongsta/koch-baby-tshirt-folding
https://huggingface.co/spaces/lerobot/visualize_dataset
Replay an episode!
This is a really helpful feature, can help validate the data integrity. It’s also kind of really cool :))
HF_USER=Gongsta lerobot-replay \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--dataset.repo_id=${HF_USER}/koch-baby-tshirt-folding \
--dataset.episode=20
Training
Run this on your RTX5090.
SmolVLA finetuning
export HF_USER=Gongsta
export OUTPUT_DIR=outputs/train/tshirt_smolvla
lerobot-train \
--policy.path=lerobot/smolvla_base \
--policy.repo_id=${HF_USER}/smolvla-hf-policy-koch-baby-tshirt-folding \
--policy.device=cuda \
--dataset.repo_id=${HF_USER}/koch-baby-tshirt-folding \
--dataset.video_backend=pyav \
--batch_size=128 \
--steps=20000 \
--save_freq=2000 \
--output_dir=$OUTPUT_DIR \
--job_name=tshirt_smolvla_training \
--num_workers=12 \
--wandb.enable=true
pi0
export HF_USER=Gongsta
export OUTPUT_DIR=outputs/train/tshirt_pi0_hf
lerobot-train \
--dataset.repo_id=${HF_USER}/koch-baby-tshirt-folding \
--dataset.video_backend=pyav \
--policy.type=pi0 \
--batch_size=4 \
--steps=20000 \
--save_freq=2000 \
--output_dir=$OUTPUT_DIR \
--job_name=tshirt-pi0-hf-training \
--policy.device=cuda \
--wandb.enable=true \
--num_workers=12 \
--policy.repo_id=${HF_USER}/pi0-hf-policy-koch-baby-tshirt-folding
Also see Openpi for using the openpi codebase.
Policy Inference
HF_USER=Gongsta lerobot-record \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--robot.cameras="{ top: {type: opencv, index_or_path: $TOP_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: $LEFT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: $RIGHT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30} }" \
--dataset.repo_id=${HF_USER}/eval_$(date +%Y-%m-%d_%H-%M-%S) \
--dataset.single_task="Fold the t-shirt and put it in the bin" \
--display_data=true \
--policy.path=${HF_USER}/smolvla-hf-policy-koch-baby-tshirt-folding \
--dataset.push_to_hub=False
export HF_USER=Gongsta
lerobot-record \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--robot.cameras="{ top: {type: opencv, index_or_path: $TOP_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: $LEFT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: $RIGHT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30} }" \
--dataset.repo_id=${HF_USER}/eval_$(date +%Y-%m-%d_%H-%M-%S) \
--dataset.single_task="Fold the t-shirt and put it in the bin" \
--display_data=true \
--policy.path=Gongsta/pi0-hf-policy-koch-baby-tshirt-folding \
--dataset.push_to_hub=False
Remote inference (NOT OWRKING)
python src/lerobot/scripts/server/robot_client.py \
--server_address=localhost:8080 \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--robot.cameras="{ top: {type: opencv, index_or_path: $TOP_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: $LEFT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: $RIGHT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30} }" \
--task="Fold the t-shirt and put it in the bin" \
--policy_type=pi0 \
--pretrained_name_or_path=Gongsta/pi0-hf-policy-koch-baby-tshirt-folding \
--policy_device=cuda \
--actions_per_chunk=50 \
--chunk_size_threshold=0.5 \
--aggregate_fn_name=weighted_average \
--debug_visualize_queue_size=True
Remote inference with my implementation on OpenPi
Server
uv run scripts/serve_policy.py policy:checkpoint --policy.config=pi05_koch --policy.dir=/home/steven/research/openpi/checkpoints/pi05_koch/my_experiment/10000/
Client
python src/lerobot/async_inference/robot_openpi_client.py \
--server_address=localhost:8000 \
--robot.type=bi_koch_follower \
--robot.left_arm_port=$FOLLOWER_LEFT_PORT \
--robot.right_arm_port=$FOLLOWER_RIGHT_PORT \
--robot.id=bimanual_follower \
--robot.cameras="{ top: {type: opencv, index_or_path: $TOP_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, left_wrist: {type: opencv, index_or_path: $LEFT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30}, right_wrist: {type: opencv, index_or_path: $RIGHT_WRIST_CAMERA_INDEX_OR_PATH, width: 640, height: 480, fps: 30} }" \
--task="Fold the t-shirt and put it in the bin" \
--actions_per_chunk=20
Debugging Issues
Power surges
Use a max_relative_target= 1.0 / (try 2.0 or more). Maybe set this as a default?
Cuda 12.9
Running with RTX5090
I had to reinstall pytorch with
pip install --force-reinstallto support CUDA12.9. And then I had issue with the video encoding, which i fixed via--dataset.video_backend=pyav.
- The pyproject.toml needs to be upgraded
pip3 install --force-reinstall torch torchvision --index-url https://download.pytorch.org/whl/cu129
Transformer version too low
https://github.com/huggingface/lerobot/issues/1406
- Transformers version is too low for running pi0 training / inference
AttributeError: 'GemmaForCausalLM' object has no attribute 'embed_tokens'
Fix:
pip install --upgrade transformers
Keyboard controls not working
You need to give permissions to your terminal / Cursor in Accessibility (if I recall correctly).
mean
Got this at inference time. The issue is that they use normalization for the dataset
assert not torch.isinf(mean).any(), _no_stats_error_str("mean")
AssertionError: mean is infinity. You should either initialize with stats as an argument, or use a pretrained model.
The urdf
<?xml version="1.0" encoding="utf-8"?>
<!-- Combined URDF file with motor links as children of original links -->
<robot
name="follower">
<mujoco>
<compiler meshdir="./meshes"/>
</mujoco>
<!-- Original base_link -->
<link name="base_link">
<inertial>
<origin
xyz="0.034538 0.0021165 0.013963"
rpy="0 0 0" />
<mass
value="0.067266" />
<inertia
ixx="4.4766E-05"
ixy="-2.3205E-06"
ixz="4.3559E-07"
iyy="2.1965E-05"
iyz="1.479E-07"
izz="5.9106E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/base_link.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/base_link_motor.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/base_link.STL" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/base_link_motor.STL" />
</geometry>
</collision>
</link>
<!-- Original link_1 -->
<link name="link_1">
<inertial>
<origin
xyz="0.011924 -0.00048792 0.013381"
rpy="0 0 0" />
<mass
value="0.05014" />
<inertia
ixx="7.6045E-06"
ixy="-2.4893E-07"
ixz="-2.8338E-08"
iyy="1.2358E-05"
iyz="-5.112E-09"
izz="1.4492E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_1.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_1_motor.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_1.STL" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_1_motor.STL" />
</geometry>
</collision>
</link>
<joint name="joint_1" type="revolute">
<origin xyz="0.012 0 0.0409" rpy="0 0 0" />
<parent link="base_link" />
<child link="link_1" />
<axis xyz="0 0 -1" />
<limit lower="-3.14" upper="3.14" effort="0.5" velocity="100" />
</joint>
<!-- Original link_2 -->
<link name="link_2">
<inertial>
<origin
xyz="0.0011747 0.02097 0.071547"
rpy="0 0 0" />
<mass
value="0.050177" />
<inertia
ixx="3.7018E-05"
ixy="2.5963E-09"
ixz="-2.8959E-06"
iyy="3.3772E-05"
iyz="-1.6011E-10"
izz="8.2375E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_2.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_2_motor.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_2.STL" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_2_motor.STL" />
</geometry>
</collision>
</link>
<joint name="joint_2" type="revolute">
<origin xyz="0 -0.0209 0.0154" rpy="0 0 0" />
<parent link="link_1" />
<child link="link_2" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="3.14" effort="0.5" velocity="100" />
</joint>
<!-- Original link_3 -->
<link name="link_3">
<inertial>
<origin
xyz="-0.05537 0.014505 0.0028659"
rpy="0 0 0" />
<mass
value="0.06379" />
<inertia
ixx="7.3427E-06"
ixy="2.6058E-09"
ixz="1.895E-07"
iyy="2.2231E-05"
iyz="9.8075E-11"
izz="2.4506E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_3.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_3_motor.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_3.STL" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_3_motor.STL" />
</geometry>
</collision>
</link>
<joint name="joint_3" type="revolute">
<origin xyz="-0.0148 0.0065 0.1083" rpy="0 0 0" />
<parent link="link_2" />
<child link="link_3" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="3.14" effort="0.5" velocity="100" />
</joint>
<!-- Original link_4 -->
<link name="link_4">
<inertial>
<origin
xyz="-0.02652 0.019195 -9.0614E-06"
rpy="0 0 0" />
<mass
value="0.019805" />
<inertia
ixx="2.9577E-06"
ixy="2.8302E-08"
ixz="-1.331E-10"
iyy="1.0783E-06"
iyz="1.3763E-09"
izz="2.8759E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_4.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_4_motor.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_4.STL" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_4_motor.STL" />
</geometry>
</collision>
</link>
<joint name="joint_4" type="revolute">
<origin xyz="-0.10048 5E-05 0.0026999" rpy="0 0 0" />
<parent link="link_3" />
<child link="link_4" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="3.14" effort="0.5" velocity="100" />
</joint>
<!-- Original link_5 -->
<link name="link_5">
<inertial>
<origin
xyz="-0.019091 0.0053379 0.00018011"
rpy="0 0 0" />
<mass
value="0.029277" />
<inertia
ixx="3.5905E-06"
ixy="1.0601E-06"
ixz="4.6179E-08"
iyy="6.8334E-06"
iyz="-2.8448E-09"
izz="8.1125E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_5.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_5_motor.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_5.STL" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_5_motor.STL" />
</geometry>
</collision>
</link>
<joint name="joint_5" type="revolute">
<origin xyz="-0.045 0.013097 0" rpy="0 0 0" />
<parent link="link_4" />
<child link="link_5" />
<axis xyz="1 0 0" />
<limit lower="-3.14" upper="3.14" effort="0.5" velocity="100" />
</joint>
<!-- Original link_6 -->
<link name="link_6">
<inertial>
<origin
xyz="-0.02507 0.0010817 -0.01414"
rpy="0 0 0" />
<mass
value="0.012831" />
<inertia
ixx="1.5159E-06"
ixy="3.156E-07"
ixz="3.409E-08"
iyy="3.449E-06"
iyz="-3.8705E-09"
izz="2.4565E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_6.STL" />
</geometry>
<material name="">
<color rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://follower/meshes/link_6.STL" />
</geometry>
</collision>
</link>
<joint name="joint_6" type="revolute">
<origin xyz="-0.01315 -0.0075 0.0145" rpy="0 0 0" />
<parent link="link_5" />
<child link="link_6" />
<axis xyz="0 0 -1" />
<limit lower="-2.45" upper="0.032" effort="0.5" velocity="100" />
</joint>
</robot>