538 lines (460 with data), 16.1 kB

//---------------------------------------------------------------
// Chair package
//---------------------------------------------------------------

// AnyFolder &RefPP = Main.PackageParameters;

//------------------------------------------------------
// Global Reference Frame
//------------------------------------------------------ 



AnyFixedRefFrame GlobalRef = {
  
  AnyFolder &Dim = .Seg.ChairDimensions;
  
  AnyRefNode GroundNode = {
    sRel = {1/2*.Dim.SeatLength, -.Dim.BackRestHeight, 0};
  };  
  
  
  /*
  AnyRefNode CoMNode = {
  sRel = {0.069568, -0.589986, -0.000000};
  AnyDrawRefFrame DrwRef = {
  RGB = {0, 1, 0};
  ScaleXYZ = {0.3, 0.3, 0.3};
  };  
  };
  */
  
};  // Global reference frame

//-----------------------------------------------------
// Segments
//-----------------------------------------------------

AnyFolder Seg = {
  
  AnyFolder ChairDimensions = {
    
    AnyVar FootRestLength = 0.50;
    AnyVar FootRestWidth = 0.40;
    AnyVar FootRestThickness = 0.05;
    AnyVar BackRestHeight = 0.90;
    AnyVar BackRestWidth = 0.40;
    AnyVar BackRestThickness = 0.05;
    AnyVar SeatLength = 0.50;
    AnyVar SeatWidth = 0.40;
    AnyVar SeatThickness = 0.05;
    AnyVar HeadRestHeight = 0.2;
    AnyVar HeadRestWidth = 0.2;
    AnyVar HeadRestThickness = 0.05;
    AnyVar LegRestLength = 0.6;
    AnyVar LegRestWidth = 0.3;
    AnyVar LegRestThickness = 0.05;
    AnyVar ArmRestX = 0.30;
    AnyVar ArmRestY = 0.3;
    AnyVar ArmRestZ = 0.05;      
    AnyVar ArmRestHeight = 0.32;    
  }; // ChairDimensions  
  
  //--------------------------------------------------------
  // BackRest
  //--------------------------------------------------------
  
  AnySeg BackRest = {
    
    AnyFolder &Dim = .ChairDimensions;
    Mass = 0.0;
    AnyVar Ixx = 1/12*(Dim.BackRestHeight^2+Dim.BackRestWidth^2);
    AnyVar Iyy = 1/12*(Dim.BackRestThickness^2+Dim.BackRestWidth^2);
    AnyVar Izz = 1/12*(Dim.BackRestHeight^2+Dim.BackRestThickness^2);
    Jii = {Ixx, Iyy, Izz};

    AnyRefNode BackRestSeatJntNode = {
      sRel = {0, -1/2*.Dim.BackRestHeight, 0};
    };
    
    AnyRefNode BackRestHeadRestJntNode = {
      sRel = {0, 1/2*.Dim.BackRestHeight, 0};
    };
     AnyRefNode BackRestSupportNode = {
      sRel = {-0.2, -0.2, 0};
    };
    
    AnyRefNode BackArmRestRightJntNode = {
      sRel = {0, .Dim.ArmRestHeight-1/2*.Dim.BackRestHeight, 1/2*.Dim.BackRestWidth};
    };
    
    AnyRefNode BackArmRestLeftJntNode = {
      sRel = {0, .Dim.ArmRestHeight-1/2*.Dim.BackRestHeight, -1/2*.Dim.BackRestWidth};
    };
        
    AnyDrawSTL BackRest = {
      FileName = "../Input/BackRest.stl";
      RGB = {0, 0, 1};
      Opacity = 0.2;
        AnyStyleDrawMaterial1 style = 
        {
          EnableCreasing = On;
          CreasingAngle = 0.524;
          SpecularRGB = 0.05*{1,1,1};
        };      
    };
  }; // BackRest        
  
  //--------------------------------------------------------
  // Seat
  //--------------------------------------------------------
  
  AnySeg Seat = {
    
    AnyFolder &Dim = .ChairDimensions;
    Mass = 0.0;
    AnyVar Ixx = 1/12*(Dim.SeatWidth^2+Dim.SeatThickness^2);
    AnyVar Iyy = 1/12*(Dim.SeatLength^2+Dim.SeatWidth^2);
    AnyVar Izz = 1/12*(Dim.SeatLength^2+Dim.SeatThickness^2);
    Jii = {Ixx, Iyy, Izz};
    AnyRefNode SeatNode = {
      sRel = {0, 0, 0};
      ARel = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 1}};
      
      AnyDrawSTL Seat = {
        FileName = "../Input/Seat.stl";
        RGB = {0, 0, 1};
        Opacity = 0.2;
        AnyStyleDrawMaterial1 style = 
        {
          EnableCreasing = On;
          CreasingAngle = 0.524;
          SpecularRGB = 0.05*{1,1,1};
        };
      };
      
    };
    
    AnyRefNode SeatBackRestJntNode = {
      sRel = {0, 1/2*.Dim.SeatLength, 0};
    };
    
    AnyRefNode SeatAdjNode = {
      sRel = {0, 0, 0};
    };
    
    AnyRefNode SeatLegRestJntNode = {
      sRel = {0, -1/2*.Dim.SeatLength, 0};
    };    
  }; // Seat 
  
  //-----------------------------------------------------
  // HeadRest
  //-----------------------------------------------------
  
  AnySeg HeadRest = {
    
    AnyFolder &Dim = .ChairDimensions;
    Mass = 0.0;
    AnyVar Ixx = 1/12*(Dim.HeadRestHeight^2+Dim.HeadRestWidth^2);
    AnyVar Iyy = 1/12*(Dim.HeadRestWidth^2+Dim.HeadRestThickness^2);
    AnyVar Izz = 1/12*(Dim.HeadRestHeight^2+Dim.HeadRestThickness^2);
    Jii = {Ixx, Iyy, Izz};
    
    AnyRefNode HeadRestBackRestJntNode = {
      sRel = {0, -1/2*.Dim.HeadRestHeight, 0};
      // AnyDrawNode DrwNode = {};
    };
    
    // AnyDrawSeg HeadRest = {};
    /*
    AnyDrawSTL HeadRest = {
    FileName = "../Input/HeadRest.stl";
    RGB = {0, 0, 1};
    Opacity = 0.4;      
    };
    */
  }; // HeadRest
  
  //-------------------------------------------------------
  // LegRest
  //-------------------------------------------------------
  
  AnySeg LegRest = {
    AnyFolder &Dim = .ChairDimensions;
    Mass = 0.0;
    AnyVar Ixx = 1/12*(Dim.LegRestLength^2+Dim.LegRestWidth^2);
    AnyVar Iyy = 1/12*(Dim.LegRestWidth^2+Dim.LegRestThickness^2);
    AnyVar Izz = 1/12*(Dim.LegRestLength^2+Dim.LegRestThickness^2);
    Jii = {Ixx, Iyy, Izz};
    
    AnyRefNode LegRestSeatJntNode = {
      sRel = {0, 1/2*.Dim.LegRestLength, 0};
    };
          
    AnyRefNode LegRestFootRestJntNode = {
      sRel = {0, -1/2*.Dim.LegRestLength, 0};
    };    
    
    AnyDrawSTL LegRest = {
      FileName = "../Input/LegRest.stl";       
      RGB = {0, 0, 1};
      Opacity = 0.2;
        AnyStyleDrawMaterial1 style = 
        {
          EnableCreasing = On;
          CreasingAngle = 0.524;
          SpecularRGB = 0.05*{1,1,1};
        };      
    };
    
  }; // LegRest 
  
  //---------------------------------------------------
  // FootRest
  //---------------------------------------------------
  
  AnySeg FootRest = {
    
    AnyFolder &Dim = .ChairDimensions;
    Mass = 5.0;
    AnyVar Ixx = 1/12*(Dim.FootRestThickness^2+Dim.FootRestWidth^2);
    AnyVar Iyy = 1/12*(Dim.FootRestLength^2+Dim.FootRestWidth^2);
    AnyVar Izz = 1/12*(Dim.FootRestLength^2+Dim.FootRestThickness^2);
    Jii = {Ixx, Iyy, Izz};     
    
    AnyRefNode FootRestLegRestJntNode = {
      sRel = {0, 1/2*.Dim.FootRestLength, 0};
      ARel = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 1}};
      /*
      AnyDrawRefFrame DrwRefFrame = {
      RGB = {1, 1, 0};
      ScaleXYZ = {0.3, 0.3, 0.3};
      };
      */
    };
    // AnyDrawSeg DrwSeg = {};
    
    AnyDrawSTL DrwSTL = {
      FileName = "../Input/FootRest2.stl";
      RGB = {0,0, 1};
      Opacity = 0.2;
        AnyStyleDrawMaterial1 style = 
        {
          EnableCreasing = On;
          CreasingAngle = 0.524;
          SpecularRGB = 0.05*{1,1,1};
        };      
    };
    
  }; // FootRest    
  
//  //---------------------------------------------------
//  // ArmRestRight
//  //---------------------------------------------------
//  
//  AnyFolder Right = {
//    
//    AnySeg ArmRest = {
//      
//      AnyFolder &Dim = ..ChairDimensions;
//      Mass = 1.0;
//      AnyVar Ixx = 1/12*(Dim.ArmRestZ^2+Dim.ArmRestX^2);
//      AnyVar Iyy = 1/12*(Dim.ArmRestY^2+Dim.ArmRestX^2);
//      AnyVar Izz = 1/12*(Dim.ArmRestY^2+Dim.ArmRestZ^2);
//      Jii = {Ixx, Iyy, Izz};     
//      
//      AnyRefNode BackArmRestJntNode = {
//        sRel = {0, .Dim.ArmRestX, -1/2*.Dim.ArmRestZ};       
//      };
//
//      AnyRefNode ArmRestUlnaJntNodeFront = {
//        sRel = {0, -1/2*.Dim.ArmRestY, 0};//-1/2*.Dim.ArmRestZ};
//      };
//
//      AnyRefNode ArmRestUlnaJntNodeBack = {
//        sRel = {0, 1/2*.Dim.ArmRestY, 0};//-1/2*.Dim.ArmRestZ};
//      };
//
////      AnyDrawSeg DrwSeg = {};
//      
//      AnyRefNode STLholder = {
//        sRel = {-0.45, 0, 0};
//        AnyDrawSTL DrwSTL = {
//          FileName = "../Input/ArmRest3.stl";
//          RGB = {0, 0, 1};
//          Opacity = 0.2;
//        };
//      };     
//    }; // ArmRest    
//    
//  }; // Right
//  
//  AnyFolder Left = {
//    
//    AnySeg ArmRest = {
//      
//      AnyFolder &Dim = ..ChairDimensions;
//      Mass = 1.0;
//      AnyVar Ixx = 1/12*(Dim.ArmRestZ^2+Dim.ArmRestX^2);
//      AnyVar Iyy = 1/12*(Dim.ArmRestY^2+Dim.ArmRestX^2);
//      AnyVar Izz = 1/12*(Dim.ArmRestY^2+Dim.ArmRestZ^2);
//      Jii = {Ixx, Iyy, Izz};     
//      
//      AnyRefNode BackArmRestJntNode = {
//        sRel = {0, .Dim.ArmRestX, -1/2*.Dim.ArmRestZ};       
//      };
//
//      AnyRefNode ArmRestUlnaJntNodeFront = {
//        sRel = {0, -1/2*.Dim.ArmRestY, 0};//-1/2*.Dim.ArmRestZ};
//      };
//
//      AnyRefNode ArmRestUlnaJntNodeBack = {
//        sRel = {0, 1/2*.Dim.ArmRestY, 0};//-1/2*.Dim.ArmRestZ};
//      };
//
////      AnyDrawSeg DrwSeg = {};
//      
//      AnyRefNode STLholder = {
//        sRel = {-0.45, 0, 0};
//        AnyDrawSTL DrwSTL = {
//          FileName = "../Input/ArmRest3.stl";
//          RGB = {0, 0, 1};
//          Opacity = 0.2;
//        };
//      };     
//    }; // ArmRest    
//    
//  }; // Left
  
}; // Seg

//--------------------------------------
// Joints
//--------------------------------------

AnyFolder Jnt = {
  
  AnyKinLinear SeatGlobalLinMeasure = {
    AnyRefNode &SegNode = ..Seg.Seat.SeatAdjNode;
    AnyRefNode &GroundNode = ..GlobalRef.GroundNode;
  };        
  
  AnyKinRotational SeatGlobalRotMeasure = {
    AnyRefNode &SegNode = ..Seg.Seat.SeatAdjNode;
    AnyRefNode &GroundNode = ..GlobalRef.GroundNode;
    Type = RotVector;
  };        
  
  AnyKinEq SeatGlobalConst = {
    AnyKinLinear &LinMeasure = .SeatGlobalLinMeasure;
    AnyKinRotational &RotMeasure = .SeatGlobalRotMeasure;
    MeasureOrganizer = {0, 2, 3, 4};
    Reaction.Type = {Off, Off, Off, Off};
  };
  
  // Create reaction forces between the seat and the environment
  AnyReacForce SeatSupport = {
    AnyKinLinear SeatGlobalLinMeasure = {
      AnyRefFrame &Seat = ...Seg.Seat;
      AnyRefFrame &Ground = ...GlobalRef;
      Ref = 0;  // In the seat reference frame
    };
    AnyKinRotational &Rot = .SeatGlobalRotMeasure;
  };
  
  
  AnyRevoluteJoint SeatBackRestJnt = {
    Axis = z;
    AnyRefNode &Seatnode = ..Seg.Seat.SeatBackRestJntNode;  
    AnyRefNode &BackRestNode = ..Seg.BackRest.BackRestSeatJntNode;
    Constraints.Reaction.Type = {Off, Off, Off, Off, Off};
  }; // SeatBackRestLink
  
  AnyRevoluteJoint BackRestHeadRestJnt = {
    Axis = z;
    AnyRefNode &BackRestNode = ..Seg.BackRest.BackRestHeadRestJntNode;
    AnyRefNode &HeadRestNode = ..Seg.HeadRest.HeadRestBackRestJntNode;
  }; // BackRestHeadRestLink
  
  AnyRevoluteJoint SeatLegRestJnt = {
    Axis = z; 
    AnyRefNode &LegRestNode = ..Seg.LegRest.LegRestSeatJntNode;
    AnyRefNode &SeatNode = ..Seg.Seat.SeatLegRestJntNode;
    Constraints.Reaction.Type = {Off, Off, Off, Off, Off};    
  }; // SeatLegRestLink
  
  AnyPrismaticJoint LegRestFootRestJnt = {
    Axis = y;
    Ref = 1;
    AnyRefNode &LegRestNode = ..Seg.LegRest.LegRestFootRestJntNode;
    AnyRefNode &FootRestNode = ..Seg.FootRest.FootRestLegRestJntNode;
  };
  
  // Additional reactions to tie the backrest and legrest to ground
  AnyReacForce BackRestGrounding = {
    AnyKinLinear Lin = {
      AnyRefFrame &Ground = ...GlobalRef;
      AnyRefFrame &BackRest = ...Seg.BackRest;
    };
    AnyKinRotational Rot = {
      AnyRefFrame &Ground = ...GlobalRef;
      AnyRefFrame &BackRest = ...Seg.BackRest;
      Type = RotAxesAngles;
    };
  };

  AnyReacForce LegRestGrounding = {
    AnyKinLinear Lin = {
      AnyRefFrame &Ground = ...GlobalRef;
      AnyRefFrame &LegRest = ...Seg.LegRest;
    };
    AnyKinRotational Rot = {
      AnyRefFrame &Ground = ...GlobalRef;
      AnyRefFrame &LegRest = ...Seg.LegRest;
      Type = RotAxesAngles;
    };
  };
  
  // Arm rests  
//  AnyFolder Right = {
//    
//    AnyRevoluteJoint BackArmRestJoint = {
//      AnyRefNode &BackNode = ...Seg.BackRest.BackArmRestRightJntNode;
//      AnyRefNode &ArmRestNode = ...Seg.Right.ArmRest.BackArmRestJntNode;
//      Ref = 1;
//    };        
//    
//    AnyKinRotational GlobalArmRestRotMeasure = {
//      AnyRefFrame &Global = Main.Model.EnvironmentModel.GlobalRef;
//      AnyRefNode &ArmRestNode = ...Seg.Right.ArmRest.BackArmRestJntNode;
//      Type = RotVector;
//    };        
//    
//    AnyKinEqSimpleDriver GlobalArmRestAngle = {
//      AnyKinRotational &RotMeasure = .GlobalArmRestRotMeasure;
//      MeasureOrganizer = {2};
//      DriverPos = {90*pi/180};
//      DriverVel = {0};
//    };
//            
//  }; // Right
//  
//  AnyFolder Left = {
//    
//    AnyRevoluteJoint BackArmRestJoint = {
//      AnyRefNode &BackNode = ...Seg.BackRest.BackArmRestLeftJntNode;
//      AnyRefNode &ArmRestNode = ...Seg.Left.ArmRest.BackArmRestJntNode;
//      Ref = 1;
//    };        
//    
//    AnyKinRotational GlobalArmRestRotMeasure = {
//      AnyRefFrame &Global = Main.Model.EnvironmentModel.GlobalRef;
//      AnyRefNode &ArmRestNode = ...Seg.Left.ArmRest.BackArmRestJntNode;
//      Type = RotVector;
//    };        
//    
//    AnyKinEqSimpleDriver GlobalArmRestAngle = {
//      AnyKinRotational &RotMeasure = .GlobalArmRestRotMeasure;
//      MeasureOrganizer = {2};
//      DriverPos = {90*pi/180};
//      DriverVel = {0};
//    };
//  }; // Left    
  
}; // Joints


AnyFolder Drivers={
 

  AnyFolder &DrvPos = Main.DrvPos;
  AnyFolder &DrvVel = Main.DrvVel;
  
  
//--------------------------------------------------------
  // Drivers - Chair adjustments
  //--------------------------------------------------------
  
//  AnyKinEqSimpleDriver SeatBackRestDrv = {
//    AnyRevoluteJoint &Jnt = ..Jnt.SeatBackRestJnt;
//    DriverPos = {.DrvPos.SeatBackRestRotZPos};
//    DriverVel = {.DrvVel.SeatBackRestRotZVel};
//    Reaction.Type = {On};
//  };
  
  AnyKinEqSimpleDriver GlobalBackRestDrv = {
    AnyKinRotational rot = {
      AnyRefFrame &Back = Main.Model.EnvironmentModel.Seg.BackRest;
      Type = RotAxesAngles;
    };
    MeasureOrganizer = {0};
    DriverPos = {.DrvPos.GlobalBackRestRotZPos*pi/180};
    DriverVel = {.DrvVel.GlobalBackRestRotZVel*pi/180};
    Reaction.Type = {Off};
  };  
  
  AnyKinEqSimpleDriver SeatGlobalDrv = {
    AnyKinLinear &LinMeasure = ..Jnt.SeatGlobalLinMeasure;
    AnyKinRotational &RotMeasure = ..Jnt.SeatGlobalRotMeasure;
    MeasureOrganizer = {1,5};
    DriverPos = {.DrvPos.SeatGlobalLinYPos,(-90+.DrvPos.SeatGlobalRotZPos)*pi/180};
    DriverVel = {.DrvVel.SeatGlobalLinYVel,.DrvVel.SeatGlobalRotZVel*pi/180};
    Reaction.Type = {Off,Off};
  };
  
  AnyKinEqSimpleDriver BackRestHeadRestDrv = {
    AnyRevoluteJoint &Jnt = ..Jnt.BackRestHeadRestJnt;
    DriverPos = {.DrvPos.BackRestHeadResRotZPos*pi/180};
    DriverVel = {0};
    Reaction.Type = {On};
  };  
  
//  AnyKinEqSimpleDriver SeatLegRestDrv = {
//    AnyRevoluteJoint &Jnt = ..Jnt.SeatLegRestJnt;
//    DriverPos = {.DrvPos.SeatLegRestRotZPos};
//    DriverVel = {.DrvVel.SeatLegRestRotZVel};
//    Reaction.Type = {On};
//  };

  AnyKinEqSimpleDriver GlobalLegRestDrv = {
    AnyKinRotational rot = {
      AnyRefFrame &LegRest = Main.Model.EnvironmentModel.Seg.LegRest;
      Type = RotAxesAngles;
    };
    MeasureOrganizer = {0};
    DriverPos = {.DrvPos.GlobalLegRestRotZPos*pi/180};
    DriverVel = {.DrvVel.GlobalLegRestRotZVel*pi/180};
    Reaction.Type = {Off};
  };
  
  AnyKinEqSimpleDriver LegRestFootRestDrv = {
    AnyPrismaticJoint &Jnt = ..Jnt.LegRestFootRestJnt;
    DriverPos = {.DrvPos.LegRestFootRestLinYPos};
    DriverVel = {.DrvVel.LegRestFootRestLinYVel};
    Reaction.Type = {On};
  };
};