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

Refactored for changes in robot lib

parent 723c7498
Pipeline #80121 passed with stage
in 10 minutes and 22 seconds
Hide whitespace changes
Inline Side-by-side
schpin_koke/src/Main.hs 100755 → 100644
View file @ 11d61ecf
import Language.OpenSCAD as LS
import Linear.Quaternion
import Linear.V3
import Numeric.Units.Dimensional.Prelude
......@@ -13,263 +12,249 @@ import qualified Numeric.Units.Dimensional.Prelude
as D
import Package
import Parser ( extractParts )
import SCAD
import Shared
import Schpin.SCAD
import Schpin.Shared
import System.IO.Unsafe
import Walker
import Schpin.Walker
data LegPos = RL | RR | FR | FL deriving Eq;
kokeTipPos :: Pose
kokeTipPos =
Pose (V3 (110 *~ mm) (0 *~ mm) (0 *~ mm)) (toQuat zAxis (0 *~ degree))
kokeTipPos = Pose (V3 (110 *~ mm) (0 *~ mm) (0 *~ mm)) neutralQuat
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 60
x = case pos of
FR -> x_val *~ mm
FL -> x_val *~ mm
RR -> (-x_val) *~ mm
RL -> (-x_val) *~ mm
y_val = if conf_type == Expanded then 85 else 52
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 60
x = case pos of
FR -> x_val *~ mm
FL -> x_val *~ mm
RR -> (-x_val) *~ mm
RL -> (-x_val) *~ mm
y_val = if conf_type == Expanded then 85 else 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
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)
actuatorName :: LegPos -> UrdfName -> String
actuatorName lp jname = "servo/" ++ (show id)
where
id :: Int
id = case lp of
RL -> case jname of
"alfa" -> 11
"beta" -> 12
"gama" -> 13
RR -> case jname of
"alfa" -> 21
"beta" -> 22
"gama" -> 23
FL -> case jname of
"alfa" -> 1
"beta" -> 2
"gama" -> 3
FR -> case jname of
"alfa" -> 21
"beta" -> 22
"gama" -> 23
where
id :: Int
id = case lp of
RL -> case jname of
"alfa" -> 11
"beta" -> 12
"gama" -> 13
RR -> case jname of
"alfa" -> 21
"beta" -> 22
"gama" -> 23
FL -> case jname of
"alfa" -> 1
"beta" -> 2
"gama" -> 3
FR -> case jname of
"alfa" -> 21
"beta" -> 22
"gama" -> 23
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
{ 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
, actuator_name = actuatorName leg_pos "alfa"
}
, sub_link = Link { link_name = "coxa"
, weight = 320 *~ gram
, assembly = Just coxa_assembly
}
, sub_joint = Just femur
}
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
, actuator_name = actuatorName leg_pos "alfa"
}
, sub_link = Link
{ link_name = "coxa"
, weight = 320 *~ gram
, 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
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
, actuator_name = actuatorName leg_pos "beta"
}
, sub_link = Link
{ link_name = "femur"
, weight = 50 *~ gram
, 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
}
coxa_assembly :: SimpleAssembly
coxa_assembly = 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"]
]
}
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
, actuator_name = actuatorName leg_pos "beta"
}
, sub_link = Link { link_name = "femur"
, weight = 50 *~ gram
, assembly = Just femur_assembly
}
, sub_joint = Just tibia
}
femur_assembly :: SimpleAssembly
femur_assembly = 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"]
]
}
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
, actuator_name = actuatorName leg_pos "gama"
}
, sub_link = Link
{ link_name = "tibia"
, weight = 170 *~ gram
, 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
tip = SimpleJoint $ Joint
{ joint_name = "tipj"
, offset = kokeTipPos
, jtype = FixedJoint
, sub_link = Link { link_name = "tipl"
, weight = 0 *~ gram
, assembly = Nothing
}
, sub_joint = Nothing
}
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
, actuator_name = actuatorName leg_pos "gama"
}
, sub_link = Link { link_name = "tibia"
, weight = 170 *~ gram
, assembly = Just tibia_assembly
}
, sub_joint = Just tip
}
tibia_assembly :: SimpleAssembly
tibia_assembly = 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"]
]
}
tip :: SimpleJoint
tip = SimpleJoint $ Joint
{ joint_name = "tipj"
, offset = kokeTipPos
, jtype = FixedJoint
, sub_link = Link { link_name = "tipl"
, weight = 0 *~ gram
, assembly = Nothing
}
, sub_joint = Nothing
}
koke :: ConfType -> SimpleWalker
koke conf_type = Walker
{ walker_name = "koke"
, legs = legs
, body = Body
{ body_name = "base_link"
, body_weight = 1000 *~ gram
, 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"]
]
}
}
{ walker_name = "koke"
, legs = legs
, body = Body
{ body_name = "base_link"
, body_weight = 1000 *~ gram
, 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
exportPackage koke
exportExtras koke
exportPackage koke
exportExtras koke
schpin_koke/src/Package.hs
View file @ 11d61ecf
......@@ -3,17 +3,17 @@ module Package where
import Control.Monad.Reader ( runReader )
import Parser
import SCAD
import Schpin.SCAD
import System.Directory ( copyFile
, createDirectory
, createDirectoryIfMissing
, getCurrentDirectory
)
import Walker
import Walker.Pkg
import Walker.SCAD
import Walker.SRDF
import Walker.URDF
import Schpin.Walker
import Schpin.Walker.Pkg
import Schpin.Walker.SCAD
import Schpin.Walker.SRDF
import Schpin.Walker.URDF
data ConfType = Default | Folded | Expanded deriving (Show, Eq);
......
schpin_koke/src/Parser.hs
View file @ 11d61ecf
......@@ -21,12 +21,12 @@ import Debug.Trace ( traceShow
, traceShowM
)
import qualified Language.OpenSCAD as LS
import qualified SCAD as SC
import Shared
import qualified Schpin.SCAD as SC
import Schpin.Shared
import System.Directory
import System.FilePath
import System.IO.Unsafe
import Walker
import Schpin.Walker
import Data.Hashable ( hash )
import Debug.Trace (traceShowId)
......
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