Skip to content

Commit 43d51b9

Browse files
author
Tim
committed
update
1 parent 7677c46 commit 43d51b9

File tree

10 files changed

+130
-37
lines changed

10 files changed

+130
-37
lines changed

‎README.md‎

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -44,13 +44,14 @@ sh scripts/reset_all_can.sh
4444

4545
## Gripper type
4646

47-
Currently YAM supports three different grippers:
47+
Currently YAM supports Four different grippers:
4848
![YAM supported Grippers](./assets/photos/yam_three_grippers.png)
4949

5050
| Gripper Name | Description |
5151
|---------------------|-------------|
5252
| `crank_4310` | Zero-linkage crank gripper, optimized for minimizing gripper width. |
5353
| `linear_3507` | Linear gripper with smaller DM3507 motor. Lightweight, but requires calibration or starting with the gripper in the closed configuration. |
54+
| `linear_4310` | Linear gripper with the standard DM4310 motor (not shown on photo above). Slightly heavier but can provide a bit more gripping force. |
5455
| `yam_teaching_handle`| Used for the leader arm setup. Includes a trigger to control the gripper and two customizable buttons that can be mapped to different functions. |
5556

5657
The linear gripper requires an additional calibration step because its motor must rotate more than 2π radians to complete the full stroke.
@@ -69,7 +70,7 @@ Default timeout is enabled for YAM motors. Please refer to [YAM configuration](#
6970
from i2rt.robots.motor_chain_robot import get_yam_robot
7071

7172
# Get a robot instance
72-
robot = get_yam_robot(channel="can0")
73+
robot = get_yam_robot(channel="can0", zero_gravity_mode=True)
7374

7475
# Get the current joint positions
7576
joint_pos = robot.get_joint_pos()
@@ -145,7 +146,7 @@ However, we understand that this feature may not always be desirable, especially
145146

146147
To remove the timeout feature, run the following command.
147148
```bash
148-
python i2rt/motor_config_tool/remove_timeout.py --channel can0
149+
python i2rt/motor_config_tool/set_timeout.py --channel can0
149150
```
150151

151152
To set the timeout feature, run the following command.
@@ -160,11 +161,23 @@ python i2rt/motor_config_tool/set_zero.py --channel can0 --motor_id 1
160161
After moving the timeout, you can initialize the YAM arm with the same following command.
161162
```python
162163
from i2rt.robots.motor_chain_robot import get_yam_robot
163-
164164
# Get a robot instance
165165
robot = get_yam_robot(channel="can0")
166166
```
167167

168+
One important way to reduce the risk of the arm going out of control is to avoid entering zero-gravity mode.
169+
170+
By default, the arm initializes in zero-gravity mode. As mentioned earlier, if the arm does not have a timeout but the gravity compensation loop fails, the motor controller will continue applying a constant torque. This can lead to unexpected and potentially unsafe behavior.
171+
172+
To prevent this, you should always set a PD target. With a PD target, the motor controller ensures the arm reaches a stable state rather than drifting under uncontrolled torque.
173+
174+
You can disable the default zero-gravity initialization like this:
175+
```python
176+
robot = get_yam_robot(channel="can0", zero_gravity_mode=False)
177+
```
178+
179+
In this mode, the current joint positions (`qpos`) are used as the PD target, keeping the arm stable in its initial state.
180+
168181
## Flow Base Usage
169182

170183
### Running the demo

‎i2rt/motor_config_tool/utils.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,11 +113,16 @@ def float32_to_bytes(value: float) -> bytearray:
113113

114114

115115
register_addr_map = {
116+
"KT_value": (1, bytes_to_float32),
117+
"OT_value": (2, bytes_to_float32),
116118
"master_id": (7, bytes_to_uint32),
117119
"id": (8, bytes_to_uint32),
118120
"timeout": (9, bytes_to_uint32),
121+
"inertia": (12, bytes_to_float32),
119122
"sw_ver": (14, bytes_to_uint32),
123+
"flux": (19, bytes_to_float32),
120124
"gear_ratio": (20, bytes_to_float32),
125+
"gear_eff": (30, bytes_to_float32),
121126
}
122127

123128
register_info_map = {"control_mode": {1: "MIT", 2: "pos_speed", 3: "speed", 4: "torque_pos"}}

‎i2rt/robot_models/yam/yam.xml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,14 +27,14 @@
2727
<geom pos="-0.121816 -0.054846 -0.000111108" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" rgba="1 0.6 0 1" mesh="link_3_collision"/>
2828
<body name="link_4" pos="-0.245 -0.06 0" quat="1 0 0 0">
2929
<inertial pos="-0.0769778 -0.0527104 0.000154878" quat="0.667 0.667 -0.236 -0.236" mass="0.463511" diaginertia="0.000791532 0.000744733 0.000282116"/>
30-
<joint name="joint4" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-10 10"/>
30+
<joint name="joint4" pos="0 0 0" axis="0 0 1" range="-1.65 1.65" actuatorfrcrange="-10 10"/>
3131
<geom pos="-0.0569778 -0.0527104 0.000154878" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" rgba="1 0.6 0 1" mesh="link_4_collision"/>
3232
<body name="link_5" pos="-0.074 -0.0395 2.5e-05" quat="1 -1 1 1">
3333
<inertial pos="3.64861e-05 0.00025665 0.0353526" quat="1 0 0 0" mass="0.350962" diaginertia="0.000197802 0.0001775 0.000147402"/>
3434
<joint name="joint5" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-10 10"/>
3535
<geom pos="3.64861e-05 0.00025665 0.0353526" quat="9.38184e-07 -0.707105 -0.707108 9.38187e-07" type="mesh" rgba="1 0.6 0 1" mesh="link_5_collision"/>
3636
<body name="link_6" pos="0 0.0353 0.0395" quat="1 -1 0 0">
37-
<inertial pos="-5.10177e-05 -0.00269952 0.0581778" quat="0.98383 0.174699 0.0 0.0" mass="0.60366" diaginertia="0.000956992 0.000799286 0.000469547"/>
37+
<inertial pos="-5.10177e-05 -0.00269952 0.0581778" quat="0.98383 0.174699 0.0 0.0" mass="0.55366" diaginertia="0.000956992 0.000799286 0.000469547"/>
3838
<joint name="joint6" pos="0 0 0" axis="0 0 1" range="-2.0944 2.0944" actuatorfrcrange="-10 10"/>
3939
<geom pos="-5.10177e-05 -0.00269952 0.0481778" quat="1 -1 -1 -1" type="mesh" rgba="1 0.6 0 1" mesh="link_6_collision"/>
4040
<site name="tcp_site" pos="0 0 0" quat="1 0 0 -1" size="0.005" rgba="1 0 0 1"/>

‎i2rt/robot_models/yam/yam_4310_linear.xml‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
<mujoco model="yam_v0">
2+
<compiler angle="radian" meshdir="assets"/>
3+
4+
<asset>
5+
<mesh name="base_link_collision" file="base_link_collision.stl"/>
6+
<mesh name="link_1_collision" file="link_1_collision.stl"/>
7+
<mesh name="link_2_collision" file="link_2_collision.stl"/>
8+
<mesh name="link_3_collision" file="link_3_collision.stl"/>
9+
<mesh name="link_4_collision" file="link_4_collision.stl"/>
10+
<mesh name="link_5_collision" file="link_5_collision.stl"/>
11+
<mesh name="link_6_collision" file="link_6_collision.stl"/>
12+
</asset>
13+
14+
<worldbody>
15+
<geom pos="-7.22812e-05 2.52572e-05 0.0159932" quat="1 0 0 0" type="mesh" rgba="1 0.6 0 1" mesh="base_link_collision"/>
16+
<body name="link_1" pos="0 0 0.0631" quat="1 0 0 1">
17+
<inertial pos="-0.00192861 -0.00795735 0.0208176" quat="0.297851 0.642634 0.360045 0.607185" mass="0.12415" diaginertia="0.000180751 0.000145532 5.89179e-05"/>
18+
<joint name="joint1" pos="0 0 0" axis="0 0 1" range="-2.61799 3.13" actuatorfrcrange="-10 10"/>
19+
<geom pos="-0.00192861 -0.00795735 0.0208176" quat="0.707105 0 0 -0.707108" type="mesh" rgba="1 0.6 0 1" mesh="link_1_collision"/>
20+
<body name="link_2" pos="2.5e-05 -0.02 0.0409" quat="1 1 1 1">
21+
<inertial pos="0.132008 2.10599e-06 0.000293073" quat="1 1 1 1" mass="1.24434" diaginertia="0.0121902 0.0121318 0.000683304"/>
22+
<joint name="joint2" pos="0 0 0" axis="0 0 1" range="0 3.65" actuatorfrcrange="-10 10"/>
23+
<geom pos="0.132008 2.10599e-06 0.000293073" quat="9.38184e-07 9.38187e-07 0.707108 0.707105" type="mesh" rgba="1 0.6 0 1" mesh="link_2_collision"/>
24+
<body name="link_3" pos="0.264 0 0" quat="0 1 0 0">
25+
<inertial pos="-0.121816 -0.054846 -0.000111108" quat="1 1 1 1" mass="0.853698" diaginertia="0.00699732 0.00696112 0.000788244"/>
26+
<joint name="joint3" pos="0 0 0" axis="0 0 1" range="0 3.13" actuatorfrcrange="-10 10"/>
27+
<geom pos="-0.121816 -0.054846 -0.000111108" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" rgba="1 0.6 0 1" mesh="link_3_collision"/>
28+
<body name="link_4" pos="-0.245 -0.06 0" quat="1 0 0 0">
29+
<inertial pos="-0.0769778 -0.0527104 0.000154878" quat="0.667 0.667 -0.236 -0.236" mass="0.463511" diaginertia="0.000791532 0.000744733 0.000282116"/>
30+
<joint name="joint4" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-10 10"/>
31+
<geom pos="-0.0569778 -0.0527104 0.000154878" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" rgba="1 0.6 0 1" mesh="link_4_collision"/>
32+
<body name="link_5" pos="-0.074 -0.0395 2.5e-05" quat="1 -1 1 1">
33+
<inertial pos="3.64861e-05 0.00025665 0.0353526" quat="1 0 0 0" mass="0.350962" diaginertia="0.000197802 0.0001775 0.000147402"/>
34+
<joint name="joint5" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-10 10"/>
35+
<geom pos="3.64861e-05 0.00025665 0.0353526" quat="9.38184e-07 -0.707105 -0.707108 9.38187e-07" type="mesh" rgba="1 0.6 0 1" mesh="link_5_collision"/>
36+
<body name="link_6" pos="0 0.0353 0.0395" quat="1 -1 0 0">
37+
<inertial pos="-5.10177e-05 -0.00269952 0.0581778" quat="0.98383 0.174699 0.0 0.0" mass="0.50366" diaginertia="0.000956992 0.000799286 0.000469547"/>
38+
<joint name="joint6" pos="0 0 0" axis="0 0 1" range="-2.0944 2.0944" actuatorfrcrange="-10 10"/>
39+
<geom pos="-5.10177e-05 -0.00269952 0.0481778" quat="1 -1 -1 -1" type="mesh" rgba="1 0.6 0 1" mesh="link_6_collision"/>
40+
<site name="tcp_site" pos="0 0 0" quat="1 0 0 -1" size="0.005" rgba="1 0 0 1"/>
41+
<site name="grasp_site" pos="0 0 0.1347" quat="1 0 0 -1" size="0.005" rgba="0 1 0 1"/>
42+
</body>
43+
</body>
44+
</body>
45+
</body>
46+
</body>
47+
</body>
48+
</worldbody>
49+
</mujoco>

‎i2rt/robot_models/yam/yam_lw_gripper.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@
3535
<joint name="joint5" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-10 10"/>
3636
<geom pos="3.64861e-05 0.00025665 0.0353526" quat="9.38184e-07 -0.707105 -0.707108 9.38187e-07" type="mesh" rgba="1 0.6 0 1" mesh="link_5_collision"/>
3737
<body name="link_6" pos="0 0.0353 0.0395" quat="1 -1 0 0">
38-
<inertial pos="-5.10177e-05 -0.01269952 0.0881778" quat="0.98383 0.174699 0.0 0.0" mass="0.40366" diaginertia="0.000956992 0.000799286 0.000469547"/>
38+
<inertial pos="-5.10177e-05 -0.01269952 0.0881778" quat="0.98383 0.174699 0.0 0.0" mass="0.30366" diaginertia="0.000956992 0.000799286 0.000469547"/>
3939
<joint name="joint6" pos="0 0 0" axis="0 0 1" range="-2.0944 2.0944" actuatorfrcrange="-10 10"/>
4040
<geom pos="-5.10177e-05 -0.00269952 0.0481778" quat="1 -1 -1 -1" type="mesh" rgba="1 0.6 0 1" mesh="link_6_collision"/>
4141
<site name="tcp_site" pos="0 0 0" quat="1 0 0 -1" size="0.005" rgba="1 0 0 1"/>

‎i2rt/robots/get_robot.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ def get_encoder_chain(can_interface: CanInterface) -> EncoderChain:
2323
def get_yam_robot(
2424
channel: str = "can0",
2525
gripper_type: GripperType = GripperType.CRANK_4310,
26+
zero_gravity_mode:bool = True,
2627
) -> MotorChainRobot:
2728
with_gripper = True
2829
with_teaching_handle = False
@@ -41,6 +42,9 @@ def get_yam_robot(
4142
]
4243
motor_offsets = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
4344
joint_limits = np.array([[-2.617, 3.13], [0, 3.65], [0.0, 3.13], [-1.57, 1.57], [-1.57, 1.57], [-2.09, 2.09]])
45+
joint_limits[:,0] += -0.15 # add some buffer to the joint limits
46+
joint_limits[:,1] += 0.15
47+
4448
motor_directions = [1, 1, 1, 1, 1, 1]
4549
kp = np.array([80, 80, 80, 40, 10, 10])
4650
kd = np.array([5, 5, 5, 1.5, 1.5, 1.5])
@@ -110,6 +114,7 @@ def get_yam_robot(
110114
joint_limits=joint_limits,
111115
kp=kp,
112116
kd=kd,
117+
zero_gravity_mode=zero_gravity_mode,
113118
)
114119

115120
if with_gripper:

‎i2rt/robots/motor_chain_robot.py‎

Lines changed: 16 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -77,8 +77,9 @@ def __init__(
7777
gripper_type: GripperType = GripperType.CRANK_4310,
7878
temp_record_flag: bool = False, # whether record the motor's temperature
7979
enable_gripper_calibration: bool = False, # whether to auto-detect gripper limits
80+
zero_gravity_mode:bool = True,
8081
# below are calibration parameters
81-
test_torque: float = 0.4, # test torque for gripper detection (Nm)
82+
test_torque: float = 0.5, # test torque for gripper detection (Nm)
8283
test_duration: float = 2.0, # max test duration for each direction (s)
8384
position_threshold: float = 0.01, # minimum position change to consider motor still moving (rad)
8485
check_interval: float = 0.05, # time interval between checks (s)
@@ -118,7 +119,7 @@ def __init__(
118119
logger.info(f"Using provided gripper limits: {gripper_limits}")
119120

120121

121-
self._last_gripper_command_qpos = None
122+
self._last_gripper_command_qpos = 1 # initialize as fully open
122123
assert clip_motor_torque >= 0.0
123124
self._clip_motor_torque = clip_motor_torque
124125
self.motor_chain = motor_chain
@@ -129,7 +130,7 @@ def __init__(
129130
self._gripper_index = gripper_index
130131
self.remapper = JointMapper({}, len(motor_chain)) # so it works without gripper
131132
self._gripper_limits = gripper_limits
132-
self._gripper_adjusted_qpos = None
133+
133134
if self._gripper_index is not None:
134135
self._gripper_force_limiter = GripperForceLimiter(
135136
max_force=limit_gripper_force, gripper_type=gripper_type, kp=kp[gripper_index]
@@ -181,23 +182,24 @@ def __init__(
181182
"Lower joint limits must be smaller than upper limits"
182183
)
183184
self._joint_limits = joint_limits
184-
185-
self._commands = JointCommands.init_all_zero(len(motor_chain))
186-
187185
self._command_lock = threading.Lock()
188186
self._state_lock = threading.Lock()
189187
self._joint_state: Optional[JointStates] = None
190188
while self._joint_state is None:
191189
# wait to recive joint data
192190
time.sleep(0.05)
193191
self._joint_state = self._motor_state_to_joint_state(self.motor_chain.read_states())
192+
self._commands = JointCommands.init_all_zero(len(motor_chain))
194193
# For SWE-454, check if the current qpos is in the joint limits
195194
self._check_current_qpos_in_joint_limits()
196195

197196
self._stop_event = threading.Event() # Add a stop event
198197
self._server_thread = threading.Thread(target=self.start_server, name="robot_server")
199198
self._server_thread.start()
200199

200+
if not zero_gravity_mode:
201+
# set current qpos as target pos with the default PD parameters
202+
self.command_joint_pos(self._joint_state.pos)
201203
def __repr__(self) -> str:
202204
return f"MotorChainRobot(motor_chain={self.motor_chain})"
203205

@@ -240,6 +242,8 @@ def _check_current_qpos_in_joint_limits(self, buffer_rad: float = 0.1) -> None:
240242
violation_details.append(f"Joint {i}: {pos:.4f} > {upper:.4f} (upper limit)")
241243

242244
violation_msg = "; ".join(violation_details)
245+
# turn off the main motor control thread as well.
246+
self.motor_chain.running = False
243247
raise RuntimeError(f"{self}: Joint limit violation detected: {violation_msg}, the root reason should be zero position offset. possible solution: 1. move the arm to zero position and power cycle the robot. 2. Recalibrate the motor zero position.")
244248

245249
def get_robot_info(self) -> Dict[str, Any]:
@@ -456,7 +460,7 @@ def command_joint_state(self, joint_state: Dict[str, np.ndarray]) -> None:
456460
self._commands.kd = kd
457461

458462
def zero_torque_mode(self) -> None:
459-
print("in zero_torque_mode", self)
463+
logging.info(f"Entering zero_torque_mode for {self}")
460464
with self._command_lock:
461465
self._commands = JointCommands.init_all_zero(len(self.motor_chain))
462466
self._kp = np.zeros(len(self.motor_chain))
@@ -556,3 +560,8 @@ def update_kp_kd(self, kp: np.ndarray, kd: np.ndarray) -> None:
556560
robot.command_joint_pos(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, gripper_pos]))
557561
time.sleep(4)
558562
print(robot.get_observations())
563+
elif args.operation_mode == "stay_current_qpos":
564+
current_qpos = robot.get_joint_pos()
565+
robot.command_joint_pos(current_qpos)
566+
while True:
567+
time.sleep(1)

‎i2rt/robots/utils.py‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
I2RT_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
1414
YAM_XML_PATH = os.path.join(I2RT_ROOT, "robot_models/yam/yam.xml")
1515
YAM_XML_LW_GRIPPER_PATH = os.path.join(I2RT_ROOT, "robot_models/yam/yam_lw_gripper.xml")
16+
YAM_XML_LINEAR_4310_PATH = os.path.join(I2RT_ROOT, "robot_models/yam/yam_4310_linear.xml")
1617
YAM_TEACHING_HANDLE_PATH = os.path.join(I2RT_ROOT, "robot_models/yam/yam_teaching_handle.xml")
1718

1819

@@ -36,7 +37,7 @@ def from_string_name(cls, name: str) -> "GripperType":
3637
return cls.YAM_TEACHING_HANDLE
3738
else:
3839
raise ValueError(
39-
f"Unknown gripper type: {name}, gripper has to be one of the following: {cls.CRANK_4310}, {cls.LINEAR_3507}, {cls.YAM_TEACHING_HANDLE}"
40+
f"Unknown gripper type: {name}, gripper has to be one of the following: {cls.CRANK_4310.value}, {cls.LINEAR_3507.value}, {cls.LINEAR_4310.value}, {cls.YAM_TEACHING_HANDLE.value}"
4041
)
4142

4243
def get_gripper_limits(self) -> Optional[tuple[float, float]]:
@@ -61,7 +62,7 @@ def get_xml_path(self) -> str:
6162
elif self == GripperType.LINEAR_3507:
6263
return YAM_XML_LW_GRIPPER_PATH
6364
elif self == GripperType.LINEAR_4310:
64-
raise NotImplementedError("Linear 4310 gripper is not supported yet")
65+
return YAM_XML_LINEAR_4310_PATH
6566
elif self == GripperType.YAM_TEACHING_HANDLE:
6667
return YAM_TEACHING_HANDLE_PATH
6768
else:
@@ -296,7 +297,6 @@ def update(self, gripper_state: Dict[str, float]) -> None:
296297

297298
if target_eff is not None:
298299
command_sign = np.sign(gripper_state["target_qpos"] - gripper_state["current_qpos"]) * self.sign
299-
300300
current_zero_eff_pos = (
301301
gripper_state["last_command_qpos"] - command_sign * np.abs(gripper_state["current_eff"]) / self._kp
302302
)
@@ -309,6 +309,8 @@ def update(self, gripper_state: Dict[str, float]) -> None:
309309
print(f"target_gripper_raw_pos: {target_gripper_raw_pos}")
310310
# Update gripper target position
311311
a = 0.1
312+
if self._gripper_adjusted_qpos is None: #initialize it to the target position
313+
self._gripper_adjusted_qpos = target_gripper_raw_pos
312314
self._gripper_adjusted_qpos = (1 - a) * self._gripper_adjusted_qpos + a * target_gripper_raw_pos
313315
return self._gripper_adjusted_qpos
314316
else:

0 commit comments

Comments
 (0)