Merge pull request #82 from dojo-sim/jan/add_mechanisms

Add mechanisms

DojoEnvironments/src/mechanisms/exoskeleton/dependencies/model.urdf

@@ -0,0 +1,161 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from                                | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="Exoskeleton">
+
+# output dir
+
+	<!-- Human body -->
+	<link name="base">
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 1.15"/>
+			<mass value="1"/>
+			<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 1.5707963267948966" xyz="0.33 0.22 0.0"/>
+			<geometry>
+				<mesh filename="mesh/torso.obj" scale="0.027 0.027 0.027"/>
+			</geometry>
+			<material name="color_human">
+				<color rgba="0.8 0.8 0.8 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<!-- 3% of 75kg = 2.25 -->
+	<link name="upper_arm">
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 -0.155"/>
+			<inertia ixx="0.01875" ixy="0" ixz="0" iyy="0.01875" iyz="0" izz="0.00375"/>
+			<mass value="2.25"/>
+		</inertial>
+		<visual>
+			<!-- x=up/down, y=side in/out, z=back/forward -->
+			<origin rpy="0 3.1415926535897  1.5707963267948966" xyz="0.08499999999999999 -0.21999999999999986 0.8412999999999999"/>
+			<geometry>
+				<mesh filename="mesh/upperarm.obj" scale="0.027 0.027 0.027"/>
+			</geometry>
+			<material name="color_human">
+				<color rgba="0.8 0.8 0.8 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<!-- 2% of 75kg = 1.5 -->
+	<link name="lower_arm">
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0.125 0"/>
+			<inertia ixx="0.008515625" ixy="0" ixz="0" iyy="0.00140625" iyz="0" izz="0.008515625"/>
+			<mass value="1.5"/>
+		</inertial>
+		<visual>
+			<!-- x=up/down, y=side in/out, z=back/forward -->
+			<origin rpy="0 -1.5707963267948966  1.5707963267948966" xyz="0.08499999999999999 0.8412999999999999 0.21999999999999986"/>
+			<geometry>
+				<mesh filename="mesh/lowerarm.obj" scale="0.027 0.027 0.027"/>
+			</geometry>
+			<material name="color_human">
+				<color rgba="0.8 0.8 0.8 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<link name="sAA_Unit">
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0.1301 -0.1279"/>
+			<mass value="6.712"/>
+			<inertia ixx="0.3787893" ixy="-0.0006556" ixz="-0.0006709" iyy="0.192833" iyz="0.0540633" izz="0.1956839"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.2"/>
+			<geometry>
+				<mesh filename="mesh/sAA_simple.obj" scale="0.001 0.001 0.001"/>
+			</geometry>
+			<material name="color_exo">
+				<color rgba="0.4 0.4 0.4 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<link name="sFE_Unit">
+		<inertial>
+			<origin rpy="0 0 0" xyz="0.0001 0.1051 -0.1201"/>
+			<mass value="3.487"/>
+			<inertia ixx="0.1108677" ixy="-0.0000977" ixz="0.0000399" iyy="0.0689688" iyz="0.0384741" izz="0.0569634"/>
+		</inertial>
+		<visual>
+			<origin rpy="1.5707963267948966 3.1415926535897 1.5707963267948966" xyz="0 -0.215 0"/>
+			<geometry>
+				<mesh filename="mesh/sFE_simple.obj" scale="0.001 0.001 0.001"/>
+			</geometry>
+			<material name="color_exo">
+				<color rgba="0.4 0.4 0.4 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<link name="sIE_Unit">
+		<inertial>
+			<origin rpy="0 0 0" xyz="-0.0006 0.1079 -0.03"/>
+			<mass value="1.663"/>
+			<inertia ixx="0.0266938" ixy="0.0001282" ixz="-0.0000556" iyy="0.007941" iyz="0.0038772" izz="0.0235228"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.512"/>
+			<geometry>
+				<mesh filename="mesh/sIE_simple.obj" scale="0.001 0.001 0.001"/>
+			</geometry>
+			<material name="color_exo">
+				<color rgba="0.4 0.4 0.4 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<link name="eFE_Unit">
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0.0557 -0.0803"/>
+			<mass value="0.494"/>
+			<inertia ixx="0.0069974" ixy="-0.0000009" ixz="-0.0000004" iyy="0.0035498" iyz="0.001812" izz="0.0037622"/>
+		</inertial>
+		<visual>
+			<origin rpy="-1.05 1.5707963267948966 0" xyz="0 0 -0.12"/>
+			<geometry>
+				<mesh filename="mesh/eFE_simple.obj" scale="0.001 0.001 0.001"/>
+			</geometry>
+			<material name="color_exo">
+				<color rgba="0.4 0.4 0.4 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<joint name="sAA" type="continuous">
+		<parent link="base"/>
+		<child link="sAA_Unit"/>
+		<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.2483 0 1.15"/>
+		<axis xyz="0 0 1"/>
+	</joint>
+	<joint name="sFE" type="continuous">
+		<parent link="sAA_Unit"/>
+		<child link="sFE_Unit"/>
+		<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+		<axis xyz="0 0 1"/>
+	</joint>
+	<joint name="sIE" type="continuous">
+		<parent link="sFE_Unit"/>
+		<child link="sIE_Unit"/>
+		<origin rpy="-1.5707963267948966 0 0" xyz="0 0.3087 0"/>
+		<axis xyz="0 0 1"/>
+	</joint>
+	<joint name="eFE" type="continuous">
+		<parent link="sIE_Unit"/>
+		<child link="eFE_Unit"/>
+		<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
+		<axis xyz="0 0 1"/>
+	</joint>
+	<joint name="sIE_to_upper" type="fixed">
+		<parent link="sIE_Unit"/>
+		<child link="upper_arm"/>
+		<origin rpy="0 0 0" xyz="0 0 0"/>
+	</joint>
+	<joint name="eFe_to_lower" type="fixed">
+		<parent link="eFE_Unit"/>
+		<child link="lower_arm"/>
+		<origin rpy="0 0 0" xyz="0 0 0"/>
+	</joint>
+</robot>

DojoEnvironments/src/mechanisms/exoskeleton/mechanism.jl

@@ -0,0 +1,56 @@
+function get_exoskeleton(;
+    timestep=0.01,
+    input_scaling=timestep, 
+    gravity=-9.81,
+    urdf=:model,
+    springs=0,
+    dampers=0,
+    parse_springs=true, 
+    parse_dampers=true,
+    limits=true,
+    joint_limits=Dict([
+        (:sAA, [0, 90]*π/180),
+        (:sFE, [0, 90]*π/180),
+        (:sIE, [-80, 25]*π/180),
+        (:eFE, [-125,0]*π/180),]),
+    keep_fixed_joints=false, 
+    T=Float64)
+
+    # mechanism
+    path = joinpath(@__DIR__, "dependencies/$(string(urdf)).urdf")
+    mechanism = Mechanism(path; floating=false, T,
+        gravity, timestep, input_scaling, 
+        parse_dampers, keep_fixed_joints)
+
+    # springs and dampers
+    !parse_springs && set_springs!(mechanism.joints, springs)
+    !parse_dampers && set_dampers!(mechanism.joints, dampers)
+
+    # joint limits    
+    if limits
+        joints = set_limits(mechanism, joint_limits)
+
+        mechanism = Mechanism(mechanism.origin, mechanism.bodies, joints;
+            gravity, timestep, input_scaling)
+    end
+
+    # zero configuration
+    initialize_exoskeleton!(mechanism)
+
+    # construction finished
+    return mechanism
+end
+
+function initialize_exoskeleton!(mechanism::Mechanism;
+    joint_angles=[pi/2;pi/2-0.1;0;-0.1])
+
+    zero_velocities!(mechanism)
+    zero_coordinates!(mechanism)
+
+    set_minimal_coordinates!(mechanism, get_joint(mechanism, :sAA), [joint_angles[1]])
+    set_minimal_coordinates!(mechanism, get_joint(mechanism, :sFE), [joint_angles[2]])
+    set_minimal_coordinates!(mechanism, get_joint(mechanism, :sIE), [joint_angles[3]])
+    set_minimal_coordinates!(mechanism, get_joint(mechanism, :eFE), [joint_angles[4]])
+
+    return
+end

DojoEnvironments/src/mechanisms/include.jl

@@ -4,6 +4,7 @@ include("block/mechanism.jl")
 include("block2d/mechanism.jl")
 include("cartpole/mechanism.jl")
 include("dzhanibekov/mechanism.jl")
+include("exoskeleton/mechanism.jl")
 include("fourbar/mechanism.jl")
 include("halfcheetah/mechanism.jl")
 include("hopper/mechanism.jl")
@@ -12,12 +13,14 @@ include("npendulum/mechanism.jl")
 include("nslider/mechanism.jl")
 include("panda/mechanism.jl")
 include("pendulum/mechanism.jl")
+include("quadrotor/mechanism.jl")
 include("quadruped/mechanism.jl")
 include("raiberthopper/mechanism.jl")
 include("slider/mechanism.jl")
 include("snake/mechanism.jl")
 include("sphere/mechanism.jl")
 include("tippetop/mechanism.jl")
 include("twister/mechanism.jl")
+include("uuv/mechanism.jl")
 include("walker/mechanism.jl")
 include("youbot/mechanism.jl")