Commit 26c03eeb authored by Jan Koniarik's avatar Jan Koniarik
Browse files

rewrite

parent 880a9cda
import Control.Monad.Reader (runReader)
import Control.Monad.Reader ( runReader )
import Linear.Quaternion
import Linear.V3
import Numeric.Units.Dimensional.Prelude (degree, meter, radian,
second, (*~))
import qualified Numeric.Units.Dimensional.Prelude as D
import Numeric.Units.Dimensional.Prelude ( degree
, meter
, radian
, second
, (*~)
)
import qualified Numeric.Units.Dimensional.Prelude
as D
import SCAD
import Shared
import SRDF.Xml (writeSRDFXML)
import System.Directory (getCurrentDirectory)
import URDF.Xml (writeURDFXML)
import SRDF.Xml ( writeSRDFXML )
import System.Directory ( getCurrentDirectory, createDirectory, copyFile)
import URDF.Xml ( writeURDFXML )
import Walker
import Walker.Pkg
import Walker.SCAD
......@@ -20,45 +25,46 @@ import Walker.URDF
data LegPos = RL | RR | FR | FL deriving Eq;
data ConfType = Default | Folded | Expanded deriving (Show, Eq);
kokeTipPos = Pose (V3 (100 *~ mm) (0 *~ mm) (0 *~ mm) ) (toQuat zAxis (0 *~ degree))
kokeTipPos =
Pose (V3 (100 *~ 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
$ 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
angle = case pos of
FR -> 0
FL -> 0
RR -> 180
RL -> 180
baseAxis :: LegPos -> Axis
baseAxis pos = case pos of
baseAxis pos = case pos of
FR -> V3 (0 *~ mm) (0 *~ mm) ((-1000) *~ mm)
FL -> zAxis
RR -> zAxis
......@@ -67,196 +73,224 @@ baseAxis pos = case pos of
leg :: LegPos -> ConfType -> SimpleLeg
leg leg_pos conf_type = Leg
{ leg_name = case leg_pos of
RL -> "rl"
RR -> "rr"
FL -> "fl"
FR -> "fr"
{ 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
}
}
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
}
, 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
}
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
}
, 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
}
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
}
, 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 $ Joint
{ joint_name = "tipj"
, offset = kokeTipPos
, jtype = FixedJoint
, sub_link = Link { link_name = "tip", parts = [] }
, 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
}
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
}
}
}
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 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))
]
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
where
ewalker cwd conf_type = runReader (extendWalker $ koke conf_type) $ config cwd
createDirectory(pkg_dir conf ++ "/launch")
copyFile "launch/robot_description.launch" (pkg_dir conf ++ "/launch/robot_description.launch")
config cwd = PackageConfig {
pkg_name = "schpin_koke",
scad_dir = cwd ++ "/tmp/scad/",
pkg_dir = cwd ++ "/pkg/",
src_dir = cwd ++ "/src/scad/"
}
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/"
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment