added leg config and udpates to joint config

src/Schpin/Walker.hs

@@ -89,13 +89,26 @@ data ExtendedLink = ExtendedLink
   }
   deriving Show
 
-data ActuatorType = Lewan { lw_goto_p :: Maybe Double, lw_goto_i :: Maybe Double, lw_goto_d :: Maybe Double,
-                            lw_vel_p :: Maybe Double, lw_vel_i :: Maybe Double, lw_vel_d :: Maybe Double
+data ActuatorType = Lewan { lw_goto_p :: Maybe Double
+                          , lw_goto_i :: Maybe Double
+                          , lw_goto_d :: Maybe Double
+                          , lw_vel_p :: Maybe Double
+                          , lw_vel_i :: Maybe Double
+                          , lw_vel_d :: Maybe Double
                           }
-                  | LewanTest
-                  | Dynamixel { dn_goto_p :: Maybe Double, dn_goto_i :: Maybe Double, dn_goto_d :: Maybe Double,
-                                dn_vel_p :: Maybe Double, dn_vel_i :: Maybe Double
-                            }
+                  | LewanTest { lwt_goto_p :: Maybe Double
+                              , lwt_goto_i :: Maybe Double
+                              , lwt_goto_d :: Maybe Double
+                              , lwt_vel_p :: Maybe Double
+                              , lwt_vel_i :: Maybe Double
+                              , lwt_vel_d :: Maybe Double
+                              }
+                  | Dynamixel { dn_goto_p :: Maybe Int
+                              , dn_goto_i :: Maybe Int
+                              , dn_goto_d :: Maybe Int
+                              , dn_vel_p :: Maybe Int
+                              , dn_vel_i :: Maybe Int
+                              }
                   deriving (Show);
 
 data Actuator = Actuator { ac_name :: String,
@@ -137,6 +150,7 @@ data ExtendedJoint = ExtendedJoint
 data Leg j = Leg
   { leg_name   :: LegName
   , root_joint :: j
+  , neutral_dir :: Axis 
   }
   deriving Show
 

src/Schpin/Walker/Config.hs

@@ -5,9 +5,10 @@ module Schpin.Walker.Config where
 import           Data.ByteString.Lazy.UTF8         (toString)
 import           Data.Maybe                        (catMaybes)
 import           Data.Scientific
+import           Data.Foldable                     (toList)
 import           Data.Text                         (pack)
 import           Data.YAML
-import           Numeric.Units.Dimensional.Prelude (radian, (/~))
+import           Numeric.Units.Dimensional.Prelude (radian, (/~), meter)
 import           Schpin.Package
 import           Schpin.Shared
 import           Schpin.Walker
@@ -15,7 +16,7 @@ import           Schpin.Walker
 acTypeToString :: ActuatorType -> String
 acTypeToString t = case t of
   Lewan {}     -> "LEWAN"
-  LewanTest    -> "LEWAN_TEST"
+  LewanTest {} -> "LEWAN_TEST"
   Dynamixel {} -> "DYNMX"
 
 writeControlConfig :: ExtendedWalker -> PackageConfig -> IO ()
@@ -38,9 +39,28 @@ writeControlConfig walker cfg = do
     [ pack "joint_state" .= pack "/control/joint_states"
     , pack "diag_freq" .= freq
     , pack "diag_topic" .= pack "/diagnostics"
+    , pack "legs" .= legsp
     , pack "servos" .= servos
     ]
 
+  legsp :: Node ()
+  legsp = mapping $ (\l -> (pack ("leg/" ++ leg_name l)) .= cfg l) <$>(legs walker)
+    where
+      cfg :: ExtendedLeg -> Node()
+      cfg Leg{root_joint, neutral_dir} = mapping $
+        [ pack "neutral_angles" .= ((\x -> x /~ radian) <$> neutral_angles root_joint)
+        , pack "neutral_dir" .= ((\x -> x /~ meter) <$> toList neutral_dir)
+        ]
+
+      neutral_angles :: ExtendedJoint -> [Angle]
+      neutral_angles root_joint = catMaybes $ mapJoints f root_joint
+        where
+          f :: ExtendedJoint -> Maybe Angle
+          f ExtendedJoint{joint=Joint{jtype}} = case jtype of
+              RevoluteJoint{default_angle} -> Just $ default_angle
+              _ -> Nothing
+
+
   servos :: Node ()
   servos =
     mapping
@@ -75,7 +95,14 @@ writeControlConfig walker cfg = do
                 , opt_arg "vel_i" $ lw_vel_i atype
                 , opt_arg "vel_d" $ lw_vel_d atype
                       ]
-             LewanTest -> []
+             LewanTest {} -> catMaybes  [
+                 opt_arg "goto_p" $ lwt_goto_p atype
+                , opt_arg "goto_i" $ lwt_goto_i atype
+                , opt_arg "goto_d" $ lwt_goto_d atype
+                , opt_arg "vel_p" $ lwt_vel_p atype
+                , opt_arg "vel_i" $ lwt_vel_i atype
+                , opt_arg "vel_d" $ lwt_vel_d atype
+                      ]
              Dynamixel {} -> catMaybes
               [ opt_arg "goto_p" $ dn_goto_p atype
               , opt_arg "goto_i" $ dn_goto_i atype