Skip to content
GitLab
Commit c2a4e49e authored by Jan Koniarik's avatar Jan Koniarik
Browse files

Wololo

parent 26c03eeb
Expand all Hide whitespace changes
Inline Side-by-side
schpin_koke/CMakeLists.txt
View file @ c2a4e49e
......@@ -119,3 +119,4 @@ include_directories(
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
schpin_koke/Makefile deleted 100644 → 0
View file @ 26c03eeb
This diff is collapsed.
schpin_koke/launch/robot_description.launch
View file @ c2a4e49e
<launch>
<param name="robot_description" textfile="$(find schpin_koke)/src/urdf/koke.urdf"/>
<param name="robot_semantics" textfile="$(find schpin_koke)/src/srdf/koke.srdf"/>
<param name="robot_description" textfile="$(find schpin_koke)/src/koke.urdf"/>
<param name="robot_semantics" textfile="$(find schpin_koke)/src/koke.srdf"/>
</launch>
schpin_koke/schpin-koke.cabal
View file @ c2a4e49e
......@@ -51,13 +51,13 @@ executable schpin-koke
main-is: Main.hs
-- Modules included in this executable, other than Main.
-- other-modules:
other-modules: Package, Parser
-- LANGUAGE extensions used by modules in this package.
-- other-extensions:
-- Other library packages from which modules are imported.
build-depends: base >=4.11, linear, schpin-robot-lib, dimensional, directory, mtl
build-depends: base >=4.11, linear, schpin-robot-lib, dimensional, directory, mtl, language-openscad, containers, bytestring, filepath, unordered-containers, csv
-- Directories containing source files.
hs-source-dirs: src
......
schpin_koke/src/Main.hs
View file @ c2a4e49e
import Control.Monad.Reader ( runReader )
import Language.OpenSCAD as LS
import Linear.Quaternion
import Linear.V3
import Numeric.Units.Dimensional.Prelude ( degree
, meter
, radian
, second
, (*~)
)
import Numeric.Units.Dimensional.Prelude
( degree
, meter
, radian
, second
, (*~)
)
import qualified Numeric.Units.Dimensional.Prelude
as D
import Package
import Parser ( extractParts )
import SCAD
import Shared
import SRDF.Xml ( writeSRDFXML )
import System.Directory ( getCurrentDirectory, createDirectory, copyFile)
import URDF.Xml ( writeURDFXML )
import System.IO.Unsafe
import Walker
import Walker.Pkg
import Walker.SCAD
import Walker.SRDF
import Walker.URDF
data LegPos = RL | RR | FR | FL deriving Eq;
data ConfType = Default | Folded | Expanded deriving (Show, Eq);
kokeTipPos :: Pose
kokeTipPos =
Pose (V3 (100 *~ mm) (0 *~ mm) (0 *~ mm)) (toQuat zAxis (0 *~ degree))
Pose (V3 (110 *~ mm) (0 *~ mm) (0 *~ mm)) (toQuat zAxis (0 *~ degree))
femurOffset :: ConfType -> Pose
femurOffset conf_type =
Pose
(V3 (if conf_type == Expanded then 100 *~ mm else 65 *~ mm)
(0 *~ mm)
(0 *~ mm)
)
$ toQuat yAxis (180 *~ degree)
Pose
(V3 (if conf_type == Expanded then 100 *~ mm else 65 *~ mm)
(0 *~ mm)
(0 *~ mm)
)
$ toQuat yAxis (180 *~ degree)
tibiaOffset :: Pose
tibiaOffset =
Pose (V3 (95 *~ mm) (0 *~ mm) (30 *~ mm)) $ toQuat yAxis (270 *~ degree)
Pose (V3 (95 *~ mm) (0 *~ mm) (30 *~ mm)) $ toQuat yAxis (270 *~ degree)
legPose :: LegPos -> ConfType -> Pose
legPose pos conf_type = Pose (V3 x y (0 *~ mm))
$ toQuat zAxis (angle *~ degree)
where
x_val = if conf_type == Expanded then 85 else 45
x = case pos of
FR -> x_val *~ mm
FL -> x_val *~ mm
RR -> (-x_val) *~ mm
RL -> (-x_val) *~ mm
y_val = 36
y = case pos of
FR -> (-y_val) *~ mm
FL -> y_val *~ mm
RR -> (-y_val) *~ mm
RL -> y_val *~ mm
angle = case pos of
FR -> 0
FL -> 0
RR -> 180
RL -> 180
$ toQuat zAxis (angle *~ degree)
where
x_val = if conf_type == Expanded then 85 else 60
x = case pos of
FR -> x_val *~ mm
FL -> x_val *~ mm
RR -> (-x_val) *~ mm
RL -> (-x_val) *~ mm
y_val = 52
y = case pos of
FR -> (-y_val) *~ mm
FL -> y_val *~ mm
RR -> (-y_val) *~ mm
RL -> y_val *~ mm
angle = case pos of
FR -> 0
FL -> 0
RR -> 180
RL -> 180
baseAxis :: LegPos -> Axis
baseAxis pos = case pos of
FR -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)
FL -> zAxis
RR -> zAxis
RL -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)
FR -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)
FL -> zAxis
RR -> zAxis
RL -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)
leg :: LegPos -> ConfType -> SimpleLeg
leg leg_pos conf_type = Leg
{ leg_name = case leg_pos of
RL -> "rl"
RR -> "rr"
FL -> "fl"
FR -> "fr"
, root_joint = coxa
}
where
coxa = SimpleJoint $ Joint
{ joint_name = "alfa"
, offset = legPose leg_pos conf_type
, jtype = RevoluteJoint
{ axis = baseAxis leg_pos
, min_angle = 0 *~ radian
, default_angle = case conf_type of
Folded -> 0 *~ degree
_ -> 45 *~ degree
, max_angle = 90 *~ degree
, vel_lim = 0.5 *~ (radian D./ second)
, effor_lim = 100
, g_steps = 16
}
, sub_link = Link
{ link_name = "coxa"
, parts =
[ Part
{ part_name = "coxa"
, model = SCADModel
"coxa"
[ toArg "femurOffset" $ pos $ femurOffset Default
, toArg
"expand"
(if conf_type == Expanded
then 10 *~ mm
else 0 *~ mm
)
]
["coxa.scad"]
, collision_models =
[ SCADModel
"coxa_collision"
[ toArg "femurOffset" $ pos $ femurOffset
Default
]
["coxa.scad"]
]
, part_weight = Nothing
, center_of_mass = Nothing
}
]
}
, sub_joint = Just femur
{ leg_name = case leg_pos of
RL -> "rl"
RR -> "rr"
FL -> "fl"
FR -> "fr"
, root_joint = coxa
}
where
coxa :: SimpleJoint
coxa = SimpleJoint $ Joint
{ joint_name = "alfa"
, offset = legPose leg_pos conf_type
, jtype = RevoluteJoint
{ axis = baseAxis leg_pos
, min_angle = 0 *~ radian
, default_angle = case conf_type of
Folded -> 0 *~ degree
_ -> 45 *~ degree
, max_angle = 90 *~ degree
, vel_lim = 0.5 *~ (radian D./ second)
, effor_lim = 100
, g_steps = 16
}
, sub_link = Link
{ link_name = "coxa"
, assembly = Just $ Assembly
{ amodel = SCADModel
"coxa"
[ toArg "femurOffset" $ pos $ femurOffset Default
, toArg
"expand"
(if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
]
["coxa.scad"]
, collision_models = [ SCADModel
"coxa_collision"
[ toArg "femurOffset" $ pos $ femurOffset
Default
]
["coxa.scad"]
]
}
}
, sub_joint = Just femur
}
femur = SimpleJoint $ Joint
{ joint_name = "beta"
, offset = femurOffset conf_type
, jtype = RevoluteJoint
{ axis = V3 (0 *~ meter) ((-1) *~ meter) (0 *~ meter)
, min_angle = 0 *~ radian
, default_angle = case conf_type of
Folded -> 0 *~ degree
_ -> 180 *~ degree
, max_angle = 240 *~ degree
, vel_lim = 0.5 *~ (radian D./ second)
, effor_lim = 100
, g_steps = 16
}
, sub_link = Link
{ link_name = "femur"
, parts =
[ Part
{ part_name = "femur"
, model = SCADModel
"femur"
[ toArg "tibiaOffset" $ pos tibiaOffset
, toArg
"expand"
(if conf_type == Expanded
then 10 *~ mm
else 0 *~ mm
)
]
["femur.scad"]
, collision_models = [ SCADModel
"femur_collision_a"
[ toArg "tibiaOffset"
$ pos tibiaOffset
]
["femur.scad"]
, SCADModel
"femur_collision_b"
[ toArg "tibiaOffset"
$ pos tibiaOffset
]
["femur.scad"]
]
, part_weight = Nothing
, center_of_mass = Nothing
}
]
}
, sub_joint = Just tibia
}
femur :: SimpleJoint
femur = SimpleJoint $ Joint
{ joint_name = "beta"
, offset = femurOffset conf_type
, jtype = RevoluteJoint
{ axis = V3 (0 *~ meter) ((-1) *~ meter) (0 *~ meter)
, min_angle = 0 *~ radian
, default_angle = case conf_type of
Folded -> 6 *~ degree
_ -> 180 *~ degree
, max_angle = 240 *~ degree
, vel_lim = 0.5 *~ (radian D./ second)
, effor_lim = 100
, g_steps = 16
}
, sub_link = Link
{ link_name = "femur"
, assembly = Just $ Assembly
{ amodel = SCADModel
"femur"
[ toArg "tibiaOffset" $ pos tibiaOffset
, toArg
"expand"
(if conf_type == Expanded
then 10 *~ mm
else 0 *~ mm
)
]
["femur.scad"]
, collision_models = [ SCADModel
"femur_collision_a"
[ toArg "tibiaOffset"
$ pos tibiaOffset
]
["femur.scad"]
, SCADModel
"femur_collision_b"
[ toArg "tibiaOffset"
$ pos tibiaOffset
]
["femur.scad"]
]
}
}
, sub_joint = Just tibia
}
tibia = SimpleJoint $ Joint
{ joint_name = "gama"
, offset = tibiaOffset
, jtype = RevoluteJoint
{ axis = yAxis
, min_angle = 0 *~ degree
, default_angle = case conf_type of
Folded -> 270 *~ degree
_ -> 180 *~ degree
, max_angle = 240 *~ degree
, vel_lim = 0.5 *~ (radian D./ second)
, effor_lim = 100
, g_steps = 16
}
, sub_link = Link
{ link_name = "tibia"
, parts =
[ Part
{ part_name = "tibia"
, model = SCADModel
"tibia"
[ toArg "tipOffset" (pos kokeTipPos)
, toArg
"expand"
(if conf_type == Expanded
then 10 *~ mm
else 0 *~ mm
)
]
["tibia.scad"]
, collision_models = [ SCADModel
"tibia_collision"
[ toArg "tipOffset"
(pos kokeTipPos)
]
["tibia.scad"]
]
, part_weight = Nothing
, center_of_mass = Nothing
}
]
}
, sub_joint = Just tip
tibia :: SimpleJoint
tibia = SimpleJoint $ Joint
{ joint_name = "gama"
, offset = tibiaOffset
, jtype = RevoluteJoint
{ axis = yAxis
, min_angle = 0 *~ degree
, default_angle = case conf_type of
Folded -> 276 *~ degree
_ -> 180 *~ degree
, max_angle = 240 *~ degree
, vel_lim = 0.5 *~ (radian D./ second)
, effor_lim = 100
, g_steps = 16
}
, sub_link = Link
{ link_name = "tibia"
, assembly = Just $ Assembly
{ amodel = SCADModel
"tibia"
[ toArg "tipOffset" (pos kokeTipPos)
, toArg "expand" (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
]
["tibia.scad"]
, collision_models = [ SCADModel "tibia_collision"
[toArg "tipOffset" (pos kokeTipPos)]
["tibia.scad"]
]
}
}
, sub_joint = Just tip
}
tip = SimpleJoint $ Joint
{ joint_name = "tipj"
, offset = kokeTipPos
, jtype = FixedJoint
, sub_link = Link { link_name = "tip", parts = [] }
, sub_joint = Nothing
}
tip :: SimpleJoint
tip = SimpleJoint $ Joint
{ joint_name = "tipj"
, offset = kokeTipPos
, jtype = FixedJoint
, sub_link = Link { link_name = "tipl", assembly = Nothing }
, sub_joint = Nothing
}
koke :: ConfType -> SimpleWalker
koke conf_type = Walker
{ walker_name = "Koke"
, legs = legs
, body = Body
{ body_name = "body"
, body_part = Part
{ part_name = "body"
, model = SCADModel
"body"
[ toArg "positions" leg_positions
, toArg
"expand"
(if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
]
["body.scad"]
, collision_models =
[ SCADModel "body_collision"
[toArg "positions" leg_positions]
["body.scad"]
]
, part_weight = Nothing
, center_of_mass = Nothing
}
}
{ walker_name = "Koke"
, legs = legs
, body = Body
{ body_name = "body"
, body_ass = Assembly
{ amodel = SCADModel
"body"
[ toArg "positions" leg_positions
, toArg "expand" (if conf_type == Expanded then 10 *~ mm else 0 *~ mm)
]
["body.scad"]
, collision_models = [ SCADModel "body_collision"
[toArg "positions" leg_positions]
["body.scad"]
]
}
}
where
leg_positions = (\x -> pos $ legPose x Default) <$> [RR, RL, FR, FL]
legs = (`leg` conf_type) <$> [RR, RL, FR, FL]
}
where
leg_positions = (\x -> pos $ legPose x Default) <$> [RR, RL, FR, FL]
legs = (`leg` conf_type) <$> [RR, RL, FR, FL]
main :: IO ()
main = do
cwd <- getCurrentDirectory
let ew = ewalker cwd Default
let conf = config cwd
mapM_ (uncurry renderSCAD)
$ [ (cwd ++ "/gen/skeleton/default.scad", getSCADVisualSkeleton ew)
, ( cwd ++ "/gen/skeleton/default_col.scad"
, getSCADCollisionSkeleton ew
)
, ( cwd ++ "/gen/skeleton/folded.scad"
, getSCADVisualSkeleton (ewalker cwd Folded)
)
, ( cwd ++ "/gen/skeleton/expanded.scad"
, getSCADVisualSkeleton (ewalker cwd Expanded)
)
]
generateWalkerPackage (koke Default) conf
createDirectory(pkg_dir conf ++ "/launch")
copyFile "launch/robot_description.launch" (pkg_dir conf ++ "/launch/robot_description.launch")
where
ewalker cwd conf_type =
runReader (extendWalker $ koke conf_type) $ config cwd
config cwd = PackageConfig { pkg_name = "schpin_koke"
, scad_dir = cwd ++ "/tmp/scad/"
, pkg_dir = cwd ++ "/pkg/schpin_koke/"
, src_dir = cwd ++ "/src/scad/"
}
main = exportPackage koke
schpin_koke/src/Package.hs 0 → 100644
View file @ c2a4e49e
module Package where
import Walker
import Control.Monad.Reader ( runReader )
import Walker.Pkg
import System.Directory ( getCurrentDirectory
, createDirectory
, copyFile, createDirectoryIfMissing
)
import SCAD
import Walker.SCAD
import Walker.SRDF
import Walker.URDF
import Parser
data ConfType = Default | Folded | Expanded deriving (Show, Eq);
exportPackage :: (ConfType -> SimpleWalker) -> IO ()
exportPackage w = do
cwd <- getCurrentDirectory
let ew = ewalker cwd Default
let conf = config cwd
mapM_ (uncurry renderSCAD)
$ [ (cwd ++ "/gen/skeleton/default.scad" , getSCADVisualSkeleton ew)
, (cwd ++ "/gen/skeleton/default_col.scad", getSCADCollisionSkeleton ew)
, ( cwd ++ "/gen/skeleton/folded.scad"
, getSCADVisualSkeleton (ewalker cwd Folded)
)
, ( cwd ++ "/gen/skeleton/expanded.scad"
, getSCADVisualSkeleton (ewalker cwd Expanded)
)
]
generateBOM (ew) (config cwd)
generateWalkerPackage (w Default) conf
createDirectoryIfMissing False (pkg_dir conf ++ "/launch")
copyFile "launch/robot_description.launch"
(pkg_dir conf ++ "/launch/robot_description.launch")
where
ewalker cwd conf_type = runReader (extendWalker $ w conf_type) $ config cwd
config cwd = PackageConfig { pkg_name = "schpin_koke"
, scad_dir = cwd ++ "/tmp/scad"
, pkg_dir = cwd ++ "/pkg/schpin_koke"
, src_dir = cwd ++ "/src/scad"
}
schpin_koke/src/Parser.hs 0 → 100644
View file @ c2a4e49e
This diff is collapsed.
schpin_koke/src/scad/LX15D.scad
View file @ c2a4e49e
include<metric.scad>;
include<tile.scad>;
include<util.scad>;
include<part.scad>;
LX15D_dim = [ 44.17, 22.95, 25.5 ];
LX15D_middle_t = 1.5; // height of the middle space
......@@ -16,7 +17,7 @@ LX15D_body_screw_d = 2;
LX15D_horn_clearance_d = 8;
LX15D_clearance_r = sqrt(pow(LX15D_dim[1] / 2, 2) + pow(LX15D_axis_offset, 2));
module LX15D() finalize("gray") difference() {
module LX15D() part("gray") difference() {
dim = LX15D_dim;
union() {
translate([ dim[0] / 2 - LX15D_axis_offset, 0, 0 ]) {
......@@ -28,7 +29,7 @@ module LX15D() finalize("gray") difference() {
-LX15D_middle_a, -LX15D_middle_a, LX15D_middle_t * 2
],
center = true);
for (i = [ -0.5, 0.5 ], j = [ -0.5, 0.5 ])
for (i = [ -0.5, 0.5 ]) for(j = [ -0.5, 0.5 ])
translate([ dim[0] * i, dim[1] * j, 0 ])
cylinder(d = 13, h = 100, center = true);
}
......@@ -40,7 +41,7 @@ module LX15D() finalize("gray") difference() {
LX15D_horn_screw_pos() translate([ 0, 0, i * LX15D_horn_w / 2 ])
cylinder(d = LX15D_horn_screw_d, h = 10, center = true);
}
for (m = [ 0, 1 ]) mirror([ 0, 0, m ]) for (i = [ -1, 1 ], j = [ -1, 1 ]) {
for (m = [ 0, 1 ]) mirror([ 0, 0, m ]) for (i = [ -1, 1 ]) for(j = [ -1, 1 ]) {
LX15D_body_screw_pos(i, j)
cylinder(d = LX15D_body_screw_d, h = 4, center = true);