新增o6/l6左手原始urdf
This commit is contained in:
@@ -71,6 +71,7 @@ Thumbs.db
|
||||
# Device-specific robot descriptions derived from local calibration runs
|
||||
# Includes full/partial zero-calibration outputs and local copies.
|
||||
/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
|
||||
/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
|
||||
|
||||
@@ -4,9 +4,11 @@ from ..registry import ProfileRegistry
|
||||
|
||||
|
||||
def register_profiles(registry: ProfileRegistry) -> None:
|
||||
from .left_transfer import build_profile as build_left_transfer_profile
|
||||
from .profile import build_profile
|
||||
|
||||
registry.register(build_profile())
|
||||
registry.register(build_left_transfer_profile())
|
||||
|
||||
|
||||
__all__ = ["register_profiles"]
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from copy import deepcopy
|
||||
import json
|
||||
import hashlib
|
||||
import math
|
||||
@@ -557,6 +558,41 @@ def load_l6_urdf_input(
|
||||
|
||||
|
||||
def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
from ...core import ProfileKey
|
||||
|
||||
key = ProfileKey.parse(str(payload.get("profile_id", "")))
|
||||
if key == KEY:
|
||||
profile = build_typed_profile()
|
||||
active_joints = ACTIVE_JOINTS
|
||||
passive_joints = PASSIVE_JOINTS
|
||||
active_transfers = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
|
||||
passive_transfers = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT
|
||||
else:
|
||||
from .left_transfer import (
|
||||
KEY as LEFT_TRANSFER_KEY,
|
||||
build_typed_profile as build_left_transfer_profile,
|
||||
left_joint_name,
|
||||
)
|
||||
|
||||
if key != LEFT_TRANSFER_KEY:
|
||||
raise ValueError(f"unsupported L6 runtime profile: {key.profile_id}")
|
||||
profile = build_left_transfer_profile()
|
||||
active_joints = tuple(left_joint_name(name) for name in ACTIVE_JOINTS)
|
||||
passive_joints = tuple(left_joint_name(name) for name in PASSIVE_JOINTS)
|
||||
active_transfers = {
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items()
|
||||
}
|
||||
passive_transfers = {
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items()
|
||||
}
|
||||
passive_set = frozenset(passive_joints)
|
||||
all_joints = frozenset(active_joints + passive_joints)
|
||||
mimic_sources = profile.zero.mimic_source_by_joint
|
||||
coupling_models = profile.zero.coupling_model_by_joint
|
||||
command_indices = profile.command.command_index_by_joint
|
||||
|
||||
required_top = {
|
||||
"schema_version", "profile_id", "layout_id", "model", "side",
|
||||
"serial_number", "calibration_scope", "publication_pointer",
|
||||
@@ -568,13 +604,14 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError("schema v6 calibration has unexpected top-level fields")
|
||||
if (
|
||||
payload["schema_version"] != 6
|
||||
or payload["profile_id"] != KEY.profile_id
|
||||
or payload["profile_id"] != profile.key.profile_id
|
||||
or payload["model"] != "L6"
|
||||
or payload["side"] != "right"
|
||||
or payload["side"] != profile.key.side
|
||||
or payload["layout_id"] != profile.key.layout
|
||||
or payload["calibration_scope"] != "partial"
|
||||
):
|
||||
raise ValueError("schema v6 identity is invalid")
|
||||
if payload["publication_pointer"] != "latest_partial_passed":
|
||||
if payload["publication_pointer"] != profile.artifacts.publication_pointer:
|
||||
raise ValueError("L6 partial result has the wrong publication pointer")
|
||||
if payload["curve_input_domain"] != "feedback_u8":
|
||||
raise ValueError("schema v6 must be indexed by feedback_u8")
|
||||
@@ -582,9 +619,9 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError("schema v6 must retain both motion directions")
|
||||
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
|
||||
raise ValueError("schema v6 units are invalid")
|
||||
if tuple(payload["command_names"]) != COMMAND_NAMES:
|
||||
if tuple(payload["command_names"]) != profile.command.names:
|
||||
raise ValueError("schema v6 command channel order is invalid")
|
||||
if payload["baseline_command_u8"] != [255] * 6:
|
||||
if tuple(payload["baseline_command_u8"]) != profile.command.baseline_u8:
|
||||
raise ValueError("schema v6 baseline must be six open commands")
|
||||
protected = payload["protected_inputs"]
|
||||
expected_hashes = {
|
||||
@@ -600,21 +637,20 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
):
|
||||
raise ValueError("schema v6 protected input hash is invalid")
|
||||
joints = payload["joints"]
|
||||
if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS:
|
||||
if not isinstance(joints, Mapping) or set(joints) != all_joints:
|
||||
raise ValueError("schema v6 must contain all 11 L6 revolute joints")
|
||||
profile = build_typed_profile()
|
||||
for name in ACTIVE_JOINTS + PASSIVE_JOINTS:
|
||||
for name in active_joints + passive_joints:
|
||||
joint = joints[name]
|
||||
motor = COMMAND_INDEX_BY_JOINT[
|
||||
MIMIC_SOURCE_BY_JOINT.get(name, name)
|
||||
motor = command_indices[
|
||||
mimic_sources.get(name, name)
|
||||
]
|
||||
if joint.get("urdf_joint") != name or int(joint.get("motor_index", -1)) != motor:
|
||||
raise ValueError(f"{name} has an invalid URDF/SDK mapping")
|
||||
if joint.get("sdk_channel") != COMMAND_NAMES[motor]:
|
||||
if joint.get("sdk_channel") != profile.command.names[motor]:
|
||||
raise ValueError(f"{name} has an invalid SDK channel")
|
||||
if joint.get("calibration_status") != profile.joint_coverage[name]:
|
||||
raise ValueError(f"{name} has an invalid coverage status")
|
||||
if joint.get("passive") is not (name in PASSIVE_JOINTS):
|
||||
if joint.get("passive") is not (name in passive_set):
|
||||
raise ValueError(f"{name} passive flag is invalid")
|
||||
if joint.get("zero_command_u8") != 255:
|
||||
raise ValueError(f"{name} zero command must be 255")
|
||||
@@ -624,11 +660,11 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError(f"{name}.{field} must contain 256 finite values")
|
||||
if np.any(np.diff(curve) > 1.0e-7):
|
||||
raise ValueError(f"{name}.{field} must be non-increasing")
|
||||
if name in PASSIVE_JOINTS:
|
||||
if joint.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]:
|
||||
if name in passive_set:
|
||||
if joint.get("source_joint") != mimic_sources[name]:
|
||||
raise ValueError(f"{name} mimic source is invalid")
|
||||
model = str(joint.get("coupling_model", ""))
|
||||
expected_model = COUPLING_MODEL_BY_JOINT.get(
|
||||
expected_model = coupling_models.get(
|
||||
name, "linear_mimic"
|
||||
)
|
||||
if model != expected_model:
|
||||
@@ -647,7 +683,7 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
"endpoint_linear_fallback"
|
||||
if model == "quadratic_runtime"
|
||||
else "exact_linear"
|
||||
if name in MEASURED_PASSIVE_JOINTS
|
||||
if name in profile.zero.fitted_mimic_joints
|
||||
else "cad_nominal"
|
||||
)
|
||||
# Early schema-v6 linear artifacts predate the explicit policy
|
||||
@@ -671,8 +707,8 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
)) > 1.0e-7:
|
||||
raise ValueError(f"{name} coupling offset is inconsistent")
|
||||
transferred_from = (
|
||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name)
|
||||
or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name)
|
||||
active_transfers.get(name)
|
||||
or passive_transfers.get(name)
|
||||
)
|
||||
if transferred_from is not None:
|
||||
if joint.get("transferred_from_joint") != transferred_from:
|
||||
@@ -688,6 +724,110 @@ def validate_l6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError("schema v6 quality.passed must be true")
|
||||
|
||||
|
||||
def build_l6_left_transferred_runtime_payload(
|
||||
*,
|
||||
right_payload: Mapping[str, Any],
|
||||
source_left_urdf: str | Path,
|
||||
transferred_left_urdf: str | Path,
|
||||
serial_number: str,
|
||||
) -> dict[str, Any]:
|
||||
"""Create a validated L6 left runtime JSON paired with a mirrored URDF."""
|
||||
from .left_transfer import (
|
||||
KEY as LEFT_TRANSFER_KEY,
|
||||
build_typed_profile as build_left_transfer_profile,
|
||||
left_joint_name,
|
||||
)
|
||||
|
||||
validate_l6_runtime_payload(right_payload)
|
||||
if str(right_payload["side"]) != "right":
|
||||
raise ValueError("L6 left transfer requires a measured right payload")
|
||||
left_source = Path(source_left_urdf).expanduser().resolve()
|
||||
left_urdf = Path(transferred_left_urdf).expanduser().resolve()
|
||||
if not left_source.is_file() or not left_urdf.is_file():
|
||||
raise ValueError("L6 left source and transferred URDF files are required")
|
||||
profile = build_left_transfer_profile()
|
||||
source_joint_names = {
|
||||
str(joint.get("name"))
|
||||
for joint in ET.parse(left_source).getroot().findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
if source_joint_names != profile.zero.active_joints | profile.zero.passive_joints:
|
||||
raise ValueError("L6 left source URDF differs from transferred profile")
|
||||
|
||||
payload = deepcopy(dict(right_payload))
|
||||
payload.update({
|
||||
"profile_id": LEFT_TRANSFER_KEY.profile_id,
|
||||
"layout_id": LEFT_TRANSFER_KEY.layout,
|
||||
"side": "left",
|
||||
"serial_number": str(serial_number),
|
||||
"publication_pointer": profile.artifacts.publication_pointer,
|
||||
})
|
||||
payload["protected_inputs"] = {
|
||||
**dict(right_payload["protected_inputs"]),
|
||||
"source_urdf_sha256": _sha256_file(left_source),
|
||||
}
|
||||
left_joints: dict[str, dict[str, Any]] = {}
|
||||
for right_name, source_item in right_payload["joints"].items():
|
||||
left_name = left_joint_name(str(right_name))
|
||||
item = deepcopy(dict(source_item))
|
||||
item["urdf_joint"] = left_name
|
||||
item["calibration_status"] = profile.joint_coverage[left_name]
|
||||
item["mirrored_from_joint"] = str(right_name)
|
||||
if "source_joint" in item:
|
||||
item["source_joint"] = left_joint_name(str(item["source_joint"]))
|
||||
if "transferred_from_joint" in item:
|
||||
item["transferred_from_joint"] = left_joint_name(
|
||||
str(item["transferred_from_joint"])
|
||||
)
|
||||
zero = item.get("zero_angles")
|
||||
if isinstance(zero, Mapping):
|
||||
zero = deepcopy(dict(zero))
|
||||
if "transferred_from_joint" in zero:
|
||||
zero["transferred_from_joint"] = left_joint_name(
|
||||
str(zero["transferred_from_joint"])
|
||||
)
|
||||
item["zero_angles"] = {
|
||||
**zero,
|
||||
"mirrored_from_joint": str(right_name),
|
||||
"transfer_kind": "right_measurement_mirrored_to_left",
|
||||
}
|
||||
left_joints[left_name] = item
|
||||
payload["joints"] = left_joints
|
||||
|
||||
quality = deepcopy(dict(right_payload["quality"]))
|
||||
thumb = quality.get("thumb_axis_zero")
|
||||
if isinstance(thumb, Mapping):
|
||||
thumb = deepcopy(dict(thumb))
|
||||
for field in (
|
||||
"offsets_rad",
|
||||
"cycle_offsets_rad",
|
||||
"geometry_fallback_reasons",
|
||||
"validation_error_by_joint_rad",
|
||||
):
|
||||
values = thumb.get(field)
|
||||
if isinstance(values, Mapping):
|
||||
thumb[field] = {
|
||||
left_joint_name(str(name)): value
|
||||
for name, value in values.items()
|
||||
}
|
||||
quality["thumb_axis_zero"] = thumb
|
||||
canonical_right = json.dumps(
|
||||
right_payload, ensure_ascii=False, sort_keys=True, separators=(",", ":")
|
||||
).encode("utf-8")
|
||||
quality["transfer_provenance"] = {
|
||||
"kind": "right_measurement_mirrored_to_left",
|
||||
"source_profile_id": str(right_payload["profile_id"]),
|
||||
"source_serial_number": str(right_payload["serial_number"]),
|
||||
"source_payload_sha256": hashlib.sha256(canonical_right).hexdigest(),
|
||||
"left_source_urdf_sha256": _sha256_file(left_source),
|
||||
"transferred_left_urdf_sha256": _sha256_file(left_urdf),
|
||||
"left_hand_measured": False,
|
||||
}
|
||||
payload["quality"] = quality
|
||||
validate_l6_runtime_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> Path:
|
||||
destination = Path(path).resolve()
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
@@ -730,6 +870,7 @@ __all__ = [
|
||||
"ALL_REVOLUTE_JOINTS",
|
||||
"artifact_hashes",
|
||||
"atomic_write_json",
|
||||
"build_l6_left_transferred_runtime_payload",
|
||||
"build_l6_urdf_input_payload",
|
||||
"build_l6_runtime_payload",
|
||||
"load_l6_urdf_input",
|
||||
|
||||
@@ -0,0 +1,155 @@
|
||||
"""Runtime-only L6 left profile derived from reviewed right-hand calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import replace
|
||||
|
||||
from ...core import ProfileKey
|
||||
from ..registry import EngineBindings, RegisteredProfile
|
||||
from .profile import build_typed_profile as build_right_typed_profile
|
||||
|
||||
|
||||
KEY = ProfileKey("L6", "left", "l6_left_transferred_8", 1)
|
||||
|
||||
|
||||
def left_joint_name(name: str) -> str:
|
||||
value = str(name)
|
||||
if not value.startswith("rh_"):
|
||||
raise ValueError(f"L6 right joint name cannot be mirrored: {value}")
|
||||
return "lh_" + value[3:]
|
||||
|
||||
|
||||
def build_typed_profile():
|
||||
right = build_right_typed_profile()
|
||||
command_index = {
|
||||
left_joint_name(name): index
|
||||
for name, index in right.command.command_index_by_joint.items()
|
||||
}
|
||||
active = frozenset(left_joint_name(name) for name in right.zero.active_joints)
|
||||
passive = frozenset(left_joint_name(name) for name in right.zero.passive_joints)
|
||||
measurements = {
|
||||
left_joint_name(name): replace(spec, joint=left_joint_name(name))
|
||||
for name, spec in right.measurement.measurements.items()
|
||||
}
|
||||
tasks = tuple(
|
||||
replace(task, joints=tuple(left_joint_name(name) for name in task.joints))
|
||||
for task in right.motion.tasks
|
||||
)
|
||||
return replace(
|
||||
right,
|
||||
key=KEY,
|
||||
namespace="/l6_left_transferred",
|
||||
command=replace(
|
||||
right.command,
|
||||
command_index_by_joint=command_index,
|
||||
urdf_joint_by_joint={name: name for name in active},
|
||||
),
|
||||
motion=replace(right.motion, tasks=tasks),
|
||||
measurement=replace(
|
||||
right.measurement,
|
||||
measurements=measurements,
|
||||
cross_view_sources={
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in right.measurement.cross_view_sources.items()
|
||||
},
|
||||
image_curve_joints=frozenset(
|
||||
left_joint_name(name)
|
||||
for name in right.measurement.image_curve_joints
|
||||
),
|
||||
),
|
||||
zero=replace(
|
||||
right.zero,
|
||||
active_joints=active,
|
||||
passive_joints=passive,
|
||||
direct_zero_joints=tuple(
|
||||
left_joint_name(name) for name in right.zero.direct_zero_joints
|
||||
),
|
||||
axis_joints=tuple(
|
||||
left_joint_name(name) for name in right.zero.axis_joints
|
||||
),
|
||||
mechanical_endpoint_joints=frozenset(
|
||||
left_joint_name(name)
|
||||
for name in right.zero.mechanical_endpoint_joints
|
||||
),
|
||||
post_solve_endpoint_joints=frozenset(
|
||||
left_joint_name(name)
|
||||
for name in right.zero.post_solve_endpoint_joints
|
||||
),
|
||||
mimic_source_by_joint={
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in right.zero.mimic_source_by_joint.items()
|
||||
},
|
||||
cad_frozen_joints=passive,
|
||||
endpoint_anchor_by_joint={
|
||||
left_joint_name(name): policy
|
||||
for name, policy in right.zero.endpoint_anchor_by_joint.items()
|
||||
},
|
||||
fitted_mimic_joints=frozenset(
|
||||
left_joint_name(name) for name in right.zero.fitted_mimic_joints
|
||||
),
|
||||
coupling_model_by_joint={
|
||||
left_joint_name(name): model
|
||||
for name, model in right.zero.coupling_model_by_joint.items()
|
||||
},
|
||||
),
|
||||
scope=replace(
|
||||
right.scope,
|
||||
calibrate_joints={
|
||||
scope: frozenset(left_joint_name(name) for name in names)
|
||||
for scope, names in right.scope.calibrate_joints.items()
|
||||
},
|
||||
frozen_joints={
|
||||
scope: frozenset(left_joint_name(name) for name in names)
|
||||
for scope, names in right.scope.frozen_joints.items()
|
||||
},
|
||||
),
|
||||
artifacts=replace(
|
||||
right.artifacts,
|
||||
calibration_filename=(
|
||||
"l6_left_{serial_number}_transferred_calibration.json"
|
||||
),
|
||||
corrected_urdf_filename=(
|
||||
"linkerhand_l6_left_{serial_number}_transferred.urdf"
|
||||
),
|
||||
publication_pointer="latest_transferred",
|
||||
session_compatibility_tokens=frozenset(
|
||||
{"l6_left_transfer_v1", "feedback_curves_v6"}
|
||||
),
|
||||
),
|
||||
joint_coverage={
|
||||
left_joint_name(name): status
|
||||
for name, status in right.joint_coverage.items()
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _unsupported(_args=None) -> None:
|
||||
raise ValueError(
|
||||
"L6 left transferred profile is runtime-only; calibrate the left hand "
|
||||
"with a dedicated measured profile before treating it as measured"
|
||||
)
|
||||
|
||||
|
||||
def build_profile() -> RegisteredProfile:
|
||||
from .motion import (
|
||||
build_calibration_motion_command,
|
||||
build_calibration_preparation_waypoints,
|
||||
build_calibration_return_waypoints,
|
||||
)
|
||||
|
||||
typed = build_typed_profile()
|
||||
return RegisteredProfile(
|
||||
profile=typed,
|
||||
engine=EngineBindings(
|
||||
hand_profile=typed,
|
||||
zero_profile=typed.zero,
|
||||
motion_command=build_calibration_motion_command,
|
||||
preparation_waypoints=build_calibration_preparation_waypoints,
|
||||
return_waypoints=build_calibration_return_waypoints,
|
||||
cli_main=_unsupported,
|
||||
node_main=_unsupported,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["KEY", "build_profile", "build_typed_profile", "left_joint_name"]
|
||||
@@ -71,6 +71,138 @@ def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str:
|
||||
)
|
||||
|
||||
|
||||
def _revolute_joints(root: ET.Element) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in root.findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
|
||||
|
||||
def _origin_rotation(joint: ET.Element) -> Rotation:
|
||||
origin = joint.find("origin")
|
||||
if origin is None or origin.get("rpy") is None:
|
||||
raise ValueError(f"joint {joint.get('name')} has no origin.rpy")
|
||||
return Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0")))
|
||||
|
||||
|
||||
def write_l6_left_from_right_calibration(
|
||||
*,
|
||||
source_left_urdf: str | Path,
|
||||
source_right_urdf: str | Path,
|
||||
calibrated_right_urdf: str | Path,
|
||||
destination_urdf: str | Path,
|
||||
) -> Path:
|
||||
"""Mirror a reviewed L6 right correction onto the original left CAD."""
|
||||
source_left = Path(source_left_urdf).expanduser().resolve()
|
||||
source_right = Path(source_right_urdf).expanduser().resolve()
|
||||
calibrated_right = Path(calibrated_right_urdf).expanduser().resolve()
|
||||
destination = Path(destination_urdf).expanduser().resolve()
|
||||
if not source_left.is_file() or not source_right.is_file():
|
||||
raise ValueError("original L6 left and right URDF files are required")
|
||||
if not calibrated_right.is_file():
|
||||
raise ValueError("calibrated L6 right URDF is required")
|
||||
if (
|
||||
"calibrated" in source_left.stem.lower()
|
||||
or "calibrated" in source_right.stem.lower()
|
||||
):
|
||||
raise ValueError("L6 transfer sources must be immutable original URDF files")
|
||||
|
||||
left_root = ET.parse(source_left).getroot()
|
||||
right_source_root = ET.parse(source_right).getroot()
|
||||
right_corrected_root = ET.parse(calibrated_right).getroot()
|
||||
left = _revolute_joints(left_root)
|
||||
right_source = _revolute_joints(right_source_root)
|
||||
right_corrected = _revolute_joints(right_corrected_root)
|
||||
expected_right = {name.replace("lh_", "rh_", 1) for name in left}
|
||||
if set(right_source) != expected_right or set(right_corrected) != expected_right:
|
||||
raise ValueError("L6 left/right revolute topology is not mirror-compatible")
|
||||
|
||||
joint_patches: dict[str, UrdfJointPatch] = {}
|
||||
for left_name, left_joint in sorted(left.items()):
|
||||
right_name = left_name.replace("lh_", "rh_", 1)
|
||||
source_joint = right_source[right_name]
|
||||
corrected_joint = right_corrected[right_name]
|
||||
source_axis_node = source_joint.find("axis")
|
||||
source_axis = _triplet(
|
||||
"1 0 0"
|
||||
if source_axis_node is None
|
||||
else source_axis_node.get("xyz", "1 0 0")
|
||||
)
|
||||
source_axis /= np.linalg.norm(source_axis)
|
||||
relative = _origin_rotation(source_joint).inv() * _origin_rotation(
|
||||
corrected_joint
|
||||
)
|
||||
rotation_vector = relative.as_rotvec()
|
||||
offset = float(rotation_vector @ source_axis)
|
||||
if np.linalg.norm(rotation_vector - offset * source_axis) > 1.0e-7:
|
||||
raise ValueError(
|
||||
f"right L6 correction is not about its declared axis: {right_name}"
|
||||
)
|
||||
if abs(offset) > math.radians(15.0):
|
||||
raise ValueError(f"right L6 correction exceeds transfer bound: {right_name}")
|
||||
corrected_limit = corrected_joint.find("limit")
|
||||
if corrected_limit is None:
|
||||
raise ValueError(f"right L6 joint has no limit: {right_name}")
|
||||
values: dict[str, str] = {
|
||||
"limit_lower": str(corrected_limit.get("lower")),
|
||||
"limit_upper": str(corrected_limit.get("upper")),
|
||||
}
|
||||
if abs(offset) > 1.0e-12:
|
||||
values["origin_rpy"] = _corrected_origin_rpy(left_joint, offset)
|
||||
left_mimic = left_joint.find("mimic")
|
||||
right_mimic = corrected_joint.find("mimic")
|
||||
if (left_mimic is None) != (right_mimic is None):
|
||||
raise ValueError(f"L6 passive topology differs: {left_name}")
|
||||
if left_mimic is not None and right_mimic is not None:
|
||||
expected_source = str(right_mimic.get("joint")).replace("rh_", "lh_", 1)
|
||||
if left_mimic.get("joint") != expected_source:
|
||||
raise ValueError(f"L6 mimic source is not mirrored: {left_name}")
|
||||
values["mimic_multiplier"] = str(right_mimic.get("multiplier"))
|
||||
values["mimic_offset"] = str(right_mimic.get("offset", "0"))
|
||||
joint_patches[left_name] = UrdfJointPatch(**values)
|
||||
|
||||
right_equalities = {
|
||||
str(node.get("joint1")): node
|
||||
for node in right_corrected_root.findall("./mujoco/equality/joint")
|
||||
}
|
||||
equality_patches: dict[str, MujocoEqualityPatch] = {}
|
||||
for left_equality in left_root.findall("./mujoco/equality/joint"):
|
||||
left_target = str(left_equality.get("joint1", ""))
|
||||
left_source_name = str(left_equality.get("joint2", ""))
|
||||
right_target = left_target.replace("lh_", "rh_", 1)
|
||||
right_equality = right_equalities.get(right_target)
|
||||
if right_equality is None:
|
||||
raise ValueError(f"right L6 equality is missing: {right_target}")
|
||||
expected_left_source = str(right_equality.get("joint2", "")).replace(
|
||||
"rh_", "lh_", 1
|
||||
)
|
||||
if left_source_name != expected_left_source:
|
||||
raise ValueError(f"L6 equality source is not mirrored: {left_target}")
|
||||
equality_name = str(left_equality.get("name", ""))
|
||||
if not equality_name:
|
||||
raise ValueError(f"left L6 equality has no name: {left_target}")
|
||||
equality_patches[equality_name] = MujocoEqualityPatch(
|
||||
polycoef=str(right_equality.get("polycoef", "")),
|
||||
expected_joint1=left_target,
|
||||
expected_joint2=left_source_name,
|
||||
)
|
||||
if len(equality_patches) != len(right_equalities):
|
||||
raise ValueError("L6 left/right equality topology differs")
|
||||
|
||||
write_urdf_patches(
|
||||
source_urdf=source_left,
|
||||
destination_urdf=destination,
|
||||
patches=UrdfPatchSet(
|
||||
joints=joint_patches,
|
||||
mujoco_equalities=equality_patches,
|
||||
),
|
||||
forbidden_source_stem_patterns=(r"calibrated",),
|
||||
copy_complete_mesh_directory=True,
|
||||
)
|
||||
return destination
|
||||
|
||||
|
||||
def _validate_passive_ranges(
|
||||
joints: Mapping[str, ET.Element], result: L6FitResult
|
||||
) -> None:
|
||||
@@ -290,4 +422,8 @@ def write_l6_corrected_urdf(
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["L6UrdfCorrection", "write_l6_corrected_urdf"]
|
||||
__all__ = [
|
||||
"L6UrdfCorrection",
|
||||
"write_l6_corrected_urdf",
|
||||
"write_l6_left_from_right_calibration",
|
||||
]
|
||||
|
||||
@@ -4,9 +4,11 @@ from ..registry import ProfileRegistry
|
||||
|
||||
|
||||
def register_profiles(registry: ProfileRegistry) -> None:
|
||||
from .left_transfer import build_profile as build_left_transfer_profile
|
||||
from .profile import build_profile
|
||||
|
||||
registry.register(build_profile())
|
||||
registry.register(build_left_transfer_profile())
|
||||
|
||||
|
||||
__all__ = ["register_profiles"]
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from copy import deepcopy
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
@@ -240,6 +241,41 @@ def build_o6_runtime_payload(
|
||||
|
||||
|
||||
def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
from ...core import ProfileKey
|
||||
|
||||
key = ProfileKey.parse(str(payload.get("profile_id", "")))
|
||||
if key == KEY:
|
||||
profile = build_typed_profile()
|
||||
active_joints = ACTIVE_JOINTS
|
||||
passive_joints = PASSIVE_JOINTS
|
||||
active_transfers = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
|
||||
passive_transfers = TRANSFERRED_PASSIVE_SOURCE_BY_JOINT
|
||||
else:
|
||||
from .left_transfer import (
|
||||
KEY as LEFT_TRANSFER_KEY,
|
||||
build_typed_profile as build_left_transfer_profile,
|
||||
left_joint_name,
|
||||
)
|
||||
|
||||
if key != LEFT_TRANSFER_KEY:
|
||||
raise ValueError(f"unsupported O6 runtime profile: {key.profile_id}")
|
||||
profile = build_left_transfer_profile()
|
||||
active_joints = tuple(left_joint_name(name) for name in ACTIVE_JOINTS)
|
||||
passive_joints = tuple(left_joint_name(name) for name in PASSIVE_JOINTS)
|
||||
active_transfers = {
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items()
|
||||
}
|
||||
passive_transfers = {
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.items()
|
||||
}
|
||||
passive_set = frozenset(passive_joints)
|
||||
all_joints = frozenset(active_joints + passive_joints)
|
||||
mimic_sources = profile.zero.mimic_source_by_joint
|
||||
coupling_models = profile.zero.coupling_model_by_joint
|
||||
command_indices = profile.command.command_index_by_joint
|
||||
|
||||
required = {
|
||||
"schema_version", "profile_id", "layout_id", "model", "side",
|
||||
"serial_number", "calibration_scope", "publication_pointer", "angle_unit",
|
||||
@@ -249,16 +285,17 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
if set(payload) != required:
|
||||
raise ValueError("O6 schema v6 has unexpected top-level fields")
|
||||
if (
|
||||
payload["schema_version"] != 6 or payload["profile_id"] != KEY.profile_id
|
||||
or payload["model"] != "O6" or payload["side"] != "right"
|
||||
or payload["layout_id"] != KEY.layout
|
||||
payload["schema_version"] != 6
|
||||
or payload["profile_id"] != profile.key.profile_id
|
||||
or payload["model"] != "O6" or payload["side"] != profile.key.side
|
||||
or payload["layout_id"] != profile.key.layout
|
||||
or payload["calibration_scope"] != "partial"
|
||||
or payload["publication_pointer"] != "latest_partial_passed"
|
||||
or payload["publication_pointer"] != profile.artifacts.publication_pointer
|
||||
or payload["curve_input_domain"] != "feedback_u8"
|
||||
or payload["runtime_curve_policy"] != "direction_aware"
|
||||
or payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]
|
||||
or tuple(payload["command_names"]) != COMMAND_NAMES
|
||||
or payload["baseline_command_u8"] != [255] * 6
|
||||
or tuple(payload["command_names"]) != profile.command.names
|
||||
or tuple(payload["baseline_command_u8"]) != profile.command.baseline_u8
|
||||
):
|
||||
raise ValueError("O6 schema v6 identity or command contract is invalid")
|
||||
protected = payload["protected_inputs"]
|
||||
@@ -268,17 +305,16 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
} or any(len(str(value)) != 64 for value in protected.values()):
|
||||
raise ValueError("O6 protected inputs are invalid")
|
||||
joints = payload["joints"]
|
||||
if not isinstance(joints, Mapping) or set(joints) != ALL_REVOLUTE_JOINTS:
|
||||
if not isinstance(joints, Mapping) or set(joints) != all_joints:
|
||||
raise ValueError("O6 schema v6 must contain all 11 revolute joints")
|
||||
profile = build_typed_profile()
|
||||
for name in ACTIVE_JOINTS + PASSIVE_JOINTS:
|
||||
for name in active_joints + passive_joints:
|
||||
item = joints[name]
|
||||
motor = COMMAND_INDEX_BY_JOINT[MIMIC_SOURCE_BY_JOINT.get(name, name)]
|
||||
motor = command_indices[mimic_sources.get(name, name)]
|
||||
if (
|
||||
item.get("urdf_joint") != name or item.get("motor_index") != motor
|
||||
or item.get("sdk_channel") != COMMAND_NAMES[motor]
|
||||
or item.get("sdk_channel") != profile.command.names[motor]
|
||||
or item.get("calibration_status") != profile.joint_coverage[name]
|
||||
or item.get("passive") is not (name in PASSIVE_JOINTS)
|
||||
or item.get("passive") is not (name in passive_set)
|
||||
or item.get("zero_command_u8") != 255
|
||||
):
|
||||
raise ValueError(f"O6 joint contract is invalid: {name}")
|
||||
@@ -288,10 +324,10 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError(f"{name}.{field} must contain 256 finite values")
|
||||
if np.any(np.diff(values) > 1e-7):
|
||||
raise ValueError(f"{name}.{field} must be non-increasing")
|
||||
if name in PASSIVE_JOINTS:
|
||||
if name in passive_set:
|
||||
if (
|
||||
item.get("source_joint") != MIMIC_SOURCE_BY_JOINT[name]
|
||||
or item.get("coupling_model") != COUPLING_MODEL_BY_JOINT[name]
|
||||
item.get("source_joint") != mimic_sources[name]
|
||||
or item.get("coupling_model") != coupling_models[name]
|
||||
or item.get("urdf_mimic_enabled") is not True
|
||||
or item.get("urdf_mimic_policy") != "endpoint_linear_fallback"
|
||||
):
|
||||
@@ -310,7 +346,7 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
multiplier = float(item.get("mimic_multiplier", "nan"))
|
||||
if not math.isfinite(multiplier) or multiplier <= 0.0:
|
||||
raise ValueError(f"O6 passive mimic multiplier is invalid: {name}")
|
||||
donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name) or TRANSFERRED_PASSIVE_SOURCE_BY_JOINT.get(name)
|
||||
donor = active_transfers.get(name) or passive_transfers.get(name)
|
||||
if donor is not None:
|
||||
if item.get("transferred_from_joint") != donor:
|
||||
raise ValueError(f"O6 transfer provenance is invalid: {name}")
|
||||
@@ -321,6 +357,112 @@ def validate_o6_runtime_payload(payload: Mapping[str, Any]) -> None:
|
||||
raise ValueError("O6 quality.passed must be true")
|
||||
|
||||
|
||||
def build_o6_left_transferred_runtime_payload(
|
||||
*,
|
||||
right_payload: Mapping[str, Any],
|
||||
source_left_urdf: str | Path,
|
||||
transferred_left_urdf: str | Path,
|
||||
serial_number: str,
|
||||
) -> dict[str, Any]:
|
||||
"""Create a validated left runtime JSON paired with a mirrored URDF."""
|
||||
from .left_transfer import (
|
||||
KEY as LEFT_TRANSFER_KEY,
|
||||
build_typed_profile as build_left_transfer_profile,
|
||||
left_joint_name,
|
||||
)
|
||||
|
||||
validate_o6_runtime_payload(right_payload)
|
||||
if str(right_payload["side"]) != "right":
|
||||
raise ValueError("O6 left transfer requires a measured right payload")
|
||||
left_source = Path(source_left_urdf).expanduser().resolve()
|
||||
left_urdf = Path(transferred_left_urdf).expanduser().resolve()
|
||||
if not left_source.is_file() or not left_urdf.is_file():
|
||||
raise ValueError("O6 left source and transferred URDF files are required")
|
||||
profile = build_left_transfer_profile()
|
||||
source_joint_names = {
|
||||
str(joint.get("name"))
|
||||
for joint in ET.parse(left_source).getroot().findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
if source_joint_names != profile.zero.active_joints | profile.zero.passive_joints:
|
||||
raise ValueError("O6 left source URDF differs from transferred profile")
|
||||
|
||||
payload = deepcopy(dict(right_payload))
|
||||
payload.update({
|
||||
"profile_id": LEFT_TRANSFER_KEY.profile_id,
|
||||
"layout_id": LEFT_TRANSFER_KEY.layout,
|
||||
"side": "left",
|
||||
"serial_number": str(serial_number),
|
||||
"publication_pointer": profile.artifacts.publication_pointer,
|
||||
})
|
||||
payload["protected_inputs"] = {
|
||||
**dict(right_payload["protected_inputs"]),
|
||||
"source_urdf_sha256": _sha256(left_source),
|
||||
}
|
||||
left_joints: dict[str, dict[str, Any]] = {}
|
||||
for right_name, source_item in right_payload["joints"].items():
|
||||
left_name = left_joint_name(str(right_name))
|
||||
item = deepcopy(dict(source_item))
|
||||
item["urdf_joint"] = left_name
|
||||
item["calibration_status"] = profile.joint_coverage[left_name]
|
||||
item["mirrored_from_joint"] = str(right_name)
|
||||
if "source_joint" in item:
|
||||
item["source_joint"] = left_joint_name(str(item["source_joint"]))
|
||||
if "transferred_from_joint" in item:
|
||||
item["transferred_from_joint"] = left_joint_name(
|
||||
str(item["transferred_from_joint"])
|
||||
)
|
||||
zero = item.get("zero_angles")
|
||||
if isinstance(zero, Mapping):
|
||||
item["zero_angles"] = {
|
||||
**dict(zero),
|
||||
"mirrored_from_joint": str(right_name),
|
||||
"transfer_kind": "right_measurement_mirrored_to_left",
|
||||
}
|
||||
left_joints[left_name] = item
|
||||
payload["joints"] = left_joints
|
||||
|
||||
quality = deepcopy(dict(right_payload["quality"]))
|
||||
hysteresis = quality.get("maximum_hysteresis_by_joint_rad")
|
||||
if isinstance(hysteresis, Mapping):
|
||||
quality["maximum_hysteresis_by_joint_rad"] = {
|
||||
left_joint_name(str(name)): value
|
||||
for name, value in hysteresis.items()
|
||||
}
|
||||
thumb = quality.get("thumb_axis_zero")
|
||||
if isinstance(thumb, Mapping):
|
||||
thumb = deepcopy(dict(thumb))
|
||||
for field in (
|
||||
"offsets_rad",
|
||||
"candidate_geometry_offsets_rad",
|
||||
"policy_by_joint",
|
||||
"validation_error_by_joint_rad",
|
||||
"validation_line_error_by_joint_m",
|
||||
):
|
||||
values = thumb.get(field)
|
||||
if isinstance(values, Mapping):
|
||||
thumb[field] = {
|
||||
left_joint_name(str(name)): value
|
||||
for name, value in values.items()
|
||||
}
|
||||
quality["thumb_axis_zero"] = thumb
|
||||
canonical_right = json.dumps(
|
||||
right_payload, ensure_ascii=False, sort_keys=True, separators=(",", ":")
|
||||
).encode("utf-8")
|
||||
quality["transfer_provenance"] = {
|
||||
"kind": "right_measurement_mirrored_to_left",
|
||||
"source_profile_id": str(right_payload["profile_id"]),
|
||||
"source_serial_number": str(right_payload["serial_number"]),
|
||||
"source_payload_sha256": hashlib.sha256(canonical_right).hexdigest(),
|
||||
"left_source_urdf_sha256": _sha256(left_source),
|
||||
"transferred_left_urdf_sha256": _sha256(left_urdf),
|
||||
"left_hand_measured": False,
|
||||
}
|
||||
payload["quality"] = quality
|
||||
validate_o6_runtime_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def build_o6_urdf_input_payload(
|
||||
*, serial_number: str, source_urdf: str | Path, result: O6FitResult
|
||||
) -> dict[str, Any]:
|
||||
@@ -444,7 +586,8 @@ def load_o6_urdf_input(
|
||||
|
||||
__all__ = [
|
||||
"ALL_REVOLUTE_JOINTS", "artifact_hashes", "atomic_write_json",
|
||||
"build_o6_runtime_payload", "build_o6_urdf_input_payload", "load_o6_urdf_input",
|
||||
"build_o6_left_transferred_runtime_payload", "build_o6_runtime_payload",
|
||||
"build_o6_urdf_input_payload", "load_o6_urdf_input",
|
||||
"publish_partial_session", "validate_o6_runtime_payload",
|
||||
"validate_o6_urdf_input_payload",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,159 @@
|
||||
"""Runtime-only O6 left profile derived from reviewed right-hand calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import replace
|
||||
|
||||
from ...core import ProfileKey
|
||||
from ..registry import EngineBindings, RegisteredProfile
|
||||
from .profile import build_typed_profile as build_right_typed_profile
|
||||
|
||||
|
||||
KEY = ProfileKey("O6", "left", "o6_left_transferred_8", 1)
|
||||
|
||||
|
||||
def left_joint_name(name: str) -> str:
|
||||
value = str(name)
|
||||
if not value.startswith("rh_"):
|
||||
raise ValueError(f"O6 right joint name cannot be mirrored: {value}")
|
||||
return "lh_" + value[3:]
|
||||
|
||||
|
||||
def build_typed_profile():
|
||||
right = build_right_typed_profile()
|
||||
command_index = {
|
||||
left_joint_name(name): index
|
||||
for name, index in right.command.command_index_by_joint.items()
|
||||
}
|
||||
active = frozenset(left_joint_name(name) for name in right.zero.active_joints)
|
||||
passive = frozenset(left_joint_name(name) for name in right.zero.passive_joints)
|
||||
measurements = {
|
||||
left_joint_name(name): replace(spec, joint=left_joint_name(name))
|
||||
for name, spec in right.measurement.measurements.items()
|
||||
}
|
||||
tasks = tuple(
|
||||
replace(task, joints=tuple(left_joint_name(name) for name in task.joints))
|
||||
for task in right.motion.tasks
|
||||
)
|
||||
return replace(
|
||||
right,
|
||||
key=KEY,
|
||||
namespace="/o6_left_transferred",
|
||||
command=replace(
|
||||
right.command,
|
||||
command_index_by_joint=command_index,
|
||||
urdf_joint_by_joint={name: name for name in active},
|
||||
),
|
||||
motion=replace(right.motion, tasks=tasks),
|
||||
measurement=replace(
|
||||
right.measurement,
|
||||
measurements=measurements,
|
||||
cross_view_sources={
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in right.measurement.cross_view_sources.items()
|
||||
},
|
||||
image_curve_joints=frozenset(
|
||||
left_joint_name(name)
|
||||
for name in right.measurement.image_curve_joints
|
||||
),
|
||||
),
|
||||
zero=replace(
|
||||
right.zero,
|
||||
active_joints=active,
|
||||
passive_joints=passive,
|
||||
direct_zero_joints=tuple(
|
||||
left_joint_name(name) for name in right.zero.direct_zero_joints
|
||||
),
|
||||
axis_joints=tuple(
|
||||
left_joint_name(name) for name in right.zero.axis_joints
|
||||
),
|
||||
mechanical_endpoint_joints=frozenset(
|
||||
left_joint_name(name)
|
||||
for name in right.zero.mechanical_endpoint_joints
|
||||
),
|
||||
post_solve_endpoint_joints=frozenset(
|
||||
left_joint_name(name)
|
||||
for name in right.zero.post_solve_endpoint_joints
|
||||
),
|
||||
mimic_source_by_joint={
|
||||
left_joint_name(name): left_joint_name(source)
|
||||
for name, source in right.zero.mimic_source_by_joint.items()
|
||||
},
|
||||
cad_frozen_joints=passive,
|
||||
endpoint_anchor_by_joint={
|
||||
left_joint_name(name): policy
|
||||
for name, policy in right.zero.endpoint_anchor_by_joint.items()
|
||||
},
|
||||
fitted_mimic_joints=frozenset(
|
||||
left_joint_name(name) for name in right.zero.fitted_mimic_joints
|
||||
),
|
||||
coupling_model_by_joint={
|
||||
left_joint_name(name): model
|
||||
for name, model in right.zero.coupling_model_by_joint.items()
|
||||
},
|
||||
),
|
||||
scope=replace(
|
||||
right.scope,
|
||||
calibrate_joints={
|
||||
scope: frozenset(left_joint_name(name) for name in names)
|
||||
for scope, names in right.scope.calibrate_joints.items()
|
||||
},
|
||||
frozen_joints={
|
||||
scope: frozenset(left_joint_name(name) for name in names)
|
||||
for scope, names in right.scope.frozen_joints.items()
|
||||
},
|
||||
),
|
||||
artifacts=replace(
|
||||
right.artifacts,
|
||||
calibration_filename=(
|
||||
"o6_left_{serial_number}_transferred_calibration.json"
|
||||
),
|
||||
corrected_urdf_filename=(
|
||||
"linkerhand_o6_left_{serial_number}_transferred.urdf"
|
||||
),
|
||||
publication_pointer="latest_transferred",
|
||||
session_compatibility_tokens=frozenset(
|
||||
{"o6_left_transfer_v1", "feedback_curves_v6"}
|
||||
),
|
||||
),
|
||||
joint_coverage={
|
||||
name: (
|
||||
"transferred_static_dynamic"
|
||||
if name in active
|
||||
else "transferred_dynamic_cad_static"
|
||||
)
|
||||
for name in active | passive
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _unsupported(_args=None) -> None:
|
||||
raise ValueError(
|
||||
"O6 left transferred profile is runtime-only; calibrate the left hand "
|
||||
"with a dedicated measured profile before treating it as measured"
|
||||
)
|
||||
|
||||
|
||||
def build_profile() -> RegisteredProfile:
|
||||
from ..l6.motion import (
|
||||
build_calibration_motion_command,
|
||||
build_calibration_preparation_waypoints,
|
||||
build_calibration_return_waypoints,
|
||||
)
|
||||
|
||||
typed = build_typed_profile()
|
||||
return RegisteredProfile(
|
||||
profile=typed,
|
||||
engine=EngineBindings(
|
||||
hand_profile=typed,
|
||||
zero_profile=typed.zero,
|
||||
motion_command=build_calibration_motion_command,
|
||||
preparation_waypoints=build_calibration_preparation_waypoints,
|
||||
return_waypoints=build_calibration_return_waypoints,
|
||||
cli_main=_unsupported,
|
||||
node_main=_unsupported,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["KEY", "build_profile", "build_typed_profile", "left_joint_name"]
|
||||
@@ -9,9 +9,10 @@ from typing import Mapping
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches
|
||||
from ..l6.urdf import L6UrdfCorrection, _corrected_origin_rpy
|
||||
from ..l6.urdf import L6UrdfCorrection, _corrected_origin_rpy, _triplet
|
||||
from .fitting import O6FitResult
|
||||
from .profile import (
|
||||
CALIBRATED_ACTIVE_JOINTS,
|
||||
@@ -27,6 +28,111 @@ from .profile import (
|
||||
O6UrdfCorrection = L6UrdfCorrection
|
||||
|
||||
|
||||
def _joint_map(root: ET.Element) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in root.findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
|
||||
|
||||
def _origin_rotation(joint: ET.Element) -> Rotation:
|
||||
origin = joint.find("origin")
|
||||
if origin is None or origin.get("rpy") is None:
|
||||
raise ValueError(f"joint {joint.get('name')} has no origin.rpy")
|
||||
return Rotation.from_euler("xyz", _triplet(origin.get("rpy", "0 0 0")))
|
||||
|
||||
|
||||
def write_o6_left_from_right_calibration(
|
||||
*,
|
||||
source_left_urdf: str | Path,
|
||||
source_right_urdf: str | Path,
|
||||
calibrated_right_urdf: str | Path,
|
||||
destination_urdf: str | Path,
|
||||
) -> Path:
|
||||
"""Mirror reviewed right-hand scalar corrections onto left-hand CAD.
|
||||
|
||||
This is an explicitly transferred preview, not a left-hand measurement.
|
||||
Left mesh, inertia, topology and joint origins remain authoritative. The
|
||||
measured right-hand ranges and passive ratios are scalar mechanism data;
|
||||
the yaw origin correction is reapplied about the left joint's mirrored
|
||||
local axis instead of copying the right-hand Euler angle.
|
||||
"""
|
||||
source_left = Path(source_left_urdf).expanduser().resolve()
|
||||
source_right = Path(source_right_urdf).expanduser().resolve()
|
||||
calibrated_right = Path(calibrated_right_urdf).expanduser().resolve()
|
||||
destination = Path(destination_urdf).expanduser().resolve()
|
||||
if not source_left.is_file() or not source_right.is_file():
|
||||
raise ValueError("original O6 left and right URDF files are required")
|
||||
if not calibrated_right.is_file():
|
||||
raise ValueError("calibrated O6 right URDF is required")
|
||||
if "calibrated" in source_left.stem.lower() or "calibrated" in source_right.stem.lower():
|
||||
raise ValueError("O6 transfer sources must be immutable original URDF files")
|
||||
|
||||
left_root = ET.parse(source_left).getroot()
|
||||
right_source_root = ET.parse(source_right).getroot()
|
||||
right_corrected_root = ET.parse(calibrated_right).getroot()
|
||||
left = _joint_map(left_root)
|
||||
right_source = _joint_map(right_source_root)
|
||||
right_corrected = _joint_map(right_corrected_root)
|
||||
expected_right = {name.replace("lh_", "rh_", 1) for name in left}
|
||||
if set(right_source) != expected_right or set(right_corrected) != expected_right:
|
||||
raise ValueError("O6 left/right revolute topology is not mirror-compatible")
|
||||
|
||||
right_yaw_source = right_source["rh_thumb_cmc_yaw"]
|
||||
right_yaw_corrected = right_corrected["rh_thumb_cmc_yaw"]
|
||||
axis = _triplet(right_yaw_source.find("axis").get("xyz", "0 0 -1"))
|
||||
axis /= np.linalg.norm(axis)
|
||||
relative = _origin_rotation(right_yaw_source).inv() * _origin_rotation(
|
||||
right_yaw_corrected
|
||||
)
|
||||
rotation_vector = relative.as_rotvec()
|
||||
yaw_offset = float(rotation_vector @ axis)
|
||||
if np.linalg.norm(rotation_vector - yaw_offset * axis) > 1.0e-8:
|
||||
raise ValueError("right O6 yaw correction is not about its declared axis")
|
||||
if abs(yaw_offset) > math.radians(15.0):
|
||||
raise ValueError("right O6 yaw correction exceeds transfer safety bound")
|
||||
|
||||
source_pitch_rpy = right_source["rh_thumb_cmc_pitch"].find("origin").get("rpy")
|
||||
corrected_pitch_rpy = right_corrected["rh_thumb_cmc_pitch"].find("origin").get("rpy")
|
||||
if source_pitch_rpy != corrected_pitch_rpy:
|
||||
raise ValueError("right O6 calibration does not use the reviewed CAD pitch zero")
|
||||
|
||||
patches: dict[str, UrdfJointPatch] = {}
|
||||
for left_name, left_joint in sorted(left.items()):
|
||||
right_name = left_name.replace("lh_", "rh_", 1)
|
||||
right_joint = right_corrected[right_name]
|
||||
right_limit = right_joint.find("limit")
|
||||
if right_limit is None:
|
||||
raise ValueError(f"right O6 joint has no limit: {right_name}")
|
||||
values: dict[str, str] = {
|
||||
"limit_lower": str(right_limit.get("lower")),
|
||||
"limit_upper": str(right_limit.get("upper")),
|
||||
}
|
||||
if left_name == "lh_thumb_cmc_yaw":
|
||||
values["origin_rpy"] = _corrected_origin_rpy(left_joint, yaw_offset)
|
||||
left_mimic = left_joint.find("mimic")
|
||||
right_mimic = right_joint.find("mimic")
|
||||
if (left_mimic is None) != (right_mimic is None):
|
||||
raise ValueError(f"O6 passive topology differs: {left_name}")
|
||||
if left_mimic is not None and right_mimic is not None:
|
||||
expected_source = str(right_mimic.get("joint")).replace("rh_", "lh_", 1)
|
||||
if left_mimic.get("joint") != expected_source:
|
||||
raise ValueError(f"O6 mimic source is not mirrored: {left_name}")
|
||||
values["mimic_multiplier"] = str(right_mimic.get("multiplier"))
|
||||
values["mimic_offset"] = str(right_mimic.get("offset", "0"))
|
||||
patches[left_name] = UrdfJointPatch(**values)
|
||||
|
||||
write_urdf_patches(
|
||||
source_urdf=source_left,
|
||||
destination_urdf=destination,
|
||||
patches=UrdfPatchSet(joints=patches),
|
||||
forbidden_source_stem_patterns=(r"calibrated",),
|
||||
copy_complete_mesh_directory=True,
|
||||
)
|
||||
return destination
|
||||
|
||||
|
||||
def _validate_passive_ranges(
|
||||
joints: Mapping[str, ET.Element], result: O6FitResult
|
||||
) -> dict[str, tuple[float, float]]:
|
||||
@@ -169,4 +275,8 @@ def write_o6_corrected_urdf(
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["O6UrdfCorrection", "write_o6_corrected_urdf"]
|
||||
__all__ = [
|
||||
"O6UrdfCorrection",
|
||||
"write_o6_corrected_urdf",
|
||||
"write_o6_left_from_right_calibration",
|
||||
]
|
||||
|
||||
@@ -145,7 +145,7 @@ def test_runtime_has_no_concrete_model_or_view_assumption() -> None:
|
||||
|
||||
def test_every_registered_profile_passes_static_integrity_checks() -> None:
|
||||
registry = get_default_registry()
|
||||
assert len(registry) == 5
|
||||
assert len(registry) == 7
|
||||
for registered in registry:
|
||||
validate_profile(registered.profile)
|
||||
assert registered.profile.zero.active_joints
|
||||
|
||||
@@ -16,6 +16,7 @@ from linkerhand_calibration.calibrated_joint_state_bridge import (
|
||||
from linkerhand_calibration.core import validate_profile
|
||||
from linkerhand_calibration.models.l6.artifacts import (
|
||||
atomic_write_json,
|
||||
build_l6_left_transferred_runtime_payload,
|
||||
build_l6_urdf_input_payload,
|
||||
build_l6_runtime_payload,
|
||||
load_l6_urdf_input,
|
||||
@@ -44,7 +45,10 @@ from linkerhand_calibration.models.l6.profile import (
|
||||
build_typed_profile,
|
||||
)
|
||||
from linkerhand_calibration.models.l6.runner import render_l6_progress_zh
|
||||
from linkerhand_calibration.models.l6.urdf import write_l6_corrected_urdf
|
||||
from linkerhand_calibration.models.l6.urdf import (
|
||||
write_l6_corrected_urdf,
|
||||
write_l6_left_from_right_calibration,
|
||||
)
|
||||
from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix
|
||||
from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel
|
||||
from linkerhand_calibration.product import load_product_config, sha256_file
|
||||
@@ -52,6 +56,7 @@ from linkerhand_calibration.product import load_product_config, sha256_file
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
SOURCE = PACKAGE / "urdf/l6_right/linkerhand_l6v3.1_right.urdf"
|
||||
LEFT_SOURCE = PACKAGE / "urdf/l6_left/linkerhand_l6v3.1_left.urdf"
|
||||
PRODUCT = PACKAGE / "config/l6_right_product.yaml"
|
||||
|
||||
TRAVELS = {
|
||||
@@ -714,6 +719,116 @@ def test_l6_urdf_writer_changes_only_authorized_joint_fields(tmp_path: Path) ->
|
||||
)
|
||||
|
||||
|
||||
def test_l6_right_corrections_are_mirrored_onto_left_cad(tmp_path: Path) -> None:
|
||||
result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records()))
|
||||
right = write_l6_corrected_urdf(
|
||||
source_urdf=SOURCE,
|
||||
output_directory=tmp_path / "right",
|
||||
serial_number="RIGHT_TEST",
|
||||
result=result,
|
||||
timestamp="20260903_120000",
|
||||
).path
|
||||
left_path = write_l6_left_from_right_calibration(
|
||||
source_left_urdf=LEFT_SOURCE,
|
||||
source_right_urdf=SOURCE,
|
||||
calibrated_right_urdf=right,
|
||||
destination_urdf=tmp_path / "left" / "l6_left_transferred.urdf",
|
||||
)
|
||||
original = {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(LEFT_SOURCE).getroot().findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
transferred_root = ET.parse(left_path).getroot()
|
||||
transferred = {
|
||||
str(joint.get("name")): joint
|
||||
for joint in transferred_root.findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
corrected_right = {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(right).getroot().findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
for left_name, joint in transferred.items():
|
||||
right_name = left_name.replace("lh_", "rh_", 1)
|
||||
assert joint.find("origin").get("xyz") == original[left_name].find(
|
||||
"origin"
|
||||
).get("xyz")
|
||||
assert joint.find("axis").attrib == original[left_name].find("axis").attrib
|
||||
assert joint.find("limit").get("lower") == corrected_right[
|
||||
right_name
|
||||
].find("limit").get("lower")
|
||||
assert joint.find("limit").get("upper") == corrected_right[
|
||||
right_name
|
||||
].find("limit").get("upper")
|
||||
if joint.find("mimic") is not None:
|
||||
assert joint.find("mimic").get("multiplier") == corrected_right[
|
||||
right_name
|
||||
].find("mimic").get("multiplier")
|
||||
for name, expected in THUMB_ZERO_OFFSETS.items():
|
||||
left_name = name.replace("rh_", "lh_", 1)
|
||||
source_rotation = Rotation.from_euler(
|
||||
"xyz",
|
||||
[float(value) for value in original[left_name].find("origin").get(
|
||||
"rpy"
|
||||
).split()],
|
||||
)
|
||||
corrected_rotation = Rotation.from_euler(
|
||||
"xyz",
|
||||
[float(value) for value in transferred[left_name].find("origin").get(
|
||||
"rpy"
|
||||
).split()],
|
||||
)
|
||||
axis = np.asarray([
|
||||
float(value)
|
||||
for value in original[left_name].find("axis").get("xyz").split()
|
||||
])
|
||||
axis /= np.linalg.norm(axis)
|
||||
applied = float((source_rotation.inv() * corrected_rotation).as_rotvec() @ axis)
|
||||
assert applied == pytest.approx(expected, abs=1.0e-6)
|
||||
left_equalities = {
|
||||
str(node.get("joint1")): node.get("polycoef")
|
||||
for node in transferred_root.findall("./mujoco/equality/joint")
|
||||
}
|
||||
right_equalities = {
|
||||
str(node.get("joint1")).replace("rh_", "lh_", 1): node.get("polycoef")
|
||||
for node in ET.parse(right).getroot().findall("./mujoco/equality/joint")
|
||||
}
|
||||
assert left_equalities == right_equalities
|
||||
|
||||
right_payload = build_l6_runtime_payload(
|
||||
serial_number="RIGHT_TEST",
|
||||
source_urdf=SOURCE,
|
||||
result=result,
|
||||
protected_inputs={
|
||||
"source_urdf_sha256": "0" * 64,
|
||||
"camera_extrinsics_sha256": "1" * 64,
|
||||
"calibration_config_sha256": "2" * 64,
|
||||
"tag_config_sha256": "3" * 64,
|
||||
},
|
||||
)
|
||||
left_payload = build_l6_left_transferred_runtime_payload(
|
||||
right_payload=right_payload,
|
||||
source_left_urdf=LEFT_SOURCE,
|
||||
transferred_left_urdf=left_path,
|
||||
serial_number="LEFT_TRANSFER_TEST",
|
||||
)
|
||||
validate_l6_runtime_payload(left_payload)
|
||||
mapper = CalibratedCommandMapper(left_payload, expected_side="left")
|
||||
assert mapper.profile_id == "L6/left/l6_left_transferred_8/v1"
|
||||
assert set(mapper.urdf_joint_names) == set(transferred)
|
||||
assert left_payload["joints"]["lh_pinky_dip"]["source_joint"] == (
|
||||
"lh_pinky_mcp_pitch"
|
||||
)
|
||||
assert left_payload["joints"]["lh_index_dip"][
|
||||
"transferred_from_joint"
|
||||
] == "lh_pinky_dip"
|
||||
assert left_payload["quality"]["transfer_provenance"][
|
||||
"left_hand_measured"
|
||||
] is False
|
||||
|
||||
|
||||
def test_l6_schema_v6_bridge_uses_feedback_and_rh_joint_names() -> None:
|
||||
result = fit_l6_session(SOURCE, accepted_records_by_joint(_synthetic_records()))
|
||||
hashes = {
|
||||
|
||||
@@ -14,6 +14,7 @@ from linkerhand_calibration.extrinsics import matrix_payload, transform_matrix
|
||||
from linkerhand_calibration.models.g20.zero_solver import UrdfKinematicModel
|
||||
from linkerhand_calibration.models.l6.node import MotionStep
|
||||
from linkerhand_calibration.models.o6.artifacts import (
|
||||
build_o6_left_transferred_runtime_payload,
|
||||
build_o6_runtime_payload,
|
||||
validate_o6_runtime_payload,
|
||||
)
|
||||
@@ -38,14 +39,26 @@ from linkerhand_calibration.models.o6.profile import (
|
||||
build_typed_profile,
|
||||
)
|
||||
from linkerhand_calibration.models.o6.runner import render_o6_progress_zh
|
||||
from linkerhand_calibration.models.o6.urdf import write_o6_corrected_urdf
|
||||
from linkerhand_calibration.models.o6.urdf import (
|
||||
write_o6_corrected_urdf,
|
||||
write_o6_left_from_right_calibration,
|
||||
)
|
||||
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
SOURCE = PACKAGE / "urdf/o6_right/linkerhand_o6_right.urdf"
|
||||
LEFT_SOURCE = PACKAGE / "urdf/o6_left/linkerhand_o6_left.urdf"
|
||||
KINEMATIC_MODEL = UrdfKinematicModel(SOURCE)
|
||||
|
||||
|
||||
def _joint_elements(path: Path) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(path).getroot().findall("joint")
|
||||
if joint.get("type") == "revolute"
|
||||
}
|
||||
|
||||
|
||||
def test_o6_profile_declares_reviewed_six_channel_contract() -> None:
|
||||
profile = build_typed_profile()
|
||||
validate_profile(profile)
|
||||
@@ -475,3 +488,80 @@ def test_o6_uses_geometric_yaw_and_cad_endpoint_pitch() -> None:
|
||||
assert result.thumb_zero_result.direct_offsets_rad[
|
||||
"rh_thumb_cmc_pitch"
|
||||
] == pytest.approx(-0.02, abs=1e-6)
|
||||
|
||||
|
||||
def test_o6_right_corrections_are_mirrored_without_replacing_left_geometry(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
result = fit_o6_session(
|
||||
SOURCE, _geometric_records(), require_thumb_axis_zero=True
|
||||
)
|
||||
right = write_o6_corrected_urdf(
|
||||
source_urdf=SOURCE,
|
||||
output_directory=tmp_path / "right",
|
||||
serial_number="RIGHT_TEST",
|
||||
result=result,
|
||||
).path
|
||||
destination = write_o6_left_from_right_calibration(
|
||||
source_left_urdf=LEFT_SOURCE,
|
||||
source_right_urdf=SOURCE,
|
||||
calibrated_right_urdf=right,
|
||||
destination_urdf=tmp_path / "left" / "o6_left_transferred.urdf",
|
||||
)
|
||||
original = _joint_elements(LEFT_SOURCE)
|
||||
transferred = _joint_elements(destination)
|
||||
corrected_right = _joint_elements(right)
|
||||
for left_name, joint in transferred.items():
|
||||
right_name = left_name.replace("lh_", "rh_", 1)
|
||||
assert joint.find("origin").get("xyz") == original[left_name].find(
|
||||
"origin"
|
||||
).get("xyz")
|
||||
assert joint.find("axis").attrib == original[left_name].find("axis").attrib
|
||||
assert joint.find("limit").get("upper") == corrected_right[
|
||||
right_name
|
||||
].find("limit").get("upper")
|
||||
if joint.find("mimic") is not None:
|
||||
assert joint.find("mimic").get("multiplier") == corrected_right[
|
||||
right_name
|
||||
].find("mimic").get("multiplier")
|
||||
assert transferred["lh_thumb_cmc_pitch"].find("origin").get("rpy") == (
|
||||
original["lh_thumb_cmc_pitch"].find("origin").get("rpy")
|
||||
)
|
||||
left_yaw = Rotation.from_euler(
|
||||
"xyz",
|
||||
[float(value) for value in transferred["lh_thumb_cmc_yaw"].find(
|
||||
"origin"
|
||||
).get("rpy").split()],
|
||||
)
|
||||
assert left_yaw.as_rotvec()[2] == pytest.approx(0.03, abs=1e-6)
|
||||
|
||||
right_payload = build_o6_runtime_payload(
|
||||
serial_number="RIGHT_TEST",
|
||||
source_urdf=SOURCE,
|
||||
result=result,
|
||||
protected_inputs={
|
||||
"source_urdf_sha256": "0" * 64,
|
||||
"camera_extrinsics_sha256": "1" * 64,
|
||||
"calibration_config_sha256": "2" * 64,
|
||||
"tag_config_sha256": "3" * 64,
|
||||
},
|
||||
)
|
||||
left_payload = build_o6_left_transferred_runtime_payload(
|
||||
right_payload=right_payload,
|
||||
source_left_urdf=LEFT_SOURCE,
|
||||
transferred_left_urdf=destination,
|
||||
serial_number="LEFT_TRANSFER_TEST",
|
||||
)
|
||||
validate_o6_runtime_payload(left_payload)
|
||||
mapper = CalibratedCommandMapper(left_payload, expected_side="left")
|
||||
assert mapper.profile_id == "O6/left/o6_left_transferred_8/v1"
|
||||
assert set(mapper.urdf_joint_names) == set(transferred)
|
||||
mapped = dict(zip(
|
||||
mapper.urdf_joint_names,
|
||||
mapper.map_positions([0, 255, 255, 255, 255, 255]),
|
||||
))
|
||||
assert mapped["lh_thumb_cmc_pitch"] == pytest.approx(0.5, abs=2.0e-4)
|
||||
assert mapped["lh_thumb_ip"] == pytest.approx(0.93, abs=2.0e-4)
|
||||
assert left_payload["quality"]["transfer_provenance"][
|
||||
"left_hand_measured"
|
||||
] is False
|
||||
|
||||
@@ -0,0 +1,956 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot
|
||||
name="linkerhand_l6v3.1_left">
|
||||
<!-- 手掌基座，作为整只手的根链接 -->
|
||||
<link
|
||||
name="lh_hand_base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00199 -0.00091 0.058381"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.4185" />
|
||||
<inertia
|
||||
ixx="0.0004998"
|
||||
ixy="0.000012853"
|
||||
ixz="-0.000008317"
|
||||
iyy="0.0003567"
|
||||
iyz="0.000010354"
|
||||
izz="0.0002301" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/hand_base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/hand_base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_thumb_metacarpals_base1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0018663 -0.0066406 0.001533"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.01" />
|
||||
<inertia
|
||||
ixx="3.3724E-07"
|
||||
ixy="1.11E-07"
|
||||
ixz="-3.6039E-08"
|
||||
iyy="2.9457E-07"
|
||||
iyz="5.7181E-08"
|
||||
izz="4.903E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals_base1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals_base1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_thumb_cmc_roll"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0078133 -0.029898 0.026692"
|
||||
rpy="-0.034907 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_thumb_metacarpals_base1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-0.087266"
|
||||
upper="1.256637"
|
||||
effort="0.588"
|
||||
velocity="5.95" />
|
||||
<dynamics damping="0.02" friction="0.05416667"/>
|
||||
</joint>
|
||||
<link
|
||||
name="lh_thumb_metacarpals">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00769270907060086 0.0859662184722988 0.0235283086675235"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.065" />
|
||||
<inertia
|
||||
ixx="8.407E-06"
|
||||
ixy="-1.1338E-09"
|
||||
ixz="1.1136E-08"
|
||||
iyy="8.3899E-06"
|
||||
iyz="3.1171E-07"
|
||||
izz="2.0091E-06" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_thumb_cmc_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0057298 -0.01332 0.0027456"
|
||||
rpy="1.1083 0 0.40143" />
|
||||
<parent
|
||||
link="lh_thumb_metacarpals_base1" />
|
||||
<child
|
||||
link="lh_thumb_metacarpals" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="0.837758"
|
||||
effort="1.008"
|
||||
velocity="3.47" />
|
||||
<dynamics damping="0.02" friction="0.0375"/>
|
||||
</joint>
|
||||
<link
|
||||
name="lh_thumb_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00769270907081708 0.0798662184721594 -0.0591716913325555"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.018" />
|
||||
<inertia
|
||||
ixx="1.5546E-06"
|
||||
ixy="2.4423E-09"
|
||||
ixz="2.8147E-09"
|
||||
iyy="1.7108E-06"
|
||||
iyz="-7.2756E-08"
|
||||
izz="9.1709E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_distal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_thumb_tip">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1e-06" />
|
||||
<inertia
|
||||
ixx="1e-09"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="1e-09"
|
||||
iyz="0"
|
||||
izz="1e-09" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_thumb_tip_fixed"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0 0.0035 0.026"
|
||||
rpy="0 0 1.5708" />
|
||||
<parent
|
||||
link="lh_thumb_distal" />
|
||||
<child
|
||||
link="lh_thumb_tip" />
|
||||
</joint>
|
||||
<joint
|
||||
name="lh_thumb_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0.0061 0.0827"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_thumb_metacarpals" />
|
||||
<child
|
||||
link="lh_thumb_distal" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="0.963287"
|
||||
effort="0"
|
||||
velocity="1.7" />
|
||||
<mimic
|
||||
joint="lh_thumb_cmc_pitch"
|
||||
multiplier="1.226495"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_index_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001776 -0.00011 0.012556"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.008553" />
|
||||
<inertia
|
||||
ixx="0.000000797543"
|
||||
ixy="-0.000000001275"
|
||||
ixz="0.000000138601"
|
||||
iyy="0.000000800499"
|
||||
iyz="0.000000000939"
|
||||
izz="0.000000357001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_proximal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_index_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.00066821 -0.028967 0.11722"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_index_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.134464"
|
||||
effort="0.574"
|
||||
velocity="6.10" />
|
||||
<dynamics damping="0.01" friction="0.04"/>
|
||||
</joint>
|
||||
<link
|
||||
name="lh_index_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0134 0.002262 0.016992"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.018408" />
|
||||
<inertia
|
||||
ixx="0.000002488712"
|
||||
ixy="0.000000008249"
|
||||
ixz="-0.000001523059"
|
||||
iyy="0.000003723754"
|
||||
iyz="0.000000014019"
|
||||
izz="0.000001813582" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_distal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_index_tip">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1e-06" />
|
||||
<inertia
|
||||
ixx="1e-09"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="1e-09"
|
||||
iyz="0"
|
||||
izz="1e-09" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_index_tip_fixed"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.0341 0 0.034"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_index_distal" />
|
||||
<child
|
||||
link="lh_index_tip" />
|
||||
</joint>
|
||||
<joint
|
||||
name="lh_index_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00093749 0 0.028002"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_index_proximal" />
|
||||
<child
|
||||
link="lh_index_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.277039"
|
||||
effort="0"
|
||||
velocity="3.1" />
|
||||
<mimic
|
||||
joint="lh_index_mcp_pitch"
|
||||
multiplier="1.125676"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_middle_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001776 -0.00011 0.012556"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.008553" />
|
||||
<inertia
|
||||
ixx="0.000000797543"
|
||||
ixy="-0.000000001275"
|
||||
ixz="0.000000138601"
|
||||
iyy="0.000000800499"
|
||||
iyz="0.000000000939"
|
||||
izz="0.000000357001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_proximal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_middle_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.00066821 -0.0089013 0.12322"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_middle_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.134464"
|
||||
effort="0.574"
|
||||
velocity="6.10" />
|
||||
<dynamics damping="0.01" friction="0.02708333"/>
|
||||
</joint>
|
||||
<link
|
||||
name="lh_middle_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0134 0.002262 0.016992"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.018408" />
|
||||
<inertia
|
||||
ixx="0.000002488712"
|
||||
ixy="0.000000008249"
|
||||
ixz="-0.000001523059"
|
||||
iyy="0.000003723754"
|
||||
iyz="0.000000014019"
|
||||
izz="0.000001813582" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_distal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_middle_tip">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1e-06" />
|
||||
<inertia
|
||||
ixx="1e-09"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="1e-09"
|
||||
iyz="0"
|
||||
izz="1e-09" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_middle_tip_fixed"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.0341 0 0.034"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_middle_distal" />
|
||||
<child
|
||||
link="lh_middle_tip" />
|
||||
</joint>
|
||||
<joint
|
||||
name="lh_middle_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00093749 0 0.028002"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_middle_proximal" />
|
||||
<child
|
||||
link="lh_middle_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.277039"
|
||||
effort="0"
|
||||
velocity="3.1" />
|
||||
<mimic
|
||||
joint="lh_middle_mcp_pitch"
|
||||
multiplier="1.125676"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_ring_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001776 -0.00011 0.012556"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.008553" />
|
||||
<inertia
|
||||
ixx="0.000000797543"
|
||||
ixy="-0.000000001275"
|
||||
ixz="0.000000138601"
|
||||
iyy="0.000000800499"
|
||||
iyz="0.000000000939"
|
||||
izz="0.000000357001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_proximal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_ring_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.00067368 0.010717 0.11822"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_ring_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.134464"
|
||||
effort="0.574"
|
||||
velocity="6.10" />
|
||||
<dynamics damping="0.01" friction="0.02291667"/>
|
||||
</joint>
|
||||
<link
|
||||
name="lh_ring_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0134 0.002262 0.016992"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.018408" />
|
||||
<inertia
|
||||
ixx="0.000002488712"
|
||||
ixy="0.000000008249"
|
||||
ixz="-0.000001523059"
|
||||
iyy="0.000003723754"
|
||||
iyz="0.000000014019"
|
||||
izz="0.000001813582" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_distal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_ring_tip">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1e-06" />
|
||||
<inertia
|
||||
ixx="1e-09"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="1e-09"
|
||||
iyz="0"
|
||||
izz="1e-09" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_ring_tip_fixed"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.0341 0 0.034"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_ring_distal" />
|
||||
<child
|
||||
link="lh_ring_tip" />
|
||||
</joint>
|
||||
<joint
|
||||
name="lh_ring_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00093749 0 0.028002"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_ring_proximal" />
|
||||
<child
|
||||
link="lh_ring_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.277039"
|
||||
effort="0"
|
||||
velocity="3.1" />
|
||||
<mimic
|
||||
joint="lh_ring_mcp_pitch"
|
||||
multiplier="1.125676"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_pinky_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001776 -0.00011 0.012556"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.008553" />
|
||||
<inertia
|
||||
ixx="0.000000797543"
|
||||
ixy="-0.000000001275"
|
||||
ixz="0.000000138601"
|
||||
iyy="0.000000800499"
|
||||
iyz="0.000000000939"
|
||||
izz="0.000000357001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_proximal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_pinky_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.00067852 0.029814 0.11424"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_pinky_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.134464"
|
||||
effort="0.574"
|
||||
velocity="6.10" />
|
||||
<dynamics damping="0.01" friction="0.01"/>
|
||||
</joint>
|
||||
<link
|
||||
name="lh_pinky_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0134 0.002262 0.016992"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.018408" />
|
||||
<inertia
|
||||
ixx="0.000002488712"
|
||||
ixy="0.000000008249"
|
||||
ixz="-0.000001523059"
|
||||
iyy="0.000003723754"
|
||||
iyz="0.000000014019"
|
||||
izz="0.000001813582" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_distal.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.796078431372549 0.823529411764706 0.937254901960784 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_pinky_tip">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1e-06" />
|
||||
<inertia
|
||||
ixx="1e-09"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="1e-09"
|
||||
iyz="0"
|
||||
izz="1e-09" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_pinky_tip_fixed"
|
||||
type="fixed">
|
||||
<origin
|
||||
xyz="0.0341 0 0.034"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_pinky_distal" />
|
||||
<child
|
||||
link="lh_pinky_tip" />
|
||||
</joint>
|
||||
<joint
|
||||
name="lh_pinky_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00093749 0 0.028002"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_pinky_proximal" />
|
||||
<child
|
||||
link="lh_pinky_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.277039"
|
||||
effort="0"
|
||||
velocity="3.1" />
|
||||
<mimic
|
||||
joint="lh_pinky_mcp_pitch"
|
||||
multiplier="1.125676"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<transmission name="trans_thumb_cmc_roll">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lh_thumb_cmc_roll">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_thumb_cmc_roll">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>0.0084</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_index_mcp_pitch">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lh_index_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_index_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>0.0082</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_middle_mcp_pitch">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lh_middle_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_middle_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>0.0082</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_ring_mcp_pitch">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lh_ring_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_ring_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>0.0082</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_pinky_mcp_pitch">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lh_pinky_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_pinky_mcp_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>0.0082</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_thumb_cmc_pitch">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lh_thumb_cmc_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="motor_thumb_cmc_pitch">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>0.0144</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<mujoco>
|
||||
<compiler meshdir="meshes/" balanceinertia="true" discardvisual="false"/>
|
||||
<default>
|
||||
<joint armature="3.4707e-08" damping="5.0" frictionloss="0.5"/>
|
||||
</default>
|
||||
<equality>
|
||||
<!-- 四指DIP关节的mimic约束 - 统一使用1.125676 -->
|
||||
<!-- polycoef格式: c0 c1 c2 c3 c4 c5，关系: q1 = c0 + c1*q2 + c2*q2^2 + ... -->
|
||||
<!-- 对于 mimic: q_dip = 0 + 1.125676*q_mcp + 0*q_mcp^2 + ... -->
|
||||
<joint name="lh_couple_index" joint1="lh_index_dip" joint2="lh_index_mcp_pitch" polycoef="0 1.125676 0 0 0 0"/>
|
||||
<joint name="lh_couple_middle" joint1="lh_middle_dip" joint2="lh_middle_mcp_pitch" polycoef="0 1.125676 0 0 0 0"/>
|
||||
<joint name="lh_couple_ring" joint1="lh_ring_dip" joint2="lh_ring_mcp_pitch" polycoef="0 1.125676 0 0 0 0"/>
|
||||
<joint name="lh_couple_pinky" joint1="lh_pinky_dip" joint2="lh_pinky_mcp_pitch" polycoef="0 1.125676 0 0 0 0"/>
|
||||
<!-- 大拇指DIP关节的mimic约束 - 使用1.226495 -->
|
||||
<joint name="lh_couple_thumb" joint1="lh_thumb_dip" joint2="lh_thumb_cmc_pitch" polycoef="0 1.226495 0 0 0 0"/>
|
||||
</equality>
|
||||
</mujoco>
|
||||
<gazebo reference="lh_hand_base_link">
|
||||
<mu1>1.5</mu1>
|
||||
<mu2>1.5</mu2>
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="lh_thumb_distal">
|
||||
<mu1>1.5</mu1>
|
||||
<mu2>1.5</mu2>
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="lh_index_distal">
|
||||
<mu1>1.1</mu1>
|
||||
<mu2>1.1</mu2>
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="lh_middle_distal">
|
||||
<mu1>1.1</mu1>
|
||||
<mu2>1.1</mu2>
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="lh_ring_distal">
|
||||
<mu1>1.1</mu1>
|
||||
<mu2>1.1</mu2>
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="lh_pinky_distal">
|
||||
<mu1>1.1</mu1>
|
||||
<mu2>1.1</mu2>
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
</robot>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,666 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="linkerhand_o6_left">
|
||||
<material name="silver">
|
||||
<color rgba="0.5 0.5 0.52 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="BlackAccent">
|
||||
<color rgba="0.15 0.15 0.15 1.0"/>
|
||||
</material>
|
||||
|
||||
<link
|
||||
name="lh_hand_base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00157788568070457 -0.00034876725935859 0.0538452219801211"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0672935238166485" />
|
||||
<inertia
|
||||
ixx="5.22525212160954E-05"
|
||||
ixy="3.06955729584715E-07"
|
||||
ixz="-3.90341166950521E-07"
|
||||
iyy="3.33522349207724E-05"
|
||||
iyz="-7.76680063574627E-07"
|
||||
izz="2.28469461105385E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/hand_base_link.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/hand_base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="lh_thumb_metacarpals_base2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00162800681276937 -0.0027668017246415 -0.00184234438641236"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00138588296544333" />
|
||||
<inertia
|
||||
ixx="5.26292160685909E-08"
|
||||
ixy="-3.92888681640568E-09"
|
||||
ixz="-2.43767371295222E-09"
|
||||
iyy="4.82620491570326E-08"
|
||||
iyz="3.71869784206147E-09"
|
||||
izz="2.88550286112821E-08" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals_base2.STL" />
|
||||
</geometry>
|
||||
<material name="BlackAccent"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals_base2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_thumb_cmc_yaw"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.011508 -0.022975 0.032794"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_thumb_metacarpals_base2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.3"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_thumb_metacarpals">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="4.24816204999817E-05 1.74183907812875E-05 0.0228341640631254"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.0123381586619931" />
|
||||
<inertia
|
||||
ixx="1.10292537368909E-06"
|
||||
ixy="-5.99642232777877E-10"
|
||||
ixz="-1.14233573940545E-07"
|
||||
iyy="1.76757772551884E-06"
|
||||
iyz="-4.03963570823777E-10"
|
||||
izz="9.36347792411098E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_metacarpals.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_thumb_cmc_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0061649 -0.010678 -0.004891"
|
||||
rpy="0 -1.1529 2.0944" />
|
||||
<parent
|
||||
link="lh_thumb_metacarpals_base2" />
|
||||
<child
|
||||
link="lh_thumb_metacarpals" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="0.58"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_thumb_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00413725702921841 -5.90704520436747E-08 0.023011401589736"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00763332957284057" />
|
||||
<inertia
|
||||
ixx="1.3057487813702E-06"
|
||||
ixy="-1.19781508112714E-11"
|
||||
ixz="-7.19551697268541E-08"
|
||||
iyy="1.2248908905341E-06"
|
||||
iyz="4.55347974652679E-12"
|
||||
izz="2.77271910592615E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_distal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/thumb_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_thumb_ip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0037776 0 0.045368"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_thumb_metacarpals" />
|
||||
<child
|
||||
link="lh_thumb_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.08"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
<mimic
|
||||
joint="lh_thumb_cmc_pitch"
|
||||
multiplier="2.29"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_index_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00208656556336452 2.53838713251686E-06 0.0172734941980264"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00253174651934627" />
|
||||
<inertia
|
||||
ixx="2.65988714157799E-07"
|
||||
ixy="-1.67174578825824E-11"
|
||||
ixz="-4.31047725546034E-08"
|
||||
iyy="2.72914069166233E-07"
|
||||
iyz="1.8756709532167E-10"
|
||||
izz="8.56017305636748E-08" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_proximal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_index_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0024758 -0.02419 0.098779"
|
||||
rpy="0.05236 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_index_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.60"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_index_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00658986616416549 5.13855642059191E-06 0.0194393600039178"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00376951085886838" />
|
||||
<inertia
|
||||
ixx="3.22347106085043E-07"
|
||||
ixy="1.3801788663582E-11"
|
||||
ixz="1.14352857317951E-07"
|
||||
iyy="3.65328133721509E-07"
|
||||
iyz="-2.11505011705634E-10"
|
||||
izz="1.24287996386442E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_distal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/index_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_index_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.0052516 0 0.036625"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_index_proximal" />
|
||||
<child
|
||||
link="lh_index_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.43"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
<mimic
|
||||
joint="lh_index_mcp_pitch"
|
||||
multiplier="0.89"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_middle_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00208660654138235 2.56918180119303E-06 0.0172735011723351"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00253177375206983" />
|
||||
<inertia
|
||||
ixx="2.65989021304483E-07"
|
||||
ixy="-1.69816046635662E-11"
|
||||
ixz="-4.31046683215273E-08"
|
||||
iyy="2.72914402745606E-07"
|
||||
iyz="1.87484652324395E-10"
|
||||
izz="8.5602213431865E-08" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_proximal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_middle_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.00052576 -0.00634 0.1027"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_middle_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.60"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_middle_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00658986516319194 5.10862924271716E-06 0.0194393315699782"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00376952789210256" />
|
||||
<inertia
|
||||
ixx="3.22348591068053E-07"
|
||||
ixy="1.37122394856588E-11"
|
||||
ixz="1.1435291391668E-07"
|
||||
iyy="3.65329535087755E-07"
|
||||
iyz="-2.10453810291991E-10"
|
||||
izz="1.24288425862546E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_distal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/middle_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_middle_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.0052516 0 0.036625"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_middle_proximal" />
|
||||
<child
|
||||
link="lh_middle_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.43"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
<mimic
|
||||
joint="lh_middle_mcp_pitch"
|
||||
multiplier="0.89"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_ring_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00208667931345812 2.63000419462034E-06 0.0172734926448377"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00253182223645066" />
|
||||
<inertia
|
||||
ixx="2.65989756076584E-07"
|
||||
ixy="-1.74912952703656E-11"
|
||||
ixz="-4.31043325594253E-08"
|
||||
iyy="2.7291510093393E-07"
|
||||
iyz="1.87158326740895E-10"
|
||||
izz="8.56031522765909E-08" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_proximal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_ring_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0010258 0.011135 0.098767"
|
||||
rpy="-0.05236 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_ring_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.60"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_ring_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00658986648895491 5.05883528165163E-06 0.019439397215532"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00376949588316609" />
|
||||
<inertia
|
||||
ixx="3.2234524157181E-07"
|
||||
ixy="1.35766976056011E-11"
|
||||
ixz="1.14353011012842E-07"
|
||||
iyy="3.65326570102147E-07"
|
||||
iyz="-2.08672149138738E-10"
|
||||
izz="1.24287770218135E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_distal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/ring_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_ring_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.0052516 0 0.036625"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_ring_proximal" />
|
||||
<child
|
||||
link="lh_ring_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.43"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
<mimic
|
||||
joint="lh_ring_mcp_pitch"
|
||||
multiplier="0.89"
|
||||
offset="0" />
|
||||
</joint>
|
||||
<link
|
||||
name="lh_pinky_proximal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00208673599799501 2.68062251165824E-06 0.0172734848131236"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00253185993162408" />
|
||||
<inertia
|
||||
ixx="2.6599033687183E-07"
|
||||
ixy="-1.79030790016555E-11"
|
||||
ixz="-4.31040829565781E-08"
|
||||
iyy="2.72915617381577E-07"
|
||||
iyz="1.86906787420486E-10"
|
||||
izz="8.56039175034871E-08" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_proximal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_proximal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_pinky_mcp_pitch"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.0024758 0.028372 0.092741"
|
||||
rpy="-0.087266 0 0" />
|
||||
<parent
|
||||
link="lh_hand_base_link" />
|
||||
<child
|
||||
link="lh_pinky_proximal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.60"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
|
||||
</joint>
|
||||
<link
|
||||
name="lh_pinky_distal">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00658987588903027 5.03087534178825E-06 0.0194394491065091"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.00376948071276134" />
|
||||
<inertia
|
||||
ixx="3.22343583730488E-07"
|
||||
ixy="1.35768173270357E-11"
|
||||
ixz="1.14353289593012E-07"
|
||||
iyy="3.65325180884183E-07"
|
||||
iyz="-2.0744198062439E-10"
|
||||
izz="1.24287567374229E-07" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_distal.STL" />
|
||||
</geometry>
|
||||
<material name="silver"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/pinky_distal.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="lh_pinky_dip"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.0052516 0 0.036625"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="lh_pinky_proximal" />
|
||||
<child
|
||||
link="lh_pinky_distal" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.43"
|
||||
effort="100"
|
||||
velocity="1" />
|
||||
<mimic
|
||||
joint="lh_pinky_mcp_pitch"
|
||||
multiplier="0.89"
|
||||
offset="0" />
|
||||
</joint>
|
||||
</robot>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user