Nao Link Definition

Obj[rule_, chain_List] := Fold[#2 /. #1 &, rule, chain];
Obj[rule_, args__] := Obj[rule, {args}];

(*How to detect if element member exists?*)

joints = {
   "Torso" -> {
     "Origin" :> {0, 0, 0}
     },
   "Head" -> {
     "Origin" :>
      Obj[joints, "Torso", "Origin"] + {0, 0, NeckOffsetZ},
     "Joint" -> {
       "Yaw" -> {0, 0, 1},
       "Pitch" -> {0, 1, 0}
       }
     },
   "RHip" -> {
     "Origin" :>
      Obj[joints, "Torso", "Origin"] + {0, -HipOffsetY, HipOffsetZ},
     "Joint" -> {
       "Pitch", {0, 1, 0},
       "Roll", {1, 0, 0},
       "YawPitch", {0, 1, 1}
       }
     },
   "RKnee" -> {
     "Origin" :>
      Obj[joints, "RHip", "Origin"] + {0, 0, -ThighLength},
     "Joint" -> {
       "Pitch" -> {0, 1, 0}
       }
     },
   "RAnkle" -> {
     "Origin" :> Obj[joints, "RKnee", "Origin"] + {0, 0, -TibiaLength},
     "Joint" -> {
       "Pitch" -> {0, 1, 0},
       "Roll" -> {1, 0, 0}
       }
     },
   "RShoulder" -> {
     "Origin" :>
      Obj[joints, "Torso", "Origin"] + {0, -ShoulderOffsetY,
        ShoulderOffsetZ},
     "Joint" -> {
       "Pitch" -> {0, 1, 0},
       "Roll" -> {0, 0, 1}
       }
     },
   "RElbow" -> {
     "Origin" :>
      Obj[joints, "RShoulder", "Origin"] + {UpperArmLength, 0, 0},
     "Joint" -> {
       "Roll" -> {0, 0, 1},
       "Yaw" -> {1, 0, 0}
       }
     },
   "RWrist" -> {
     "Origin" :>
      Obj[joints, "RElbow", "Origin"] + {LowerArmLength, 0, 0}
     }
   };

rightRegex = RegularExpression["^R([A-Z]\\w+)"];
RightQ[str_] := StringMatchQ[str, rightRegex]

For[i = 1, i < Length[joints], i++,
  oldName = joints[[i, 1]];
  If[RightQ[oldName],
   (*Make new joint*)
   newName = "L" <> StringTake[oldName, {2, -1}];
   (*newO="Origin"*{1,-1,1}/.joints[[i,2]];*)

   newO = Obj[joints, oldName, "Origin"]*{1, -1, 1};
   If[Position[joints[[i]], "Joint" -> _] != {},
    newJoints = Obj[joints, oldName, "Joint"];
    AppendTo[joints,
     newName -> {"Origin" -> newO, "Joints" -> newJoints}],
    AppendTo[joints, newName -> {"Origin" -> newO}]
    ]
   ]
  ];

pieces = {
   "Head" -> {
     "Origin" :> Obj[joints, "Head", "Origin"]
     },
   "Neck" -> {
     "Origin" :> Obj[joints, "Head", "Origin"]
     },
   "RBicep" -> {
     "Origin" :> Obj[joints, "RShoulder", "Origin"]
     },
   "RForearm" -> {
     "Origin" :> Obj[joints, "RElbow", "Origin"]
     },
   "RHand" -> {
     "Origin" :> Obj[joints, "RWrist", "Origin"]
     },
   "RPelvis" -> {
     "Origin" :> Obj[joints, "RHip", "Origin"]
     },
   "RHip" -> {
     "Origin" :> Obj[joints, "RHip", "Origin"]
     },
   "RThigh" -> {
     "Origin" :> Obj[joints, "RHip", "Origin"]
     },
   "RTibia" -> {
     "Origin" :> Obj[joints, "RKnee", "Origin"]
     },
   "RAnkle" -> {
     "Origin" :> Obj[joints, "RAnkle", "Origin"]
     },
   "RFoot" -> {
     "Origin" :> Obj[joints, "RAnkle", "Origin"]
     },
   (*Screw automation*)

   "LBicep" -> {"Origin" :> Obj[joints, "LShoulder", "Origin"]},
   "LForearm" -> {"Origin" :> Obj[joints, "LElbow", "Origin"]},
   "LHand" -> {"Origin" :> Obj[joints, "LWrist", "Origin"]},
   "LPelvis" -> {"Origin" :> Obj[joints, "LHip", "Origin"]},
   "LHip" -> {"Origin" :> Obj[joints, "LHip", "Origin"]},
   "LThigh" -> {"Origin" :> Obj[joints, "LHip", "Origin"]},
   "LTibia" -> {"Origin" :> Obj[joints, "LKnee", "Origin"]},
   "LAnkle" -> {"Origin" :> Obj[joints, "LAnkle", "Origin"]},
   "LFoot" -> {"Origin" :> Obj[joints, "LAnkle", "Origin"]}
   };

chains = {"Head" -> {"HeadYaw", "HeadPitch"},
   "RArm" -> {"RShoulderPitch", "RShoulderRoll", "RElbowYaw",
     "RElbowRoll", "RWristYaw"},
   "RLeg" -> {"RHipYawPitch", "RHipRoll", "RHipPitch", "RKneePitch",
     "RAnklePitch", "RAnkleRoll"},
   "LArm" -> {"LShoulderPitch", "LShoulderRoll", "LElbowYaw",
     "LElbowRoll", "LWristYaw"},
   "LLeg" -> {"LHipYawPitch", "LHipRoll", "LHipPitch", "LKneePitch",
     "LAnklePitch", "LAnkleRoll"}};