Skip to content

Commit

Permalink
Merge pull request #200 from shadow-robot/ubi-agni-B_realistic_inertia
Browse files Browse the repository at this point in the history
Realistic inertia
  • Loading branch information
ugocupcic committed Feb 23, 2015
2 parents 234786b + a95c983 commit 34200ab
Show file tree
Hide file tree
Showing 34 changed files with 616 additions and 265 deletions.
5 changes: 5 additions & 0 deletions sr_description/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
This package contains the description for Shadow Robot's Hand and Arm, as well as some additional models used in our robot (kinect, etc...).

You can find the different complete models in [robots](robots).

You can also find more information about the computation of the [arm kinematics](doc/ArmInertia.md) and of the [hand kinematics](doc/HandInertia.md).
26 changes: 13 additions & 13 deletions sr_description/arm/config/arm_controller.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2,37 +2,37 @@ sa_sr_position_controller:
type: effort_controllers/JointPositionController
joint: ShoulderJRotate
pid:
p: 60.0
i: 2.0
d: 80.0
p: 15.0
i: 1.0
d: 5.0
i_clamp: 30.0

sa_ss_position_controller:
type: effort_controllers/JointPositionController
joint: ShoulderJSwing
pid:
p: 100.0
i: 20.0
d: 80.0
p: 15.0
i: 30.0
d: 10.0
i_clamp: 90.0

sa_es_position_controller:
type: effort_controllers/JointPositionController
joint: ElbowJSwing
pid:
p: 50.0
i: 5.0
d: 40.0
p: 30.0
i: 10.0
d: 5.0
i_clamp: 90.0

sa_er_position_controller:
type: effort_controllers/JointPositionController
joint: ElbowJRotate
pid:
p: 50.0
i: 5.0
d: 50.0
i_clamp: 2.0
p: 30.0
i: 3.0
d: 5.0
i_clamp: 5.0

r_arm_cartesian_pose_controller:
type: robot_mechanism_controllers/CartesianPoseController
Expand Down
2 changes: 1 addition & 1 deletion sr_description/arm/xacro/base/shadowarm_base.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.4" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
<inertia ixx="0.00071" ixy="0.0" ixz="0.0" iyy="0.00071" iyz="0.0" izz="0.0014" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.75" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.016" />
<inertia ixx="0.00086" ixy="0.0" ixz="0.0" iyy="0.00086" iyz="0.0" izz="0.0017" />
</inertial>
<visual>
<origin xyz="0 0 -0.04" rpy="0 0 0"/>
Expand Down Expand Up @@ -38,7 +38,10 @@
effort="30" velocity="1.0"/>
<dynamics damping="5.5"/>
</joint>

<gazebo reference="ElbowJRotate">
<provideFeedback>1</provideFeedback>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<xacro:shadowarm_handsupport_motor_transmission />

</xacro:macro>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.75" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.016" />
<inertia ixx="0.00092" ixy="0.0" ixz="0.0" iyy="0.00092" iyz="0.0" izz="0.0018" />
</inertial>
<visual>
<origin xyz="0 0 -0.04" rpy="${M_PI/2} 0 0"/>
Expand Down Expand Up @@ -38,7 +38,10 @@
effort="30" velocity="1.0"/>
<dynamics damping="5.5"/>
</joint>

<gazebo reference="ElbowJRotate">
<provideFeedback>1</provideFeedback>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<xacro:shadowarm_handsupport_muscle_transmission />

</xacro:macro>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<inertial>
<origin xyz="0.08 0 0.0625" rpy="0 0 0" />
<mass value="1.7" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
<inertia ixx="0.0016" ixy="0.0" ixz="0.0" iyy="0.0016" iyz="0.0" izz="0.0018" />
</inertial>
<visual>
<origin xyz="0.03 0 0.036" rpy="0 0 0"/>
Expand Down Expand Up @@ -37,7 +37,10 @@
effort="50" velocity="1.0"/>
<dynamics damping="5.5"/>
</joint>

<gazebo reference="ElbowJSwing">
<provideFeedback>1</provideFeedback>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<xacro:shadowarm_lowerarm_transmission />

</xacro:macro>
Expand Down
7 changes: 6 additions & 1 deletion sr_description/arm/xacro/trunk/shadowarm_trunk.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<inertial>
<origin xyz="0 0 0.26525" rpy="0 0 0" />
<mass value="10" />
<inertia ixx="0.15" ixy="0.0" ixz="0.0" iyy="0.15" iyz="0.0" izz="0.02" />
<inertia ixx="0.298" ixy="0.0" ixz="0.0" iyy="0.298" iyz="0.0" izz="0.0361" />
</inertial>
<visual>
<origin xyz="0 0 0.005" rpy="0 0 0" />
Expand Down Expand Up @@ -38,6 +38,11 @@
<dynamics damping="5.5"/>
</joint>

<gazebo reference="ShoulderJRotate">
<provideFeedback>1</provideFeedback>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>

<xacro:shadowarm_trunk_transmission />

</xacro:macro>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<inertial>
<origin xyz="0 0 0.1845" rpy="0 0 0" />
<mass value="2" />
<inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.05" />
<inertia ixx="0.044" ixy="0.0" ixz="0.0" iyy="0.044" iyz="0.0" izz="0.0056" />
</inertial>
<visual>
<origin xyz="0 0.067 0" rpy="0 0 0" />
Expand Down Expand Up @@ -36,7 +36,10 @@
<limit lower="0" upper="${M_PI/2}" effort="100" velocity="1.0"/>
<dynamics damping="5.3"/>
</joint>

<gazebo reference="ShoulderJSwing">
<provideFeedback>1</provideFeedback>
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
<xacro:shadowarm_upperarm_transmission />

</xacro:macro>
Expand Down
86 changes: 86 additions & 0 deletions sr_description/doc/ArmInertia.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
Author Guillaume WALCK [email protected]

work done during the HANDLE project in 2011 (inertia tensor moved to joint axis)
reworked 2015 (inertia tensor at COM)

Overview
========

Arm inertia estimated from bounding box or bounding cylinders.
mass estimated from volume of bounding boxes times density of material
computation done with octave


Octave functions
================

```octave
function [ix,iy,iz] = inertiabox(sx,sy,sz,mass)
ix = 1/12.0*mass*(sy*sy+sz*sz);
iy = 1/12.0*mass*(sx*sx+sz*sz);
iz = 1/12.0*mass*(sy*sy+sx*sx);
end
```

```octave
function [ix,iy,iz] = inertiacyl(radius,length,mass)
% along z
iz = mass* radius*radius/2;
ix = 1/12.0* mass * (3*radius*radius+length*length);
iy = ix;
end
```

Inertia Tensors
===============

```octave
base aluminum m=400g
inertiabox(0.145,0.145,0.012,0.4)
Ix = 7.0563e-04
Iy = 7.0563e-04
Iz = 0.0014017
```


```octave
hand_support_plate_motor aluminum m=750g
inertiacyl(0.0675,0.006,0.75)
Ix = 8.5655e-04
Iy = 8.5655e-04
Iz = 0.0017086
```

```octave
hand_support_plate_muscle aluminum m=750g
inertiacyl(0.07,0.006,0.75)
Ix = 9.2100e-04
Iy = 9.2100e-04
Iz = 0.0018375
```

```octave
trunk various material m=10kg
inertiacyl(0.17/2,0.580,10.0)
Ix = 0.29840
Iy = 0.29840
Iz = 0.036125
```

```octave
upperarm various material m=2kg
inertiacyl(0.15/2,0.500,2.0)
Ix = 0.044479
Iy = 0.044479
Iz = 0.0056250
```

```octave
lower_arm various material m=1.7kg
inertiabox(0.080,0.078,0.07,1.7)
Ix = 0.0015561
Iy = 0.0016008
Iz = 0.0017686
```
137 changes: 137 additions & 0 deletions sr_description/doc/HandInertia.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
Author Guillaume WALCK [email protected]

work done during the HANDLE project in 2011 (inertia tensor moved to joint axis)
reworked 2015 (inertia tensor at COM)

Overview
========

Hand inertia estimated from bounding box or bounding cylinders.
mass estimated from volume of bounding boxes times density of material
(sometimes adjusted to match real values)
computation done with octave


Octave functions
================

```octave
function [ix,iy,iz] = inertiabox(sx,sy,sz,mass)
ix = 1/12.0*mass*(sy*sy+sz*sz);
iy = 1/12.0*mass*(sx*sx+sz*sz);
iz = 1/12.0*mass*(sy*sy+sx*sx);
end
```

```octave
function [ix,iy,iz] = inertiacyl(radius,length,mass)
% along z
iz = mass* radius*radius/2;
ix = 1/12.0* mass * (3*radius*radius+length*length);
iy = ix;
end
```

Inertia tensors
===============

density delrin = 1.41g/cm3
aluminum density = 2.7g/cm3

```octave
wrist aluminum V=16+37.7/2 = 41cm3 m=100g
inertiabox(0.030,0.066,0.058,0.1)
Ix = 6.4333e-05
Iy = 3.5533e-05
Iz = 4.3800e-05
```

```octave
palm delrin V=200cm3 m=300g
inertiabox(0.085,0.020,0.118,0.30)
Ix = 3.5810e-04
Iy = 5.2872e-04
Iz = 1.9063e-04
```

```octave
lfmetacarpal V=13cm3 m=30g
inertiabox(0.035,0.022,0.073,0.030)
Ix = 1.4532e-05
Iy = 1.6385e-05
Iz = 4.2725e-06
```

```octave
knuckle aluminum V=3cm3 m=8g
inertiacyl(0.009,0.012,0.008)
Ix = 2.5800e-07
Iy = 2.5800e-07
Iz = 3.2400e-07
```

```octave
proximal delrin V=21cm3 m=30g
inertiabox(0.02,0.018,0.06,0.030)
Ix = 9.8100e-06
Iy = 1.0000e-05
Iz = 1.8100e-06
```

```octave
middle delrin V=12.24cm3 m = 17g
inertiabox(0.018,0.017,0.04,0.017)
Ix = 2.6761e-06
Iy = 2.7257e-06
Iz = 8.6842e-07
```

```octave
distal delrin + rubber V=7cm3 m=12g
inertiabox(0.018,0.0145,0.027,0.012)
Ix = 9.3925e-07
Iy = 1.0530e-06
Iz = 5.3425e-07
```

```octave
thdistal delrin + rubber V=13cm3 m=16g
inertiabox(0.021,0.018,0.035,0.016)
Ix = 2.0653e-06
Iy = 2.2213e-06
Iz = 1.0200e-06
```

```octave
thproximal delrin V=29.5cm3 m=40g
inertiacyl(0.025/2,0.06,0.04)
Ix = 1.3562e-05
Iy = 1.3562e-05
Iz = 3.1250e-06
```

```octave
thmiddle delrin V=19.77cm3 m=27g
inertiacyl(0.022/2,0.052,0.027)
Ix = 6.9007e-06
Iy = 6.9007e-06
Iz = 1.6335e-06
```

```octave
thhub m=5g
inertiabox(0.005,0.005,0.005,0.005)
Ix = 2.0833e-08
Iy = 2.0833e-08
Iz = 2.0833e-08
```

```octave
thbase m=10g
inertiabox(0.010,0.010,0.010,0.010)
Ix = 1.6667e-07
Iy = 1.6667e-07
Iz = 1.6667e-07
```
Loading

0 comments on commit 34200ab

Please sign in to comment.