realrobotshk
/
ros_robot_visualiser
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed parenting of sample.urdf

master
Jake 2025-11-05 15:26:02 +08:00
parent f7eef1b53f
commit f77ff74f94
2 changed files with 361 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
script.js
View File

@ -80,6 +80,65 @@ function createRotationArc(axis, lower, upper, radius = 0.1, segments = 32) {
return new THREE.Line(geometry, material); return new THREE.Line(geometry, material);
} }
function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) {
const shape = new THREE.Shape();
shape.moveTo(0, 0);
for (let i = 0; i <= segments; i++) {
const angle = lower + (upper - lower) * (i / segments);
shape.lineTo(Math.cos(angle) * radius, Math.sin(angle) * radius);
}
shape.lineTo(0, 0); // close the shape
const geometry = new THREE.ShapeGeometry(shape);
const material = new THREE.MeshBasicMaterial({
color: 0x888888, // light gray
transparent: true,
opacity: 0.3, // translucent
side: THREE.DoubleSide, // show both sides of the fill
depthTest: false, // ✅ always on top
depthWrite: false
});
const mesh = new THREE.Mesh(geometry, material);
// Orient the sector to match the axis
const basis = new THREE.Matrix4();
const up = new THREE.Vector3(0, 0, 1);
const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
basis.makeRotationFromQuaternion(quaternion);
mesh.applyMatrix4(basis);
return mesh;
}
function createAngleIndicator(axis, angle, radius = 0.1) {
const dir = new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).multiplyScalar(radius);
const geometry = new THREE.BufferGeometry().setFromPoints([
new THREE.Vector3(0, 0, 0),
dir,
]);
const material = new THREE.LineBasicMaterial({
color: 0xffffff, // white
linewidth: 2, // note: linewidth only works in some renderers
depthTest: false, // ✅ always on top
depthWrite: false
});
const line = new THREE.Line(geometry, material);
// Rotate into axis plane
const up = new THREE.Vector3(0, 0, 1);
const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
line.applyQuaternion(quaternion);
return line;
}
// Load URDF // Load URDF
@ -136,7 +195,20 @@ loader.load('./urdf/sample.urdf', robot => {
// ✅ Apply rotation // ✅ Apply rotation
draggedJoint.rotateOnAxis(worldAxis, actualDelta); draggedJoint.rotateOnAxis(worldAxis, actualDelta);
jointAngles[jointName] = clampedAngle; jointAngles[jointName] = clampedAngle;
}
// Remove old indicator
if (draggedJoint.userData.gizmo?.indicator) {
draggedJoint.parent.remove(draggedJoint.userData.gizmo.indicator);
}
// Add updated indicator
const newIndicator = createAngleIndicator(worldAxis, jointAngles[jointName]);
newIndicator.position.copy(draggedJoint.position);
draggedJoint.parent.add(newIndicator);
draggedJoint.userData.gizmo.indicator = newIndicator;
}
@ -206,14 +278,22 @@ loader.load('./urdf/sample.urdf', robot => {
} }
worldAxis = urdfAxis.normalize(); worldAxis = urdfAxis.normalize();
const limits = draggedJoint?.limit; const limits = robot.joints[jointName]?.limit;
console.log(limits);
const lower = limits?.lower ?? -Math.PI; const lower = limits?.lower ?? -Math.PI;
const upper = limits?.upper ?? Math.PI; const upper = limits?.upper ?? Math.PI;
arc = createRotationArc(worldAxis, lower, upper);
draggedJoint.parent.add(arc); const sector = createRotationSector(worldAxis, lower, upper);
arc.position.copy(draggedJoint.position); const indicator = createAngleIndicator(worldAxis, jointAngles[jointName] ?? 0);
jointAngles[jointName] ??= 0;
sector.position.copy(draggedJoint.position);
indicator.position.copy(draggedJoint.position);
draggedJoint.parent.add(sector);
draggedJoint.parent.add(indicator);
// Store for cleanup
draggedJoint.userData.gizmo = { sector, indicator };
@ -228,7 +308,12 @@ loader.load('./urdf/sample.urdf', robot => {
canvas.addEventListener('pointerup', () => { canvas.addEventListener('pointerup', () => {
isDragging = false; isDragging = false;
controls.enabled = true; controls.enabled = true;
draggedJoint.parent.remove(arc); const gizmo = draggedJoint?.userData.gizmo;
if (gizmo) {
draggedJoint.parent.remove(gizmo.sector);
draggedJoint.parent.remove(gizmo.indicator);
}
draggedJoint = null; draggedJoint = null;
worldAxis = null; worldAxis = null;

375
urdf/sample.urdf
View File

@ -4,6 +4,7 @@
<parent link="base_footprint" /> <parent link="base_footprint" />
<child link="base_link" /> <child link="base_link" />
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="base_link"> <link name="base_link">
<visual> <visual>
@ -28,11 +29,12 @@
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_spine_1" type="revolute"> <joint name="base_link_to_spine_1" type="revolute">
<parent link="base_link" /> <parent link="base_link"/>
<child link="spine_1" /> <child link="spine_1"/>
<origin xyz="0 0 0.033737961440044734" rpy="0 0 0" /> <origin xyz="0 0 0.0337" rpy="0 0 0"/>
<axis xyz="-0.9996738127289346 -0.011364625593810783 0.02287167307189009"/> <axis xyz="-0.9997 -0.0114 0.0229"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="spine_1"> <link name="spine_1">
<visual> <visual>
<origin xyz="0 0 0.05" rpy="0 0 0" /> <origin xyz="0 0 0.05" rpy="0 0 0" />
@ -55,12 +57,13 @@
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_upper_arm_right" type="revolute"> <joint name="spine_1_to_upper_arm_right" type="revolute">
<parent link="base_link" /> <parent link="spine_1"/>
<child link="upper_arm_right" /> <child link="upper_arm_right"/>
<origin xyz="0.062354968072652533 0 0.13096467297466818" rpy="-3.141592653589793 -0.6586873842445309 -1.548624773543236e-16" /> <origin xyz="0.0624 0 0.0973" rpy="-3.1416 -0.6587 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="-0.9997 -0.0114 0.0229"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="upper_arm_right"> <link name="upper_arm_right">
<visual> <visual>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 0.03" rpy="0 0 0" />
@ -83,12 +86,13 @@
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_lower_arm_right" type="revolute"> <joint name="upper_arm_right_to_lower_arm_right" type="revolute">
<parent link="base_link" /> <parent link="upper_arm_right"/>
<child link="lower_arm_right" /> <child link="lower_arm_right"/>
<origin xyz="0.10009022777221713 0 0.08243944985877427" rpy="-3.141592653589793 -0.6586873842445309 -2.496506045944997e-16" /> <origin xyz="0.0 0 .065" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="-0.9997 -0.0114 0.0229"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="lower_arm_right"> <link name="lower_arm_right">
<visual> <visual>
<origin xyz="0 0 0.025" rpy="0 0 0" /> <origin xyz="0 0 0.025" rpy="0 0 0" />
@ -111,12 +115,13 @@
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_hand_right" type="revolute"> <joint name="lower_arm_right_to_hand_right" type="revolute">
<parent link="base_link" /> <parent link="lower_arm_right"/>
<child link="hand_right" /> <child link="hand_right"/>
<origin xyz="0.13276109632093022 0 0.03940068352661779" rpy="-3.141592653589793 -0.6586873842445309 -2.496506045944997e-16" /> <origin xyz="0 0 0.055" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="0 1 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="hand_right"> <link name="hand_right">
<visual> <visual>
<origin xyz="0 0 0.015" rpy="0 0 0" /> <origin xyz="0 0 0.015" rpy="0 0 0" />
@ -141,258 +146,311 @@
</link> </link>
<!-- Left Upper Arm --> <!-- Left Upper Arm -->
<joint name="base_link_to_upper_arm_left" type="revolute"> <joint name="spine_1_to_upper_arm_left" type="revolute">
<parent link="base_link" /> <parent link="spine_1"/>
<child link="upper_arm_left" /> <child link="upper_arm_left"/>
<origin xyz="-0.062354968072652533 0 0.13096467297466818" rpy="-3.141592653589793 0.6586873842445309 -1.548624773543236e-16" /> <origin xyz="-0.0624 0 0.0973" rpy="3.1416 0.6587 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="-1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="upper_arm_left"> <link name="upper_arm_left">
<visual> <visual>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 0.03" rpy="0 0 0"/>
<geometry><box size="0.03 0.03 0.06" /></geometry> <geometry>
<box size="0.03 0.03 0.06"/>
</geometry>
<material name="upper_arm_left-material"> <material name="upper_arm_left-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 0.03" rpy="0 0 0"/>
<geometry><box size="0.03 0.03 0.06" /></geometry> <geometry>
<box size="0.03 0.03 0.06"/>
</geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 0.03" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
</inertial> </inertial>
</link> </link>
<!-- Left Lower Arm --> <!-- Left Lower Arm -->
<joint name="base_link_to_lower_arm_left" type="revolute"> <joint name="upper_arm_left_to_lower_arm_left" type="revolute">
<parent link="base_link" /> <parent link="upper_arm_left"/>
<child link="lower_arm_left" /> <child link="lower_arm_left"/>
<origin xyz="-0.10009022777221713 0 0.08243944985877427" rpy="-3.141592653589793 0.6586873842445309 -2.496506045944997e-16" /> <origin xyz="0.0 0 0.065" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="-1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="lower_arm_left"> <link name="lower_arm_left">
<visual> <visual>
<origin xyz="0 0 0.025" rpy="0 0 0" /> <origin xyz="0 0 0.025" rpy="0 0 0"/>
<geometry><box size="0.03 0.03 0.05" /></geometry> <geometry>
<box size="0.03 0.03 0.05"/>
</geometry>
<material name="lower_arm_left-material"> <material name="lower_arm_left-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.025" rpy="0 0 0" /> <origin xyz="0 0 0.025" rpy="0 0 0"/>
<geometry><box size="0.03 0.03 0.05" /></geometry> <geometry>
<box size="0.03 0.03 0.05"/>
</geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.025" rpy="0 0 0" /> <origin xyz="0 0 0.025" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
</inertial> </inertial>
</link> </link>
<!-- Left Hand --> <!-- Left Hand -->
<joint name="base_link_to_hand_left" type="revolute"> <joint name="lower_arm_left_to_hand_left" type="revolute">
<parent link="base_link" /> <parent link="lower_arm_left"/>
<child link="hand_left" /> <child link="hand_left"/>
<origin xyz="-0.13276109632093022 0 0.03940068352661779" rpy="-3.141592653589793 0.6586873842445309 -2.496506045944997e-16" /> <origin xyz="0 0 0.055" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="0 1 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="hand_left"> <link name="hand_left">
<visual> <visual>
<origin xyz="0 0 0.015" rpy="0 0 0" /> <origin xyz="0 0 0.015" rpy="0 0 0"/>
<geometry><box size="0.03 0.03 0.03" /></geometry> <geometry>
<box size="0.03 0.03 0.03"/>
</geometry>
<material name="hand_left-material"> <material name="hand_left-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.015" rpy="0 0 0" /> <origin xyz="0 0 0.015" rpy="0 0 0"/>
<geometry><box size="0.03 0.03 0.03" /></geometry> <geometry>
<box size="0.03 0.03 0.03"/>
</geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.015" rpy="0 0 0" /> <origin xyz="0 0 0.015" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_upper_leg_right" type="revolute"> <joint name="base_link_to_upper_leg_right" type="revolute">
<parent link="base_link" /> <parent link="base_link" />
<child link="upper_leg_right" /> <child link="upper_leg_right" />
<origin xyz="0.027260906368426083 0 -0.0372788919866843" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" /> <origin xyz="0.035 0 -0.035" rpy="0 0 0" />
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint>
<link name="upper_leg_right"> <link name="upper_leg_right">
<visual> <visual>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry> <geometry>
<box size="0.04 0.04 0.07" /> <box size="0.04 0.04 0.07"/>
</geometry> </geometry>
<material name="upper_leg_right-material"> <material name="upper_leg_right-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021 0.0497 0.4851 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry> <geometry>
<box size="0.04 0.04 0.07" /> <box size="0.04 0.04 0.07"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_lower_leg_right" type="revolute">
<parent link="base_link" />
<child link="lower_leg_right" /> <joint name="upper_leg_right_to_lower_leg_right" type="revolute">
<origin xyz="0.027260906368426083 0 -0.11088915657500885" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" /> <parent link="upper_leg_right"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <child link="lower_leg_right"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <origin xyz="0 0 -0.075" rpy="0 0 0"/>
<axis xyz="-0.9997 -0.0114 0.0229"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="lower_leg_right"> <link name="lower_leg_right">
<visual> <visual>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry> <geometry>
<box size="0.04 0.04 0.07" /> <box size="0.04 0.04 0.07"/>
</geometry> </geometry>
<material name="lower_leg_right-material"> <material name="lower_leg_right-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021 0.0497 0.4851 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry> <geometry>
<box size="0.04 0.04 0.07" /> <box size="0.04 0.04 0.07"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_lower_leg_right1" type="revolute">
<parent link="base_link" />
<child link="lower_leg_right1" /> <joint name="lower_leg_right_to_foot_right" type="revolute">
<origin xyz="0.027260906368426083 -0.004866298635659946 -0.17845456205258786" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" /> <parent link="lower_leg_right"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <child link="foot_right"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <origin xyz="0 0 -0.075" rpy="0 0 0"/>
<link name="lower_leg_right1"> <axis xyz="-1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="foot_right">
<visual> <visual>
<origin xyz="0 0.025 0.01" rpy="0 0 0" /> <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
<geometry> <geometry>
<box size="0.04 0.08 0.02" /> <box size="0.04 0.08 0.02"/>
</geometry> </geometry>
<material name="lower_leg_right1-material"> <material name="foot_right-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021 0.0497 0.4851 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0.025 0.01" rpy="0 0 0" /> <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
<geometry> <geometry>
<box size="0.04 0.08 0.02" /> <box size="0.04 0.08 0.02"/>
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0.025 0.01" rpy="0 0 0" /> <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
</inertial> </inertial>
</link> </link>
<!-- Left Upper Leg --> <!-- Left Upper Leg -->
<joint name="base_link_to_upper_leg_left" type="revolute"> <joint name="base_link_to_upper_leg_left" type="revolute">
<parent link="base_link" /> <parent link="base_link"/>
<child link="upper_leg_left" /> <child link="upper_leg_left"/>
<origin xyz="-0.027260906368426083 0 -0.0372788919866843" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" /> <origin xyz="-0.035 0 -0.035" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="upper_leg_left"> <link name="upper_leg_left">
<visual> <visual>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry><box size="0.04 0.04 0.07" /></geometry> <geometry>
<box size="0.04 0.04 0.07"/>
</geometry>
<material name="upper_leg_left-material"> <material name="upper_leg_left-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021 0.0497 0.4851 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry><box size="0.04 0.04 0.07" /></geometry> <geometry>
<box size="0.04 0.04 0.07"/>
</geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
</inertial> </inertial>
</link> </link>
<!-- Left Lower Leg --> <!-- Left Lower Leg -->
<joint name="base_link_to_lower_leg_left" type="revolute"> <joint name="upper_leg_left_to_lower_leg_left" type="revolute">
<parent link="base_link" /> <parent link="upper_leg_left"/>
<child link="lower_leg_left" /> <child link="lower_leg_left"/>
<origin xyz="-0.027260906368426083 0 -0.11088915657500885" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" /> <origin xyz="0 0 -0.075" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="-0.9997 -0.0114 0.0229"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="lower_leg_left"> <link name="lower_leg_left">
<visual> <visual>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry><box size="0.04 0.04 0.07" /></geometry> <geometry>
<box size="0.04 0.04 0.07"/>
</geometry>
<material name="lower_leg_left-material"> <material name="lower_leg_left-material">
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> <color rgba="0.0021 0.0497 0.4851 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<geometry><box size="0.04 0.04 0.07" /></geometry> <geometry>
<box size="0.04 0.04 0.07"/>
</geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0 0.03" rpy="0 0 0" /> <origin xyz="0 0 -0.035" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
</inertial> </inertial>
</link> </link>
<!-- Left Foot --> <!-- Left Foot -->
<joint name="base_link_to_lower_leg_left1" type="revolute"> <joint name="lower_leg_left_to_foot_left" type="revolute">
<parent link="base_link" /> <parent link="lower_leg_left"/>
<child link="lower_leg_left1" /> <child link="foot_left"/>
<origin xyz="-0.027260906368426083 -0.004866298635659946 -0.17845456205258786" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" /> <origin xyz="0 0 -0.075" rpy="0 0 0"/>
<axis xyz="-0.9996738127289344 -0.011364625593814749 0.022871673071897854"/> <axis xyz="-1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint> </joint>
<link name="lower_leg_left1">
<link name="foot_left">
<visual> <visual>
<origin xyz="0 0.025 0.01" rpy="0 0 0" /> <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
<geometry><box size="0.04 0.08 0.02" /></geometry> <geometry>
<material name="lower_leg_left1-material"> <box size="0.04 0.08 0.02"/>
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" /> </geometry>
<material name="foot_left-material">
<color rgba="0.0021 0.0497 0.4851 1"/>
</material> </material>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0.025 0.01" rpy="0 0 0" /> <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
<geometry><box size="0.04 0.08 0.02" /></geometry> <geometry>
<box size="0.04 0.08 0.02"/>
</geometry>
</collision> </collision>
<inertial> <inertial>
<origin xyz="0 0.025 0.01" rpy="0 0 0" /> <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
<mass value="1" /> <mass value="1"/>
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_neck" type="revolute">
<parent link="base_link" />
<child link="neck" /> <joint name="spine_1_to_neck" type="revolute">
<origin xyz="0 0 0.13701911638953843" rpy="0 0 0" /> <parent link="spine_1"/>
<axis xyz="0 -0 0.9999894272033905"/> <child link="neck"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <origin xyz="0 0 0.12" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="neck"> <link name="neck">
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
@ -415,12 +473,15 @@
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" /> <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
</inertial> </inertial>
</link> </link>
<joint name="base_link_to_head" type="revolute">
<parent link="base_link" /> <joint name="neck_to_head" type="revolute">
<child link="head" /> <parent link="neck"/>
<origin xyz="0 0 0.1835254369219812" rpy="0 0 0" /> <child link="head"/>
<axis xyz="0.9999427237624327 -0.0025510738064054572 0.010394287710165623"/> <origin xyz="0 0 0.04" rpy="0 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint> <axis xyz="-1 0 0"/>
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
</joint>
<link name="head"> <link name="head">
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />