diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100755 index e00d1afc..00000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,498 +0,0 @@ -# Minimum required CMake version -cmake_minimum_required(VERSION 3.18) - -# Project name -project(RAPTOR - DESCRIPTION "Rapid and Robust Trajectory Optimization for Robots") - -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() - -# set(CMAKE_CXX_FLAGS "-Wall -Wextra") -set(CMAKE_CXX_FLAGS_DEBUG "-g -fopenmp") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fopenmp") - -# Set C++ standard -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED True) - -# Find Python library -add_subdirectory(extern/pybind) - -# Find Boost library -find_package(Boost REQUIRED COMPONENTS - system - filesystem - serialization -) -include_directories(${Boost_INCLUDE_DIRS}) -link_directories(${Boost_LIBRARY_DIRS}) - -# Find GSL library -find_package(GSL REQUIRED) -include_directories(${GSL_INCLUDE_DIRS}) -link_directories(${GSL_LIBRARY_DIRS}) - -# Find Eigen library -find_package(Eigen3 3.3.7 REQUIRED NO_MODULE) -include_directories(${EIGEN3_INCLUDE_DIR}) - -find_package(pinocchio REQUIRED) -include_directories(${PINOCCHIO_INCLUDE_DIRS}) -link_directories(${PINOCCHIO_LIBRARY_DIRS}) - -set(PINOCCHIO_FLAGS - ${pinocchio_CFLAGS_OTHER} - -Wno-ignored-attributes - -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor - -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR - -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -) - -# Find YAML library -find_package(yaml-cpp REQUIRED) -include_directories(${YAML_CPP_INCLUDE_DIR}) -link_directories(${YAML_CPP_LIBRARY_DIR}) - -# Find Ipopt library -set(IPOPT_INCLUDE_DIR /usr/local/include/coin-or) -set(IPOPT_LIBRARY_DIR /usr/local/lib) -include_directories(${IPOPT_INCLUDE_DIR}) -link_directories(${IPOPT_LIBRARY_DIR}) - -# Find HSL library -set(HSL_INCLUDE_DIR /usr/local/include/coin/ThirdParty) -set(HSL_LIBRARY_DIR /usr/local/lib) -include_directories(${HSL_INCLUDE_DIR}) -link_directories(${HSL_LIBRARY_DIR}) - -# Find pardiso library -set(PARDISO_INCLUDE_DIR /usr/local/include) -set(PARDISO_LIBRARY_DIR /usr/local/lib) -include_directories(${PARDISO_INCLUDE_DIR}) -link_directories(${PARDISO_LIBRARY_DIR}) - -include_directories(Utils/include) - -## Trajectories library -add_library(trajlib SHARED - Trajectories/src/Trajectories.cpp - Trajectories/src/FourierCurves.cpp - Trajectories/src/FixedFrequencyFourierCurves.cpp - Trajectories/src/BezierCurves.cpp - Trajectories/src/ArmourBezierCurves.cpp - Trajectories/src/WaitrBezierCurves.cpp - Trajectories/src/Polynomials.cpp - Trajectories/src/Plain.cpp - Trajectories/src/TrajectoryGroup.cpp) - -target_include_directories(trajlib PUBLIC - Trajectories/include) - -include_directories(Trajectories/include) - -## Inverse dynamics library -add_library(IDlib SHARED - KinematicsDynamics/src/InverseDynamics.cpp - KinematicsDynamics/src/CustomizedInverseDynamics.cpp - KinematicsDynamics/src/RegressorInverseDynamics.cpp - KinematicsDynamics/src/DynamicsConstraints.cpp - KinematicsDynamics/src/ConstrainedInverseDynamics.cpp - KinematicsDynamics/src/Transform.cpp - KinematicsDynamics/src/Spatial.cpp - KinematicsDynamics/src/ForwardKinematics.cpp) - -target_include_directories(IDlib PUBLIC - KinematicsDynamics/include) - -include_directories(KinematicsDynamics/include) - -## Constraints library -add_library(Conslib SHARED - Constraints/src/Constraints.cpp - Constraints/src/JointLimits.cpp - Constraints/src/VelocityLimits.cpp - Constraints/src/ConstrainedJointLimits.cpp - Constraints/src/TorqueLimits.cpp - Constraints/src/SurfaceContactConstraints.cpp - Constraints/src/WaitrContactConstraints.cpp - Constraints/src/LieSpaceResidual.cpp - Constraints/src/KinematicsConstraints.cpp - Constraints/src/BoxCollisionAvoidance.cpp) - # Constraints/src/ZonotopeCollisionAvoidance.cpp - # Constraints/src/fclCollisionAvoidance.cpp) - -target_include_directories(Conslib PUBLIC - Constraints/include) - -include_directories(Constraints/include) - -## Constraints library -add_library(Optlib SHARED - Optimization/src/Optimizer.cpp) - -target_include_directories(Optlib PUBLIC - Optimization/include) - -include_directories(Optimization/include) - -## Digit library -add_library(Digitlib SHARED - Examples/Digit/src/DigitDynamicsConstraints.cpp - Examples/Digit/src/DigitConstrainedInverseDynamics.cpp - Examples/Digit/src/DigitSingleStepOptimizer.cpp - Examples/Digit/src/DigitMultipleStepOptimizer.cpp - Examples/Digit/src/DigitCustomizedConstraints.cpp - Examples/Digit/src/DigitSingleStepPeriodicityConstraints.cpp - Examples/Digit/src/DigitMultipleStepPeriodicityConstraints) - -target_link_libraries(Digitlib PUBLIC - trajlib - IDlib - Conslib - Optlib - ipopt - coinhsl - ${GSL_LIBRARIES} - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_include_directories(Digitlib PUBLIC - Examples/Digit/include) - -## Digit-modified library -add_library(DigitModifiedlib SHARED - Examples/Digit-modified/src/DigitModifiedDynamicsConstraints.cpp - Examples/Digit-modified/src/DigitModifiedConstrainedInverseDynamics.cpp - Examples/Digit-modified/src/DigitModifiedSingleStepOptimizer.cpp - Examples/Digit-modified/src/DigitModifiedCustomizedConstraints.cpp - Examples/Digit-modified/src/DigitModifiedSingleStepPeriodicityConstraints.cpp) - -target_link_libraries(DigitModifiedlib PUBLIC - trajlib - IDlib - Conslib - Optlib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_include_directories(DigitModifiedlib PUBLIC - Examples/Digit-modified/include) - -# Kinova libraries -add_library(Kinovalib SHARED - Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp - Examples/Kinova/ArmourUnsafe/src/KinovaOptimizer.cpp - Examples/Kinova/ArmourUnsafe/src/KinovaCustomizedConstraints.cpp - Examples/Kinova/WaitrUnsafe/src/KinovaWaitrOptimizer.cpp - Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp - Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp) - -target_link_libraries(Kinovalib PUBLIC - trajlib - IDlib - Conslib - Optlib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_include_directories(Kinovalib PUBLIC - Examples/Kinova - Examples/Kinova/InverseKinematics - Examples/Kinova/ArmourUnsafe/include - Examples/Kinova/WaitrUnsafe/include - Examples/Kinova/SystemIdentification/DataFilter/include - Examples/Kinova/SystemIdentification/ExcitingTrajectories/include - Examples/Kinova/SystemIdentification/IterativeSystemIdentification/include) - -## Example scripts -add_executable(Digit_example - Examples/Digit/DigitSingleStep.cpp) - # Examples/Digit/DigitSingleStepSpeedTest.cpp) - # Examples/Digit/DigitSingleStepRobustnessTest.cpp) - -target_include_directories(Digit_example PUBLIC - Examples/Digit/include - KinematicsDynamics/include - Trajectories/include - Constraints/include - Optimization/include - Utils/include) - -target_link_libraries(Digit_example PUBLIC - trajlib - IDlib - Conslib - Optlib - Digitlib - ipopt - coinhsl - yaml-cpp - ${GSL_LIBRARIES} - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(Digit_example PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(DigitMultipleStep_example - Examples/Digit/DigitMultipleStep.cpp) - -target_include_directories(DigitMultipleStep_example PRIVATE - Examples/Digit/include - KinematicsDynamics/include - Trajectories/include - Constraints/include - Optimization/include - Utils/include) - -target_link_libraries(DigitMultipleStep_example PRIVATE - trajlib - IDlib - Conslib - Optlib - Digitlib - ipopt - coinhsl - yaml-cpp - ${GSL_LIBRARIES} - ${BOOST_LIBRARIES} - ${PKG_PIN_CONFIG_LIBRARIES}) - -target_compile_options(DigitMultipleStep_example PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(DigitModified_example - Examples/Digit-modified/DigitModifiedSingleStep.cpp) - # Examples/Digit-modified/DigitModifiedSingleStepSpeedTest.cpp) - # Examples/Digit-modified/DigitModifiedSingleStepRobustnessTest.cpp) - -target_include_directories(DigitModified_example PUBLIC - Examples/Digit-modified/include) - -target_link_libraries(DigitModified_example PUBLIC - trajlib - IDlib - Conslib - Optlib - DigitModifiedlib - ipopt - coinhsl - yaml-cpp - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(DigitModified_example PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(KinovaIK_example - Examples/Kinova/InverseKinematics/KinovaIKExample.cpp) - -target_include_directories(KinovaIK_example PUBLIC - Examples/Kinova - Examples/Kinova/InverseKinematics) - -target_link_libraries(KinovaIK_example PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(KinovaIK_example PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(Kinova_example - Examples/Kinova/ArmourUnsafe/KinovaExample.cpp) - -target_include_directories(Kinova_example PUBLIC - Examples/Kinova - Examples/Kinova/ArmourUnsafe/include) - -target_link_libraries(Kinova_example PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(Kinova_example PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(KinovaWaitr_example - Examples/Kinova/WaitrUnsafe/KinovaWaitrExample.cpp) - -target_include_directories(KinovaWaitr_example PUBLIC - Examples/Kinova - Examples/Kinova/WaitrUnsafe/include) - -target_link_libraries(KinovaWaitr_example PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(KinovaWaitr_example PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(KinovaFilter_example - Examples/Kinova/SystemIdentification/DataFilter/KinovaAccelerationEstimator.cpp) - -target_include_directories(KinovaFilter_example PUBLIC - Examples/Kinova - Examples/Kinova/SystemIdentification/DataFilter/include - Examples/Kinova/SystemIdentification/ExcitingTrajectories/include - Examples/Kinova/SystemIdentification/IterativeSystemIdentification/include) - -target_link_libraries(KinovaFilter_example PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES}) - -add_executable(KinovaSysId_example - Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExample.cpp) - -target_include_directories(KinovaSysId_example PUBLIC - Examples/Kinova - Examples/Kinova/SystemIdentification/DataFilter/include - Examples/Kinova/SystemIdentification/ExcitingTrajectories/include - Examples/Kinova/SystemIdentification/IterativeSystemIdentification/include) - -target_link_libraries(KinovaSysId_example PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(KinovaSysId_example PUBLIC - ${PINOCCHIO_FLAGS}) - -pybind11_add_module(oracle_pybind SHARED - Examples/Kinova/ArmourUnsafe/KinovaPybind.cpp - Examples/Kinova/ArmourUnsafe/src/KinovaPybindWrapper.cpp) - -target_include_directories(oracle_pybind PUBLIC - Examples/Kinova - Examples/Kinova/ArmourUnsafe/include) - -target_link_libraries(oracle_pybind PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio - ${PYTHON_LIBRARIES}) - -target_compile_options(oracle_pybind PUBLIC - ${PINOCCHIO_FLAGS}) - -set_property(TARGET oracle_pybind PROPERTY POSITION_INDEPENDENT_CODE ON) - -pybind11_add_module(oracle_waitr_pybind SHARED - Examples/Kinova/WaitrUnsafe/KinovaWaitrPybind.cpp - Examples/Kinova/WaitrUnsafe/src/KinovaWaitrPybindWrapper.cpp) - -target_include_directories(oracle_waitr_pybind PUBLIC - Examples/Kinova - Examples/Kinova/WaitrUnsafe/include) - -target_link_libraries(oracle_waitr_pybind PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio - ${PYTHON_LIBRARIES}) - -target_compile_options(oracle_waitr_pybind PUBLIC - ${PINOCCHIO_FLAGS}) - -set_property(TARGET oracle_waitr_pybind PROPERTY POSITION_INDEPENDENT_CODE ON) - -## Test scripts -add_executable(ForwardKinematics_test - Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp) - -target_link_libraries(ForwardKinematics_test PUBLIC - IDlib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(ForwardKinematics_test PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(ForwardKinematicsGradient_test - Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) - -target_link_libraries(ForwardKinematicsGradient_test PUBLIC - IDlib - Optlib - Conslib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(ForwardKinematicsGradient_test PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(KinematicsConstraints_test - Tests/Constraints/TestKinematicsConstraints.cpp) - -target_link_libraries(KinematicsConstraints_test PUBLIC - trajlib - IDlib - Conslib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(KinematicsConstraints_test PUBLIC - ${PINOCCHIO_FLAGS}) - -## Install -install(TARGETS - trajlib - IDlib - Conslib - Optlib - Kinovalib - Digitlib - DigitModifiedlib - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} -) \ No newline at end of file diff --git a/Constraints/README.md b/Constraints/README.md deleted file mode 100644 index 9db556cd..00000000 --- a/Constraints/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# Constraints - -This class implements different types of constraints that will be later put into the optimization. -Specifically, we would like to provide the formulation `g_lb <= g(z) <= g_ub`. -The core function is called `compute(const Eigen::VectorXd& z, bool compute_derivatives)`. -The constraint class usually requires a shared pointer of a trajectory class in the constructor, and a shared pointer of a forward kinematics and inverse dynamics class. -The results are stored in `g`, which is a class member, for the optimizer class to access. -The gradient is stored in `pg_pz`. - -Another core function is called `compute_bounds()`. -This is called by the optimizer class at the very beginning of the optimization. -This updates `g_lb` and `g_ub`, which are also class members. - -There's also a function called `print_violation_information`, which will be called by the optimizer class at the end of the optimization, to print relevant information for tuning and debugging. - -### JointLimits - -Constrain robot joint angles by user-defined lower bounds and upper bounds. - -### VelocityLimits - -Constrain robot joint velocities by user-defined upper bounds. - -### TorqueLimits - -Constrain the torque of each joint within the limits. -We compute the torque using inverse dynamics implemented by [pinocchio](https://stack-of-tasks.github.io/pinocchio/). - -### KinematicsConstraints - -Constrain the position and the orientation of a specific joint at a specific time instance at a desired position and orientation. -This [website](https://wang-yimu.com/introduction-to-optimization-on-manifolds/) provides a pretty good introduction on how optimization should be done for this type of problem. -We have not implemented the axis-angle representation yet. -Instead, we constrain all 9 elements of the rotation matrix. -For axis-angle representation (SE(3)) of the constraints and the jacobian, please refer to this [paper](https://arxiv.org/abs/1606.05285). - -### ContactConstraints - -Assuming that an object is attached on the end effector of the robot, we need to satisfy certain constraints so that the object keeps a rigid contact with the robot. -We first need to access the contact wrench between the object and the robot. -The contact wrench contains 6 elements (`fx`, `fy`, `fz`, `mx`, `my`, `mz`). -`fx`, `fy`, `fz` represents the translational contact force in the local frame. -`mx`, `my`, `mz` represents the rotational contact moment in the local frame. -There are several conditions for the object to stay on the end effector: - -1. adsads \ No newline at end of file diff --git a/Constraints/include/BoxCollisionAvoidance.h b/Constraints/include/BoxCollisionAvoidance.h deleted file mode 100644 index d1efa7b9..00000000 --- a/Constraints/include/BoxCollisionAvoidance.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef BOX_COLLISION_AVOIDANCE_H -#define BOX_COLLISION_AVOIDANCE_H - -#include "CollisionAvoidance.h" -#include - -namespace RAPTOR { - -#define PROJECT_POINT_ON_FACE_THRESHOLD 1e-4 - -namespace Box { -constexpr double SQUARE_ROOT_THRESHOLD = 1e-12; - -constexpr int HYPERPLANE_NUM = 6; // 3 * (3 - 1) -constexpr int VERTICES_NUM = 8; - -void TensorProduct(const Eigen::Matrix3d& R, - const Eigen::Array& inp, - Eigen::Array& out); -}; - -double distancePointAndLineSegment(const Eigen::Vector3d& point, - const Eigen::Vector3d& p1, - const Eigen::Vector3d& p2); - -class BoxCollisionAvoidance : public CollisionAvoidance { -public: - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - BoxCollisionAvoidance() = default; - - BoxCollisionAvoidance(const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input); - - // Destructor - ~BoxCollisionAvoidance() = default; - - // class methods: - void initialize(); - - Vec3 computeDifferenceWithCloestPoint(const Vec3& point, - const int obs_id, - double& isInside) const; - - Vec3 computeDifferenceWithCloestPoint(const Vec3& point, - const MatX& ppoint_pz, - const int obs_id, - MatX& pdiff_pz, - double& isInside) const; - - Vec3 computeDifferenceWithCloestPoint(const Vec3& point, - const MatX& ppoint_pz, - const Eigen::Array& ppoint_pz_pz, - const int obs_id, - MatX& pdiff_pz, - Eigen::Array& pdiff_pz_pz, - double& isInside) const; - - void computeDistance(const Vec3& point) override; - - void computeDistance(const Vec3& point, - const MatX& ppoint_pz) override; - - void computeDistance(const Vec3& point, - const MatX& ppoint_pz, - const Eigen::Array& ppoint_pz_pz) override; - - // class members: - // box information - std::vector boxCenters; - std::vector boxOrientation; - std::vector boxSize; - Eigen::Array boxR; - - // hyperplane representation - Eigen::Array normals; //!< Normals of the box faces - Eigen::Array intercepts; //!< Intercepts of the box faces - - // vertices representation - Eigen::Array vertices; //!< Vertices of the box -}; - -}; // namespace RAPTOR - -#endif // BOX_COLLISION_AVOIDANCE_H diff --git a/Constraints/include/CollisionAvoidance.h b/Constraints/include/CollisionAvoidance.h deleted file mode 100644 index 19987f83..00000000 --- a/Constraints/include/CollisionAvoidance.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef COLLISION_AVOIDANCE_H -#define COLLISION_AVOIDANCE_H - -#include "Constraints.h" - -namespace RAPTOR { - -/* -CollisionAvoidance class is one special class that does not inherit from Constraints class. -It is used to compute the distance between a point and all obstacles. -*/ - -class CollisionAvoidance { -public: - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - CollisionAvoidance() = default; - - // Destructor - ~CollisionAvoidance() = default; - - // class methods: - virtual void computeDistance(const Vec3& point) = 0; - - virtual void computeDistance(const Vec3& point, - const MatX& ppoint_pz) = 0; - - virtual void computeDistance(const Vec3& point, - const MatX& ppoint_pz, - const Eigen::Array& ppoint_pz_pz) = 0; - - // class members: - int numObstacles = 0; - - // compute results are stored here - VecX distances; - MatX pdistances_pz; - Eigen::Array pdistances_pz_pz; - - double minimumDistance = 0; - size_t minimumDistanceIndex = 0; - - bool onlyComputeDerivativesForMinimumDistance = false; -}; - -}; // namespace RAPTOR - -#endif // COLLISION_AVOIDANCE_H diff --git a/Constraints/include/ConstrainedJointLimits.h b/Constraints/include/ConstrainedJointLimits.h deleted file mode 100644 index 8e8d4a8e..00000000 --- a/Constraints/include/ConstrainedJointLimits.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef CONSTRAINEDJOINTLIMITS_H -#define CONSTRAINEDJOINTLIMITS_H - -#include "JointLimits.h" -#include "DynamicsConstraints.h" - -namespace RAPTOR { - -class ConstrainedJointLimits : public JointLimits { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - ConstrainedJointLimits() = default; - - // Constructor - ConstrainedJointLimits(std::shared_ptr& trajPtr_input, - std::shared_ptr& dcPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input); - - // Destructor - ~ConstrainedJointLimits() = default; - - // class methods: - // compute constraints - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - virtual void compute_bounds() override; - - // class variables: - int NB = 0; - std::shared_ptr dcPtr_; -}; - -}; // namespace RAPTOR - -#endif // CONSTRAINEDJOINTLIMITS_H diff --git a/Constraints/include/Constraints.h b/Constraints/include/Constraints.h deleted file mode 100644 index 636e5454..00000000 --- a/Constraints/include/Constraints.h +++ /dev/null @@ -1,65 +0,0 @@ - -#ifndef CONSTRAINTS_H -#define CONSTRAINTS_H - -#include -#include -#include "Utils.h" - -namespace RAPTOR { - -// This is the base (abstract) class for all constraints -class Constraints { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - Constraints() = default; - - Constraints(int m_input, - int varLength); - - // Destructor - ~Constraints() = default; - - // class methods: - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) = 0; - - virtual void compute_bounds() = 0; - - virtual void print_violation_info() {} - - // determine if the constraints are computed before and save the current decision variable - bool is_computed(const VecX& z, - bool compute_derivatives, - bool compute_hessian); - - void initialize_memory(const int m_input, - const int varLength, - bool initialize_hessian = true); - - // class members: - int m = 0; // number of constraints - - // the decision variable that was evaluated last time - VecX current_z; - bool if_compute_derivatives = false; - bool if_compute_hessian = false; - - // compute results are stored here - VecX g; - MatX pg_pz; - Eigen::Array pg_pz_pz; - - VecX g_lb; - VecX g_ub; - - // define the variables that stores the results here -}; - -}; // namespace RAPTOR - -#endif // CONSTRAINTS_H diff --git a/Constraints/include/ContactConstraints.h b/Constraints/include/ContactConstraints.h deleted file mode 100644 index ba1bad82..00000000 --- a/Constraints/include/ContactConstraints.h +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef CONTACT_CONSTRAINTS_H -#define CONTACT_CONSTRAINTS_H - -#include "Constraints.h" -#include "Trajectories.h" -#include "CustomizedInverseDynamics.h" -#include "Utils.h" - -namespace RAPTOR { - -typedef struct contactSurfaceParams_ { - double mu = 0.7; // friction coefficient - double Lx = 0.1; // radius of the contact surface (assumed to be rectangle) - double Ly = 0.1; // radius of the contact surface (assumed to be rectangle) - double maxSuctionForce = 100; // maximum suction force - - contactSurfaceParams_() = default; - - contactSurfaceParams_(double mu_input, - double Lx_input, - double Ly_input, - double maxSuctionForce_input) : - mu(mu_input), - Lx(Lx_input), - Ly(Ly_input), - maxSuctionForce(maxSuctionForce_input) {} -} contactSurfaceParams; - -class ContactConstraints : public Constraints { -public: - using Force = pinocchio::Data::Force; - using Vec3 = Eigen::Vector3d; - using Vec6 = Vector6d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - ContactConstraints() = default; - - // Constructor - ContactConstraints(std::shared_ptr& idPtr_input, - const contactSurfaceParams& csp_input); - - // Destructor - ~ContactConstraints() = default; - - // class methods: - // compute constraints - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - // class members: - std::shared_ptr idPtr_; - - contactSurfaceParams csp; -}; - -}; // namespace RAPTOR - -#endif // CONTACT_CONSTRAINTS_H diff --git a/Constraints/include/JointLimits.h b/Constraints/include/JointLimits.h deleted file mode 100644 index 276bf32e..00000000 --- a/Constraints/include/JointLimits.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef JOINTLIMITS_H -#define JOINTLIMITS_H - -#include - -#include "Constraints.h" -#include "Trajectories.h" -#include "Utils.h" - -namespace RAPTOR { - -class JointLimits : public Constraints { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - JointLimits() = default; - - // Constructor - JointLimits(std::shared_ptr& trajPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input); - - // Destructor - ~JointLimits() = default; - - // class methods: - // compute constraints - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - virtual void compute_bounds() override; - - // print violation information - virtual void print_violation_info() override; - - // class members: - std::shared_ptr trajPtr_; - - VecX lowerLimits; - VecX upperLimits; -}; - -}; // namespace RAPTOR - -#endif // JOINTLIMITS_H diff --git a/Constraints/include/KinematicsConstraints.h b/Constraints/include/KinematicsConstraints.h deleted file mode 100644 index 422ba654..00000000 --- a/Constraints/include/KinematicsConstraints.h +++ /dev/null @@ -1,85 +0,0 @@ - -#ifndef KINEMATICS_CONSTRAINTS_H -#define KINEMATICS_CONSTRAINTS_H - -#include "Constraints.h" -#include "Trajectories.h" -#include "ForwardKinematics.h" -#include "LieSpaceResidual.h" - -namespace RAPTOR { - -class KinematicsConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - KinematicsConstraints() = default; - - KinematicsConstraints(std::shared_ptr& trajPtr_input, - const Model* model_input, - const Eigen::VectorXi& jtype_input, - const size_t joint_id_input, - const size_t time_id_input, - const Transform& desiredTransform_input, - const Transform endT_input = Transform()); - - KinematicsConstraints(std::shared_ptr& trajPtr_input, - const Model* model_input, - const Eigen::VectorXi& jtype_input, - const size_t joint_id_input, - const size_t time_id_input, - const Vec3& desiredPosition_input, - const Transform endT_input = Transform()); - - KinematicsConstraints(std::shared_ptr& trajPtr_input, - const Model* model_input, - const Eigen::VectorXi& jtype_input, - const size_t joint_id_input, - const size_t time_id_input, - const Mat3& desiredRotation_input, - const Transform endT_input = Transform()); - - // Destructor - ~KinematicsConstraints() = default; - - // class methods: - // compute constraints - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - void print_violation_info() override; - - // class members: - std::shared_ptr& trajPtr_; - - const Model* modelPtr_ = nullptr; - Eigen::VectorXi jtype; - - std::unique_ptr fkPtr_; - - Vec3 desiredPosition; - Mat3 desiredRotation; - - bool constrainPosition = false; - bool constrainRotation = false; - - size_t joint_id = 0; - size_t time_id = 0; - - // the transform matrix at the beginning and at the end - Transform startT; - Transform endT; -}; - -}; // namespace RAPTOR - -#endif // KINEMATICS_CONSTRAINTS_H diff --git a/Constraints/include/LieSpaceResidual.h b/Constraints/include/LieSpaceResidual.h deleted file mode 100644 index 5a0750bd..00000000 --- a/Constraints/include/LieSpaceResidual.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef LIE_SPACE_RESIDUAL_H -#define LIE_SPACE_RESIDUAL_H - -#include -#include - -#include "Utils.h" -#include "HigherOrderDerivatives.h" -#include "ForwardKinematics.h" - -namespace RAPTOR { - -// This namespace contains functions to compute the residual of the Lie space -// including the translation and rotation part in SE(3) space, with their derivatives -// Assume that fkPtr_ is a valid pointer and has computed all corresponding kinematics -namespace LieSpaceResidual { - -// You have to make sure that fkPtr_ has already computed the corresponding forward kinematics -// and its gradient or hessian before using this function!!! -Eigen::Vector3d translationResidual(const std::unique_ptr& fkPtr_, - const Eigen::Vector3d& desiredPosition, - Eigen::MatrixXd* gradientPtr_ = nullptr, - Eigen::Array* hessianPtr_ = nullptr); - -// You have to make sure that fkPtr_ has already computed the corresponding forward kinematics -// and its gradient or hessian before using this function!!! -Eigen::Vector3d rotationResidual(const std::unique_ptr& fkPtr_, - const Eigen::Matrix3d& desiredRotation, - Eigen::MatrixXd* gradientPtr_ = nullptr, - Eigen::Array* hessianPtr_ = nullptr); - -}; // namespace LieSpaceResidual -}; // namespace RAPTOR - -#endif // LIE_SPACE_RESIDUAL_H \ No newline at end of file diff --git a/Constraints/include/SurfaceContactConstraints.h b/Constraints/include/SurfaceContactConstraints.h deleted file mode 100644 index b59d428e..00000000 --- a/Constraints/include/SurfaceContactConstraints.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef CONTACT_CONSTRAINTS_H -#define CONTACT_CONSTRAINTS_H - -#include "Constraints.h" -#include "Trajectories.h" -#include "ConstrainedInverseDynamics.h" -#include "Utils.h" - -namespace RAPTOR { - -typedef struct frictionParams_ { - double mu = 0.7; - double gamma = 0.7; - double Lx = 0; - double Ly = 0; - - frictionParams_(double mu_input, - double gamma_input, - double Lx_input, - double Ly_input) : - mu(mu_input), - gamma(gamma_input), - Lx(Lx_input), - Ly(Ly_input) {} -} frictionParams; - -class SurfaceContactConstraints : public Constraints { -public: - // Constructor - SurfaceContactConstraints() = default; - - // Constructor - SurfaceContactConstraints(std::shared_ptr& cidPtr_input, - const frictionParams& fp_input); - - // Destructor - ~SurfaceContactConstraints() = default; - - // class methods: - // compute constraints - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - // class variables: - std::shared_ptr cidPtr_; - - frictionParams fp; -}; - -}; // namespace RAPTOR - -#endif // CONTACT_CONSTRAINTS_H diff --git a/Constraints/include/TorqueLimits.h b/Constraints/include/TorqueLimits.h deleted file mode 100644 index 0fe95067..00000000 --- a/Constraints/include/TorqueLimits.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef TORQUE_LIMITS_H -#define TORQUE_LIMITS_H - -#include "Constraints.h" -#include "InverseDynamics.h" - -namespace RAPTOR { - -class TorqueLimits : public Constraints { -public: - // Constructor - TorqueLimits() = default; - - // Constructor - TorqueLimits(std::shared_ptr& trajPtr_input, - std::shared_ptr idPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input); - - // Destructor - ~TorqueLimits() = default; - - // class methods: - // compute constraints - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - // print violation information - virtual void print_violation_info() override; - - // class members: - std::shared_ptr trajPtr_; - std::shared_ptr idPtr_; - - VecX lowerLimits; - VecX upperLimits; -}; - -} // namespace RAPTOR - -#endif // TORQUE_LIMITS_H diff --git a/Constraints/include/VelocityLimits.h b/Constraints/include/VelocityLimits.h deleted file mode 100644 index 43cc16ff..00000000 --- a/Constraints/include/VelocityLimits.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef VELOCITYLIMITS_H -#define VELOCITYLIMITS_H - -#include - -#include "Constraints.h" -#include "Trajectories.h" -#include "Utils.h" - -namespace RAPTOR { - -class VelocityLimits : public Constraints { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - VelocityLimits() = default; - - // Constructor - VelocityLimits(std::shared_ptr& trajPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input); - - // Destructor - ~VelocityLimits() = default; - - // class methods: - // compute constraints - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - virtual void compute_bounds() override; - - // print violation information - virtual void print_violation_info() override; - - // class members: - std::shared_ptr trajPtr_; - - VecX lowerLimits; - VecX upperLimits; -}; - -}; // namespace RAPTOR - -#endif // VELOCITYLIMITS_H diff --git a/Constraints/include/WaitrContactConstraints.h b/Constraints/include/WaitrContactConstraints.h deleted file mode 100644 index c044fc98..00000000 --- a/Constraints/include/WaitrContactConstraints.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef WAITR_CONTACT_CONSTRAINTS_H -#define WAITR_CONTACT_CONSTRAINTS_H - -#include "Constraints.h" -#include "Trajectories.h" -#include "CustomizedInverseDynamics.h" -#include "Utils.h" - -namespace RAPTOR { - -typedef struct contactSurfaceParams_ { - double mu = 0.7; - double Lx = 0.1; - double Ly = 0.1; - double maxSuctionForce = 100; - - contactSurfaceParams_() = default; - - contactSurfaceParams_(double mu_input, - double Lx_input, - double Ly_input, - double maxSuctionForce_input) : - mu(mu_input), - Lx(Lx_input), - Ly(Ly_input), - maxSuctionForce(maxSuctionForce_input) {} -} contactSurfaceParams; - -class WaitrContactConstraints : public Constraints { -public: - using Force = pinocchio::Data::Force; - using Vec3 = Eigen::Vector3d; - using Vec6 = Vector6d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - WaitrContactConstraints() = default; - - // Constructor - WaitrContactConstraints(std::shared_ptr& idPtr_input, - const contactSurfaceParams& csp_input); - - // Destructor - ~WaitrContactConstraints() = default; - - // class methods: - // compute constraints - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - // print violation information - void print_violation_info() override; - - // class variables: - std::shared_ptr idPtr_; - - contactSurfaceParams csp; -}; - -}; // namespace RAPTOR - -#endif // WAITR_CONTACT_CONSTRAINTS_H diff --git a/Constraints/include/ZonotopeCollisionAvoidance.h b/Constraints/include/ZonotopeCollisionAvoidance.h deleted file mode 100644 index 19240175..00000000 --- a/Constraints/include/ZonotopeCollisionAvoidance.h +++ /dev/null @@ -1,96 +0,0 @@ -#ifndef ZONOTOPE_COLLISION_AVOIDANCE_H -#define ZONOTOPE_COLLISION_AVOIDANCE_H - -#include "CollisionAvoidance.h" - -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED - -namespace RAPTOR { - -namespace ZonotopeParams { - constexpr int MAX_OBSTACLE_GENERATOR_NUM = 3; - constexpr int HYPERPLANE_NUM = MAX_OBSTACLE_GENERATOR_NUM * (MAX_OBSTACLE_GENERATOR_NUM - 1); - constexpr int COMB_NUM = HYPERPLANE_NUM / 2; - constexpr int VERTICES_NUM = HYPERPLANE_NUM * 4; -}; - -class ZonotopeCollisionAvoidance : public CollisionAvoidance { -public: - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - const int MAX_OBSTACLE_GENERATOR_NUM = ZonotopeParams::MAX_OBSTACLE_GENERATOR_NUM; - const int HYPERPLANE_NUM = ZonotopeParams::HYPERPLANE_NUM; - const int COMB_NUM = ZonotopeParams::COMB_NUM; - const int VERTICES_NUM = ZonotopeParams::VERTICES_NUM; - - // Constructor - ZonotopeCollisionAvoidance() = default; - - ZonotopeCollisionAvoidance(const std::vector& zonotopeCenters_input, - const Eigen::Array& zonotopeGenerators_input); - - // Destructor - ~ZonotopeCollisionAvoidance() { - delete [] A; - delete [] b; - delete [] v_start_idx; - delete [] v_size; - delete [] v; - } - - // class methods: - void initialize(); - - void computeDistance(const Vec3& point) override; - - void computeDistance(const Vec3& point, const MatX& ppoint_pz) override; - - // class members: - // zonotope centers and generators - std::vector zonotopeCenters; - Eigen::Array zonotopeGenerators; - - // hyperplane representation - double* A = nullptr; - double* b = nullptr; - - // vertices representation - int* v_start_idx = nullptr; - int* v_size = nullptr; - double* v = nullptr; -}; - -}; // namespace RAPTOR - -#endif // ZONOTOPE_COLLISION_AVOIDANCE_H diff --git a/Constraints/include/fclCollisionAvoidance.h b/Constraints/include/fclCollisionAvoidance.h deleted file mode 100644 index 9c797ed2..00000000 --- a/Constraints/include/fclCollisionAvoidance.h +++ /dev/null @@ -1,126 +0,0 @@ -#ifndef FCL_COLLISION_AVOIDANCE_H -#define FCL_COLLISION_AVOIDANCE_H - -#include -#include -#include -#include -#include - -#include "BoxCollisionAvoidance.h" - -namespace RAPTOR { - -struct customizedUserDataForSphere { - customizedUserDataForSphere() {} - customizedUserDataForSphere(const std::string name_input, - const int link_id_input) : - name(name_input), - linkId(link_id_input){ - ppoint_pz = Eigen::MatrixXd::Zero(0, 0); - } - customizedUserDataForSphere(const std::string name_input, - const int link_id_input, - const Eigen::MatrixXd& ppoint_pz_input) : - name(name_input), - linkId(link_id_input), - ppoint_pz(ppoint_pz_input) {} - - std::string name = ""; //!< Name of the object - int linkId = 0; //!< The link that this sphere belongs to (we don't check collision between adajacent links) - Eigen::MatrixXd ppoint_pz; //!< Derivative of the center of the sphere (this is only for sphere!) -}; - -struct customizedUserDataForBox { - customizedUserDataForBox() {} - customizedUserDataForBox(const std::string name_input) : - name(name_input) { - } - - std::string name = ""; //!< Name of the object -}; - -struct CustomizedDistanceData { - CustomizedDistanceData() { - result.min_distance = std::numeric_limits::max(); - min_distance = std::numeric_limits::max(); - done = false; - } - - void reset() { - result.min_distance = std::numeric_limits::max(); - min_distance = std::numeric_limits::max(); - done = false; - } - - // the following are default values - fcl::DistanceRequestd request; //!< Request parameters for distance computation. - fcl::DistanceResultd result; //!< Result of distance computation. - bool done = false; //!< Flag indicating if distance computation is completed. - - // the following are customized values - std::string name1 = ""; //!< Name of the first object in the collision pair. - std::string name2 = ""; //!< Name of the second object in the collision pair. - double min_distance = 0.0; //!< Minimum distance between two collision managers. - Eigen::VectorXd pmin_distance_pz; //!< Derivative of the minimum distances between two collision managers. -}; - -bool CustomizedDistanceFunction(fcl::CollisionObjectd* o1, - fcl::CollisionObjectd* o2, - void* cdata_, - double& dist); - -bool CustomizedDistanceFunctionDerivative(fcl::CollisionObjectd* o1, - fcl::CollisionObjectd* o2, - void* cdata_, - double& dist); - -class fclCollisionAvoidance { -public: - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - fclCollisionAvoidance() { - fclBroadPhaseManagerPtr_ = std::make_shared(); - } - - ~fclCollisionAvoidance() { - clear(); - } - - bool empty() const { - return fclObjectCollection.empty(); - } - - void addObstacleBox(const std::string& name, - const Vec3& boxCenter, - const Vec3& boxOrientation, - const Vec3& boxSize); - - void addRobotSphere(const std::string& name, - const int linkId, - const Vec3& sphereCenter, - const double sphereRadius, - const MatX& ppoint_pz = MatX::Zero(0, 0)); - - void clear(); - - // self collision check - void computeDistance(bool compute_derivatives = false); - - // collision check with other collision manager - void computeDistance(const std::shared_ptr& otherCollisionManager, - bool compute_derivatives = false); - - std::unordered_map fclObjectCollection; //!< Collection of collision objects. - - std::shared_ptr fclBroadPhaseManagerPtr_ = nullptr; //!< Broad phase collision manager. - - CustomizedDistanceData distanceData; //!< Data for customized distance computation. -}; - -}; // namespace RAPTOR - -#endif // FCL_COLLISION_AVOIDANCE_H \ No newline at end of file diff --git a/Constraints/src/BoxCollisionAvoidance.cpp b/Constraints/src/BoxCollisionAvoidance.cpp deleted file mode 100644 index 2473a335..00000000 --- a/Constraints/src/BoxCollisionAvoidance.cpp +++ /dev/null @@ -1,477 +0,0 @@ -#include "BoxCollisionAvoidance.h" - -namespace RAPTOR { - -namespace Box{ -void TensorProduct(const Eigen::Matrix3d& R, - const Eigen::Array& inp, - Eigen::Array& out) { - out(0) = R(0, 0) * inp(0) + R(0, 1) * inp(1) + R(0, 2) * inp(2); - out(1) = R(1, 0) * inp(0) + R(1, 1) * inp(1) + R(1, 2) * inp(2); - out(2) = R(2, 0) * inp(0) + R(2, 1) * inp(1) + R(2, 2) * inp(2); -} -}; // namespace Box - -double distancePointAndLineSegment(const Eigen::Vector3d& point, - const Eigen::Vector3d& p1, - const Eigen::Vector3d& p2) { - Eigen::Vector3d v = p2 - p1; //!< Vector representing the line segment - Eigen::Vector3d w = point - p1; //!< Vector from p1 to the point - - double lambda = w.dot(v) / v.dot(v); - double t = std::max(0.0, std::min(1.0, lambda)); - Eigen::Vector3d projection = p1 + t * v; //!< Projected point on the line segment - - // Compute the distance between the projected point and the given point - return (projection - point).norm(); -} - -BoxCollisionAvoidance::BoxCollisionAvoidance(const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input) : - boxCenters(boxCenters_input), - boxOrientation(boxOrientation_input), - boxSize(boxSize_input) { - if (boxCenters.size() != boxOrientation.size() || - boxCenters.size() != boxSize.size()) { - throw std::invalid_argument("boxCenters, boxOrientation, and boxSize should have the same size"); - } - - numObstacles = boxCenters.size(); - - distances.resize(numObstacles); - - initialize(); -} - -void BoxCollisionAvoidance::initialize() { - // allocate memory - boxR.resize(numObstacles); - normals.resize(numObstacles, Box::HYPERPLANE_NUM); - intercepts.resize(numObstacles, Box::HYPERPLANE_NUM); - vertices.resize(numObstacles, Box::VERTICES_NUM); - - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - const Vec3 half_size = boxSize[obs_id] / 2; - const Vec3& center = boxCenters[obs_id]; - const Vec3& rpy = boxOrientation[obs_id]; - - Mat3 R = (Eigen::AngleAxisd(rpy[2], Vec3::UnitZ()) * - Eigen::AngleAxisd(rpy[1], Vec3::UnitY()) * - Eigen::AngleAxisd(rpy[0], Vec3::UnitX())).matrix(); - - // initialize vertices - vertices(obs_id, 0) = center + R * Vec3(half_size(0), half_size(1), half_size(2)); - vertices(obs_id, 1) = center + R * Vec3(half_size(0), half_size(1), -half_size(2)); - vertices(obs_id, 2) = center + R * Vec3(half_size(0), -half_size(1), half_size(2)); - vertices(obs_id, 3) = center + R * Vec3(half_size(0), -half_size(1), -half_size(2)); - vertices(obs_id, 4) = center + R * Vec3(-half_size(0), half_size(1), half_size(2)); - vertices(obs_id, 5) = center + R * Vec3(-half_size(0), half_size(1), -half_size(2)); - vertices(obs_id, 6) = center + R * Vec3(-half_size(0), -half_size(1), half_size(2)); - vertices(obs_id, 7) = center + R * Vec3(-half_size(0), -half_size(1), -half_size(2)); - - // initialize hyperplanes - normals(obs_id, 0) = R * Vec3(1, 0, 0); - normals(obs_id, 1) = R * Vec3(0, 1, 0); - normals(obs_id, 2) = R * Vec3(0, 0, 1); - normals(obs_id, 3) = R * Vec3(-1, 0, 0); - normals(obs_id, 4) = R * Vec3(0, -1, 0); - normals(obs_id, 5) = R * Vec3(0, 0, -1); - - intercepts(obs_id, 0) = normals(obs_id, 0).dot(center + R * Vec3(half_size(0), 0, 0)); - intercepts(obs_id, 1) = normals(obs_id, 1).dot(center + R * Vec3(0, half_size(1), 0)); - intercepts(obs_id, 2) = normals(obs_id, 2).dot(center + R * Vec3(0, 0, half_size(2))); - intercepts(obs_id, 3) = normals(obs_id, 3).dot(center + R * Vec3(-half_size(0), 0, 0)); - intercepts(obs_id, 4) = normals(obs_id, 4).dot(center + R * Vec3(0, -half_size(1), 0)); - intercepts(obs_id, 5) = normals(obs_id, 5).dot(center + R * Vec3(0, 0, -half_size(2))); - - boxR(obs_id) = R; - } -} - -Eigen::Vector3d BoxCollisionAvoidance::computeDifferenceWithCloestPoint(const Vec3& point, - const int obs_id, - double& isInside) const { - // Compute the closest point in the local frame of the box - Vec3 localPoint = boxR(obs_id).transpose() * (point - boxCenters[obs_id]); - Vec3 localClosestPoint; - isInside = -1.0; - double distancesWithFace[3][2]; - - for (int i = 0; i < 3; i++) { - distancesWithFace[i][0] = localPoint(i) + boxSize[obs_id](i) / 2.0; - distancesWithFace[i][1] = boxSize[obs_id](i) / 2.0 - localPoint(i); - localClosestPoint(i) = std::max(-boxSize[obs_id](i) / 2.0, std::min(localPoint(i), boxSize[obs_id](i) / 2.0)); - - if (localClosestPoint(i) != localPoint(i)) { - isInside = 1.0; - } - } - - // The point is inside the box, find the closest point on the surface - if (isInside == -1.0) { - double minDist = 1e19; - size_t faceIndex = 0, directionIndex = 0; - for (int i = 0; i < 3; i++) { - if (distancesWithFace[i][0] < minDist) { - minDist = distancesWithFace[i][0]; - faceIndex = i; - directionIndex = 0; - } - if (distancesWithFace[i][1] < minDist) { - minDist = distancesWithFace[i][1]; - faceIndex = i; - directionIndex = 1; - } - } - - localClosestPoint(faceIndex) = (directionIndex == 0) ? - -boxSize[obs_id](faceIndex) / 2.0 : - boxSize[obs_id](faceIndex) / 2.0; - } - - // Transform the closest point from the local frame to the global frame - Vec3 closestPoint = boxR(obs_id) * localClosestPoint + boxCenters[obs_id]; - - return point - closestPoint; -} - -Eigen::Vector3d BoxCollisionAvoidance::computeDifferenceWithCloestPoint(const Vec3& point, - const MatX& ppoint_pz, - const int obs_id, - MatX& pdiff_pz, - double& isInside) const { - pdiff_pz.resize(3, ppoint_pz.cols()); - - // Compute the closest point in the local frame of the box - Vec3 localPoint = boxR(obs_id).transpose() * (point - boxCenters[obs_id]); - MatX plocalPoint_pz = boxR(obs_id).transpose() * ppoint_pz; - Vec3 localClosestPoint; - MatX plocalClosestPoint_pz(3, ppoint_pz.cols()); - isInside = -1.0; - double distancesWithFace[3][2]; - - for (int i = 0; i < 3; i++) { - distancesWithFace[i][0] = localPoint(i) + boxSize[obs_id](i) / 2.0; - distancesWithFace[i][1] = boxSize[obs_id](i) / 2.0 - localPoint(i); - - if (localPoint(i) < -boxSize[obs_id](i) / 2.0) { - localClosestPoint(i) = -boxSize[obs_id](i) / 2.0; - isInside = 1.0; - plocalClosestPoint_pz.row(i).setZero(); - } - else if (localPoint(i) > boxSize[obs_id](i) / 2.0) { - localClosestPoint(i) = boxSize[obs_id](i) / 2.0; - isInside = 1.0; - plocalClosestPoint_pz.row(i).setZero(); - } - else { - localClosestPoint(i) = localPoint(i); - plocalClosestPoint_pz.row(i) = plocalPoint_pz.row(i); - } - } - - // The point is inside the box, find the closest point on the surface - if (isInside == -1.0) { - double minDist = 1e19; - size_t faceIndex = 0, directionIndex = 0; - for (int i = 0; i < 3; i++) { - if (distancesWithFace[i][0] < minDist) { - minDist = distancesWithFace[i][0]; - faceIndex = i; - directionIndex = 0; - } - if (distancesWithFace[i][1] < minDist) { - minDist = distancesWithFace[i][1]; - faceIndex = i; - directionIndex = 1; - } - } - - localClosestPoint(faceIndex) = (directionIndex == 0) ? - -boxSize[obs_id](faceIndex) / 2.0 : - boxSize[obs_id](faceIndex) / 2.0; - plocalClosestPoint_pz.row(faceIndex).setZero(); - } - - // Transform the closest point from the local frame to the global frame - Vec3 closestPoint = boxR(obs_id) * localClosestPoint + boxCenters[obs_id]; - MatX pclosestPoint_pz = boxR(obs_id) * plocalClosestPoint_pz; - - pdiff_pz = ppoint_pz - pclosestPoint_pz; - - return point - closestPoint; -} - -Eigen::Vector3d BoxCollisionAvoidance::computeDifferenceWithCloestPoint(const Vec3& point, - const MatX& ppoint_pz, - const Eigen::Array& ppoint_pz_pz, - const int obs_id, - MatX& pdiff_pz, - Eigen::Array& pdiff_pz_pz, - double& isInside) const { - pdiff_pz.resize(3, ppoint_pz.cols()); - for (int i = 0; i < 3; i++) { - pdiff_pz_pz(i).resize(ppoint_pz.cols(), ppoint_pz.cols()); - } - - // Compute the closest point in the local frame of the box - Vec3 localPoint = boxR(obs_id).transpose() * (point - boxCenters[obs_id]); - MatX plocalPoint_pz = boxR(obs_id).transpose() * ppoint_pz; - Eigen::Array plocalPoint_pz_pz; - Box::TensorProduct(boxR(obs_id).transpose(), ppoint_pz_pz, plocalPoint_pz_pz); - Vec3 localClosestPoint; - MatX plocalClosestPoint_pz(3, ppoint_pz.cols()); - Eigen::Array plocalClosestPoint_pz_pz; - isInside = -1.0; - double distancesWithFace[3][2]; - - for (int i = 0; i < 3; i++) { - distancesWithFace[i][0] = localPoint(i) + boxSize[obs_id](i) / 2.0; - distancesWithFace[i][1] = boxSize[obs_id](i) / 2.0 - localPoint(i); - - if (localPoint(i) < -boxSize[obs_id](i) / 2.0) { - localClosestPoint(i) = -boxSize[obs_id](i) / 2.0; - isInside = 1.0; - plocalClosestPoint_pz.row(i).setZero(); - plocalClosestPoint_pz_pz(i) = MatX::Zero(ppoint_pz.cols(), ppoint_pz.cols()); - } - else if (localPoint(i) > boxSize[obs_id](i) / 2.0) { - localClosestPoint(i) = boxSize[obs_id](i) / 2.0; - isInside = 1.0; - plocalClosestPoint_pz.row(i).setZero(); - plocalClosestPoint_pz_pz(i) = MatX::Zero(ppoint_pz.cols(), ppoint_pz.cols()); - } - else { - localClosestPoint(i) = localPoint(i); - plocalClosestPoint_pz.row(i) = plocalPoint_pz.row(i); - plocalClosestPoint_pz_pz(i) = plocalPoint_pz_pz(i); - } - } - - // The point is inside the box, find the closest point on the surface - if (isInside == -1.0) { - double minDist = 1e19; - size_t faceIndex = 0, directionIndex = 0; - for (int i = 0; i < 3; i++) { - if (distancesWithFace[i][0] < minDist) { - minDist = distancesWithFace[i][0]; - faceIndex = i; - directionIndex = 0; - } - if (distancesWithFace[i][1] < minDist) { - minDist = distancesWithFace[i][1]; - faceIndex = i; - directionIndex = 1; - } - } - - localClosestPoint(faceIndex) = (directionIndex == 0) ? - -boxSize[obs_id](faceIndex) / 2.0 : - boxSize[obs_id](faceIndex) / 2.0; - plocalClosestPoint_pz.row(faceIndex).setZero(); - plocalClosestPoint_pz_pz(faceIndex).setZero(); - } - - // Transform the closest point from the local frame to the global frame - Vec3 closestPoint = boxR(obs_id) * localClosestPoint + boxCenters[obs_id]; - MatX pclosestPoint_pz = boxR(obs_id) * plocalClosestPoint_pz; - Eigen::Array pclosestPoint_pz_pz; - Box::TensorProduct(boxR(obs_id), plocalClosestPoint_pz_pz, pclosestPoint_pz_pz); - - pdiff_pz = ppoint_pz - pclosestPoint_pz; - for (int i = 0; i < 3; i++) { - pdiff_pz_pz(i) = ppoint_pz_pz(i) - pclosestPoint_pz_pz(i); - } - - return point - closestPoint; -} - -void BoxCollisionAvoidance::computeDistance(const Vec3& point) { - minimumDistance = 1e19; - Vec3 diff; - double isInside = 1.0; - - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - diff = computeDifferenceWithCloestPoint(point, - obs_id, - isInside); - - const double diffSquared = diff.dot(diff); - distances(obs_id) = (diffSquared > Box::SQUARE_ROOT_THRESHOLD) ? - isInside * std::sqrt(diffSquared) : - 0.0; - - if (distances(obs_id) < minimumDistance) { - minimumDistance = distances(obs_id); - minimumDistanceIndex = obs_id; - } - } -} - -void BoxCollisionAvoidance::computeDistance(const Vec3& point, - const MatX& ppoint_pz) { - if (ppoint_pz.rows() != 3) { - throw std::invalid_argument("ppoint_pz should have 3 rows"); - } - - minimumDistance = 1e19; - Vec3 diff; - MatX pdiff_pz; - double isInside = 1.0; - - pdistances_pz.resize(numObstacles, ppoint_pz.cols()); - - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - if (onlyComputeDerivativesForMinimumDistance) { - diff = computeDifferenceWithCloestPoint(point, - obs_id, - isInside); - } - else { - diff = computeDifferenceWithCloestPoint(point, ppoint_pz, - obs_id, - pdiff_pz, - isInside); - } - - const double diffSquared = diff.dot(diff); - const double distanceNorm = std::sqrt(diffSquared); - distances(obs_id) = (diffSquared > Box::SQUARE_ROOT_THRESHOLD) ? - isInside * distanceNorm : - 0.0; - - if (distances(obs_id) < minimumDistance) { - minimumDistance = distances(obs_id); - minimumDistanceIndex = obs_id; - } - - if (!onlyComputeDerivativesForMinimumDistance) { - if (diffSquared > Box::SQUARE_ROOT_THRESHOLD) { - pdistances_pz.row(obs_id) = isInside * diff.transpose() * pdiff_pz / distanceNorm; - } - else { - pdistances_pz.row(obs_id).setZero(); - } - } - } - - if (onlyComputeDerivativesForMinimumDistance) { - diff = computeDifferenceWithCloestPoint(point, ppoint_pz, - minimumDistanceIndex, - pdiff_pz, - isInside); - - const double diffSquared = diff.dot(diff); - if (diffSquared > Box::SQUARE_ROOT_THRESHOLD) { - pdistances_pz.row(minimumDistanceIndex) = isInside * diff.transpose() * pdiff_pz / std::sqrt(diffSquared); - } - else { - pdistances_pz.row(minimumDistanceIndex).setZero(); - } - } -} - -void BoxCollisionAvoidance::computeDistance(const Vec3& point, - const MatX& ppoint_pz, - const Eigen::Array& ppoint_pz_pz) { - if (ppoint_pz.rows() != 3) { - throw std::invalid_argument("ppoint_pz should have 3 rows"); - } - - if (ppoint_pz_pz(0).rows() != ppoint_pz.cols() || - ppoint_pz_pz(0).cols() != ppoint_pz.cols()) { - throw std::invalid_argument("ppoint_pz_pz should have the same number of columns as ppoint_pz"); - } - if (ppoint_pz_pz(1).rows() != ppoint_pz.cols() || - ppoint_pz_pz(1).cols() != ppoint_pz.cols()) { - throw std::invalid_argument("ppoint_pz_pz should have the same number of columns as ppoint_pz"); - } - if (ppoint_pz_pz(2).rows() != ppoint_pz.cols() || - ppoint_pz_pz(2).cols() != ppoint_pz.cols()) { - throw std::invalid_argument("ppoint_pz_pz should have the same number of columns as ppoint_pz"); - } - - minimumDistance = 1e19; - Vec3 diff; - MatX pdiff_pz; - Eigen::Array pdiff_pz_pz; - double isInside = 1.0; - - pdistances_pz.resize(numObstacles, ppoint_pz.cols()); - - pdistances_pz_pz.resize(numObstacles); - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - pdistances_pz_pz(obs_id).resize(ppoint_pz.cols(), ppoint_pz.cols()); - } - - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - if (onlyComputeDerivativesForMinimumDistance) { - diff = computeDifferenceWithCloestPoint(point, - obs_id, - isInside); - } - else { - diff = computeDifferenceWithCloestPoint(point, ppoint_pz, ppoint_pz_pz, - obs_id, - pdiff_pz, pdiff_pz_pz, - isInside); - } - - const double diffSquared = diff.dot(diff); - const double distanceNorm = std::sqrt(diffSquared); - distances(obs_id) = (diffSquared > Box::SQUARE_ROOT_THRESHOLD) ? - isInside * distanceNorm : - 0.0; - - if (distances(obs_id) < minimumDistance) { - minimumDistance = distances(obs_id); - minimumDistanceIndex = obs_id; - } - - if (!onlyComputeDerivativesForMinimumDistance) { - if (diffSquared > Box::SQUARE_ROOT_THRESHOLD) { - const MatX pdiffSquare_pz = diff.transpose() * pdiff_pz; - pdistances_pz.row(obs_id) = isInside * pdiffSquare_pz / distanceNorm; - - pdistances_pz_pz(obs_id) = pdiff_pz.transpose() * pdiff_pz / distanceNorm; - for (int i = 0; i < 3; i++) { - pdistances_pz_pz(obs_id) += diff(i) * pdiff_pz_pz(i) / distanceNorm; - } - pdistances_pz_pz(obs_id) -= pdiffSquare_pz.transpose() * pdiffSquare_pz / std::pow(distanceNorm, 3); - pdistances_pz_pz(obs_id) *= isInside; - } - else { - pdistances_pz.row(obs_id).setZero(); - pdistances_pz_pz(obs_id).setZero(); - } - } - } - - if (onlyComputeDerivativesForMinimumDistance) { - diff = computeDifferenceWithCloestPoint(point, ppoint_pz, ppoint_pz_pz, - minimumDistanceIndex, - pdiff_pz, pdiff_pz_pz, - isInside); - - double diffSquared = diff.dot(diff); - if (diffSquared > Box::SQUARE_ROOT_THRESHOLD) { - double dist = std::sqrt(diffSquared); - MatX pdiffSquare_pz = diff.transpose() * pdiff_pz; - pdistances_pz.row(minimumDistanceIndex) = isInside * pdiffSquare_pz / dist; - - pdistances_pz_pz(minimumDistanceIndex) = pdiff_pz.transpose() * pdiff_pz / dist; - for (int i = 0; i < 3; i++) { - pdistances_pz_pz(minimumDistanceIndex) += diff(i) * pdiff_pz_pz(i) / dist; - } - pdistances_pz_pz(minimumDistanceIndex) -= pdiffSquare_pz.transpose() * pdiffSquare_pz / std::pow(diffSquared, 1.5); - pdistances_pz_pz(minimumDistanceIndex) *= isInside; - } - else { - pdistances_pz.row(minimumDistanceIndex).setZero(); - pdistances_pz_pz(minimumDistanceIndex).setZero(); - } - } -} - -}; // namespace RAPTOR diff --git a/Constraints/src/ConstrainedJointLimits.cpp b/Constraints/src/ConstrainedJointLimits.cpp deleted file mode 100644 index ebe65c5b..00000000 --- a/Constraints/src/ConstrainedJointLimits.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "ConstrainedJointLimits.h" - -namespace RAPTOR { - -ConstrainedJointLimits::ConstrainedJointLimits(std::shared_ptr& trajPtr_input, - std::shared_ptr& dcPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input) { - trajPtr_ = trajPtr_input; - dcPtr_ = dcPtr_input; - lowerLimits = lowerLimits_input; - upperLimits = upperLimits_input; - - if (lowerLimits.size() != upperLimits.size()) { - throw std::invalid_argument("lowerLimits and upperLimits must be the same size"); - } - - if (trajPtr_->Nact != dcPtr_->numIndependentJoints) { - throw std::invalid_argument("Trajectory and DynamicsConstraints must have the same number of actuated joints"); - } - - NB = dcPtr_->numIndependentJoints + dcPtr_->numDependentJoints; - - if (lowerLimits.size() != NB) { - throw std::invalid_argument("lowerLimits and upperLimits must be the same size as the number of actuated joints"); - } - - m = trajPtr_->N * NB; - - g = VecX::Zero(m); - g_lb = VecX::Zero(m); - g_ub = VecX::Zero(m); - pg_pz.resize(m, trajPtr_->varLength); -} - -void ConstrainedJointLimits::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("ConstrainedJointLimits does not support hessian computation"); - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - for (int i = 0; i < trajPtr_->N; i++) { - VecX qfull(NB); - dcPtr_->fill_independent_vector(qfull, trajPtr_->q(i), true); - - dcPtr_->setupJointPosition(qfull, compute_derivatives); - g.block(i * NB, 0, NB, 1) = Utils::wrapToPi(qfull); - - if (compute_derivatives) { - // fill in independent joints derivatives directly - for (int j = 0; j < dcPtr_->numIndependentJoints; j++) { - int indenpendentJointIndex = dcPtr_->return_independent_joint_index(j); - pg_pz.row(i * NB + indenpendentJointIndex) = trajPtr_->pq_pz(i).row(j); - } - - // compute and fill in dependent joints derivatives - MatX pq_dep_pz = dcPtr_->pq_dep_pq_indep * trajPtr_->pq_pz(i); - for (int j = 0; j < dcPtr_->numDependentJoints; j++) { - int denpendentJointIndex = dcPtr_->return_dependent_joint_index(j); - pg_pz.row(i * NB + denpendentJointIndex) = pq_dep_pz.row(j); - } - } - } -} - -void ConstrainedJointLimits::compute_bounds() { - for (int i = 0; i < trajPtr_->N; i++) { - g_lb.block(i * NB, 0, NB, 1) = lowerLimits; - g_ub.block(i * NB, 0, NB, 1) = upperLimits; - } -} - -}; // namespace RAPTOR diff --git a/Constraints/src/Constraints.cpp b/Constraints/src/Constraints.cpp deleted file mode 100644 index 11f7ae3c..00000000 --- a/Constraints/src/Constraints.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "Constraints.h" - -namespace RAPTOR { - -Constraints::Constraints(int m_input, - int varLength) { - initialize_memory(m_input, varLength); -} - -bool Constraints::is_computed(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (compute_hessian && !compute_derivatives) { - throw std::invalid_argument("compute_derivatives needs to be true when compute_hessian is true."); - return false; - } - - if (!Utils::ifTwoVectorEqual(current_z, z, 0)) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - if (compute_derivatives != if_compute_derivatives) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - // current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return true; -} - -void Constraints::initialize_memory(const int m_input, - const int varLength, - bool initialize_hessian) { - m = m_input; - - g = VecX::Zero(m); - - g_lb = VecX::Zero(m); - g_ub = VecX::Zero(m); - - pg_pz = MatX::Zero(m, varLength); - - if (initialize_hessian) { - pg_pz_pz.resize(m); - for (int i = 0; i < m; i++) { - pg_pz_pz(i) = MatX::Zero(varLength, varLength); - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Constraints/src/ContactConstraints.cpp b/Constraints/src/ContactConstraints.cpp deleted file mode 100644 index 733f3053..00000000 --- a/Constraints/src/ContactConstraints.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include "ContactConstraints.h" - -namespace RAPTOR { - -ContactConstraints::ContactConstraints(std::shared_ptr& idPtr_input, - const contactSurfaceParams& csp_input) : - idPtr_(idPtr_input), - csp(csp_input) { - // (1) positive contact force - // (2) translation friction cone - // (3, 4) ZMP on one axis - // (5, 6) ZMP on the other axis - initialize_memory(idPtr_->trajPtr_->N * 6, - idPtr_->trajPtr_->varLength, - false); -} - -void ContactConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("ContactConstraints: compute_hessian is not implemented yet!"); - } - - idPtr_->compute(z, compute_derivatives, compute_hessian); - - for (int i = 0; i < idPtr_->trajPtr_->N; i++) { - // access the last contact wrench, - // which should be the contact wrench between the end effector and the object - const Vec6& lambda = idPtr_->lambda(i); - - const Vec3& rotation_torque = lambda.head(3); - const Vec3& translation_force = lambda.tail(3); - - double contact_force = translation_force(2) + csp.maxSuctionForce; - double friction_force = sqrt(pow(translation_force(0), 2) + pow(translation_force(1), 2)); - double max_friction_force = csp.mu * contact_force; - double mx_lower_limit = -csp.Lx * contact_force; - double mx_upper_limit = csp.Lx * contact_force; - double my_lower_limit = -csp.Ly * contact_force; - double my_upper_limit = csp.Ly * contact_force; - - // (1) positive contact force - g(i * 6 + 0) = contact_force; - - // (2) translation friction cone - g(i * 6 + 1) = friction_force - max_friction_force; - - // (3, 4) ZMP on one axis - g(i * 6 + 2) = rotation_torque(0) - mx_upper_limit; - g(i * 6 + 3) = mx_lower_limit - rotation_torque(0); - - // (5, 6) ZMP on the other axis - g(i * 6 + 4) = rotation_torque(1) - my_upper_limit; - g(i * 6 + 5) = my_lower_limit - rotation_torque(1); - - if (compute_derivatives) { - // assume the contact wrench is always located at the end - const MatX& protation_torque_pz = idPtr_->plambda_pz(i).topRows(3); - const MatX& ptranslation_force_pz = idPtr_->plambda_pz(i).bottomRows(3); - - // (1) positive contact force - pg_pz.row(i * 6 + 0) = ptranslation_force_pz.row(2); - - // (2) translation friction cone - if (friction_force <= 1e-3) { // avoid singularity when friction_force is close to 0 - pg_pz.row(i * 6 + 1) = csp.mu * ptranslation_force_pz.row(2); - } - else { - pg_pz.row(i * 6 + 1) = (translation_force(0) * ptranslation_force_pz.row(0) + - translation_force(1) * ptranslation_force_pz.row(1)) / friction_force - - csp.mu * ptranslation_force_pz.row(2); - } - - // (3, 4) ZMP on one axis - pg_pz.row(i * 6 + 2) = protation_torque_pz.row(0) - csp.Lx * ptranslation_force_pz.row(2); - pg_pz.row(i * 6 + 3) = -csp.Lx * ptranslation_force_pz.row(2) - protation_torque_pz.row(0); - - // (5, 6) ZMP on the other axis - pg_pz.row(i * 6 + 4) = protation_torque_pz.row(1) - csp.Ly * ptranslation_force_pz.row(2); - pg_pz.row(i * 6 + 5) = -csp.Ly * ptranslation_force_pz.row(2) - protation_torque_pz.row(1); - } - } -} - -void ContactConstraints::compute_bounds() { - for (int i = 0; i < idPtr_->trajPtr_->N; i++) { - // (1) positive contact force - g_lb(i * 6 + 0) = 0; - g_ub(i * 6 + 0) = 1e19; - - // (2) translation friction cone - g_lb(i * 6 + 1) = -1e19; - g_ub(i * 6 + 1) = 0; - - // (3, 4) ZMP on one axis - g_lb(i * 6 + 2) = -1e19; - g_ub(i * 6 + 2) = 0; - - g_lb(i * 6 + 3) = -1e19; - g_ub(i * 6 + 3) = 0; - - // (5, 6) ZMP on the other axis - g_lb(i * 6 + 4) = -1e19; - g_ub(i * 6 + 4) = 0; - - g_lb(i * 6 + 5) = -1e19; - g_ub(i * 6 + 5) = 0; - } -} - -}; // namespace RAPTOR - diff --git a/Constraints/src/JointLimits.cpp b/Constraints/src/JointLimits.cpp deleted file mode 100644 index 4642e6d3..00000000 --- a/Constraints/src/JointLimits.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "JointLimits.h" - -namespace RAPTOR { - -JointLimits::JointLimits(std::shared_ptr& trajPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input) : - lowerLimits(lowerLimits_input), - upperLimits(upperLimits_input) { - trajPtr_ = trajPtr_input; - - if (lowerLimits.size() != upperLimits.size()) { - throw std::invalid_argument("lowerLimits and upperLimits must be the same size"); - } - - if (lowerLimits.size() != trajPtr_->Nact) { - throw std::invalid_argument("lowerLimits and upperLimits must be the same size as the number of actuated joints"); - } - - initialize_memory(trajPtr_->N * trajPtr_->Nact, - trajPtr_->varLength); -} - -void JointLimits::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - for (int i = 0; i < trajPtr_->N; i++) { - // g.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = Utils::wrapToPi(trajPtr_->q(i).head(trajPtr_->Nact)); - g.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = trajPtr_->q(i).head(trajPtr_->Nact); - - if (compute_derivatives) { - pg_pz.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, trajPtr_->varLength) = trajPtr_->pq_pz(i); - } - - if (compute_hessian) { - for (int j = 0; j < trajPtr_->Nact; j++) { - pg_pz_pz(i * trajPtr_->Nact + j) = trajPtr_->pq_pz_pz(j, i); - } - } - } -} - -void JointLimits::compute_bounds() { - for (int i = 0; i < trajPtr_->N; i++) { - g_lb.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = lowerLimits; - g_ub.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = upperLimits; - } -} - -void JointLimits::print_violation_info() { - for (int i = 0; i < trajPtr_->N; i++) { - for (int j = 0; j < trajPtr_->Nact; j++) { - if (g(i * trajPtr_->Nact + j) <= lowerLimits(j)) { - std::cout << " JointLimits.cpp: Joint " - << j - << " at time instance " - << i - << " is below lower limit: " - << g(i * trajPtr_->Nact + j) - << " < " - << lowerLimits(j) - << std::endl; - } - else if (g(i * trajPtr_->Nact + j) >= upperLimits(j)) { - std::cout << " JointLimits.cpp: Joint " - << j - << " at time instance " - << i - << " is above upper limit: " - << g(i * trajPtr_->Nact + j) - << " > " - << upperLimits(j) - << std::endl; - } - } - } -} - -}; // namespace RAPTOR diff --git a/Constraints/src/KinematicsConstraints.cpp b/Constraints/src/KinematicsConstraints.cpp deleted file mode 100644 index d622859a..00000000 --- a/Constraints/src/KinematicsConstraints.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include "KinematicsConstraints.h" - -namespace RAPTOR { - -KinematicsConstraints::KinematicsConstraints(std::shared_ptr& trajPtr_input, - const Model* model_input, - const Eigen::VectorXi& jtype_input, - const size_t joint_id_input, - const size_t time_id_input, - const Transform& desiredTransform_input, - const Transform endT_input) : - trajPtr_(trajPtr_input), - modelPtr_(model_input), - jtype(jtype_input), - joint_id(joint_id_input), - time_id(time_id_input), - endT(endT_input) { - fkPtr_ = std::make_unique(modelPtr_, jtype); - - if (joint_id > modelPtr_->nq) { - throw std::invalid_argument("joint_id should not be larger than model.nq"); - } - - if (time_id >= trajPtr_->N) { - throw std::invalid_argument("time_id should not be larger than number of instances in the trajectory"); - } - - desiredPosition = desiredTransform_input.p; - desiredRotation = desiredTransform_input.R; - - if (Utils::ifTwoMatrixEqual(desiredRotation, -desiredRotation.transpose())) { - throw std::invalid_argument("Input matrix is not skew-symmetric"); - } - - constrainPosition = true; - constrainRotation = true; - - initialize_memory(6, trajPtr_->varLength); -} - -KinematicsConstraints::KinematicsConstraints(std::shared_ptr& trajPtr_input, - const Model* model_input, - const Eigen::VectorXi& jtype_input, - const size_t joint_id_input, - const size_t time_id_input, - const Vec3& desiredPosition_input, - const Transform endT_input) : - trajPtr_(trajPtr_input), - modelPtr_(model_input), - jtype(jtype_input), - joint_id(joint_id_input), - time_id(time_id_input), - endT(endT_input) { - fkPtr_ = std::make_unique(modelPtr_, jtype); - - if (joint_id > modelPtr_->nq) { - throw std::invalid_argument("joint_id should not be larger than model.nq"); - } - - if (time_id >= trajPtr_->N) { - throw std::invalid_argument("time_id should not be larger than number of instances in the trajectory"); - } - - desiredPosition = desiredPosition_input; - - constrainPosition = true; - constrainRotation = false; - - initialize_memory(3, trajPtr_->varLength); -} - -KinematicsConstraints::KinematicsConstraints(std::shared_ptr& trajPtr_input, - const Model* model_input, - const Eigen::VectorXi& jtype_input, - const size_t joint_id_input, - const size_t time_id_input, - const Mat3& desiredRotation_input, - const Transform endT_input) : - trajPtr_(trajPtr_input), - modelPtr_(model_input), - jtype(jtype_input), - joint_id(joint_id_input), - time_id(time_id_input), - endT(endT_input) { - fkPtr_ = std::make_unique(modelPtr_, jtype); - - if (joint_id > modelPtr_->nq) { - throw std::invalid_argument("joint_id should not be larger than model.nq"); - } - - if (time_id >= trajPtr_->N) { - throw std::invalid_argument("time_id should not be larger than number of instances in the trajectory"); - } - - desiredRotation = desiredRotation_input; - - if (Utils::ifTwoMatrixEqual(desiredRotation, -desiredRotation.transpose())) { - throw std::invalid_argument("Input matrix is not skew-symmetric"); - } - - constrainPosition = false; - constrainRotation = true; - - initialize_memory(3, trajPtr_->varLength); -} - -void KinematicsConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - const VecX& q = trajPtr_->q(time_id); - const MatX& pq_pz = trajPtr_->pq_pz(time_id); - const Eigen::Array& pq_pz_pz = trajPtr_->pq_pz_pz.col(time_id); - - if (compute_hessian) { - fkPtr_->compute(0, joint_id, q, &startT, &endT, 2); - } - else if (compute_derivatives) { - fkPtr_->compute(0, joint_id, q, &startT, &endT, 1); - } - else { - fkPtr_->compute(0, joint_id, q, &startT, &endT, 0); - } - - MatX pg_pq; - Eigen::Array pg_pq_pq; - - if (constrainPosition) { - if (compute_hessian) { - g.head(3) = LieSpaceResidual::translationResidual(fkPtr_, desiredPosition, &pg_pq, &pg_pq_pq); - pg_pz.topRows(3) = pg_pq * pq_pz; - for (int i = 0; i < 3; i++) { - pg_pz_pz(i) = pq_pz.transpose() * pg_pq_pq(i) * pq_pz; - - for (int j = 0; j < pq_pz_pz.size(); j++) { - pg_pz_pz(i) += pg_pq(i, j) * pq_pz_pz(j); - } - } - - } - else if (compute_derivatives) { - g.head(3) = LieSpaceResidual::translationResidual(fkPtr_, desiredPosition, &pg_pq); - pg_pz.topRows(3) = pg_pq * pq_pz; - } - else { - g.head(3) = LieSpaceResidual::translationResidual(fkPtr_, desiredPosition); - } - } - - if (constrainRotation) { - if (compute_hessian) { - g.tail(3) = LieSpaceResidual::rotationResidual(fkPtr_, desiredRotation, &pg_pq, &pg_pq_pq); - pg_pz.bottomRows(3) = pg_pq * pq_pz; - - int startIdx = constrainPosition ? 3 : 0; - - for (int i = startIdx; i < startIdx + 3; i++) { - pg_pz_pz(i) = pq_pz.transpose() * pg_pq_pq(i - startIdx) * pq_pz; - - for (int j = 0; j < pq_pz_pz.size(); j++) { - pg_pz_pz(i) += pg_pq(i - startIdx, j) * pq_pz_pz(j); - } - } - } - else if (compute_derivatives) { - g.tail(3) = LieSpaceResidual::rotationResidual(fkPtr_, desiredRotation, &pg_pq); - pg_pz.bottomRows(3) = pg_pq * pq_pz; - } - else { - g.tail(3) = LieSpaceResidual::rotationResidual(fkPtr_, desiredRotation); - } - } -} - -void KinematicsConstraints::compute_bounds() { - g_lb = VecX::Zero(m); - g_ub = VecX::Zero(m); -} - -void KinematicsConstraints::print_violation_info() { - if (constrainPosition) { - const VecX& g_position = g.head(3); - - if (abs(g_position(0)) > 1e-5) { - std::cout << " Error on position x: " - << g_position(0) - << std::endl; - } - if (abs(g_position(1)) > 1e-5) { - std::cout << " Error on position y: " - << g_position(1) << std::endl; - } - if (abs(g_position(2)) > 1e-5) { - std::cout << " Error on position z: " - << g_position(2) << std::endl; - } - } - - if (constrainRotation) { - const VecX& g_rotation = g.tail(3); - - if (g_rotation.norm() > 1e-5) { - std::cout << " Error on rotation (norm of skew residual): " - << g_rotation.norm() << std::endl; - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Constraints/src/LieSpaceResidual.cpp b/Constraints/src/LieSpaceResidual.cpp deleted file mode 100644 index aa91a0f8..00000000 --- a/Constraints/src/LieSpaceResidual.cpp +++ /dev/null @@ -1,164 +0,0 @@ -#include "LieSpaceResidual.h" - -namespace RAPTOR { -namespace LieSpaceResidual { - -Eigen::Vector3d translationResidual(const std::unique_ptr& fkPtr_, - const Eigen::Vector3d& desiredPosition, - Eigen::MatrixXd* gradientPtr_, - Eigen::Array* hessianPtr_) { - if (gradientPtr_ != nullptr) { - *gradientPtr_ = fkPtr_->getTranslationJacobian(); - } - - if (hessianPtr_ != nullptr) { - fkPtr_->getTranslationHessian(*hessianPtr_); - } - - return fkPtr_->getTranslation() - desiredPosition; -} - -Eigen::Vector3d rotationResidual(const std::unique_ptr& fkPtr_, - const Eigen::Matrix3d& desiredRotation, - Eigen::MatrixXd* gradientPtr_, - Eigen::Array* hessianPtr_) { - bool compute_derivatives = (gradientPtr_ != nullptr) || - (hessianPtr_ != nullptr); - bool compute_hessian = (hessianPtr_ != nullptr); - - // kinematics chain (derivative is only related to these joints) - const auto& chain = fkPtr_->chain; - - const Eigen::Matrix3d currentRotation = fkPtr_->getRotation(); - Eigen::Matrix3d residualMatrix = desiredRotation.transpose() * currentRotation; - - Eigen::Array dRdq; - Eigen::Array ddRddq; - Eigen::Tensor dddRdddq; - if (compute_derivatives) { - fkPtr_->getRotationJacobian(dRdq); - - for (auto i : chain) { - dRdq(i) = desiredRotation.transpose() * dRdq(i); - } - - if (compute_hessian) { - fkPtr_->getRotationHessian(ddRddq); - - for (auto i : chain) { - for (auto j : chain) { - ddRddq(i, j) = desiredRotation.transpose() * ddRddq(i, j); - } - } - } - } - - double traceR = residualMatrix.trace(); - - Eigen::VectorXd dtraceRdq; - Eigen::MatrixXd ddtraceRddq; - Eigen::Tensor dddtraceRdddq; - if (compute_derivatives) { - dtraceRdq = Eigen::VectorXd::Zero(dRdq.size()); - for (auto i : chain) { - dtraceRdq(i) = dRdq(i).trace(); - } - - if (compute_hessian) { - ddtraceRddq = Eigen::MatrixXd::Zero(ddRddq.rows(), ddRddq.cols()); - for (auto i : chain) { - for (auto j : chain) { - ddtraceRddq(i, j) = ddRddq(i, j).trace(); - } - } - } - } - - double theta = HigherOrderDerivatives::safeacos((traceR - 1) / 2); - - Eigen::VectorXd dthetadq; - Eigen::MatrixXd ddthetaddq; - Eigen::Tensor dddthetadddq; - if (compute_derivatives) { - const double dacosdxTraceR = HigherOrderDerivatives::safedacosdx((traceR - 1) / 2); - - dthetadq = Eigen::VectorXd::Zero(dRdq.size()); - for (auto i : chain) { - dthetadq(i) = 0.5 * dacosdxTraceR * dtraceRdq(i); - } - - if (compute_hessian) { - const double ddacosddxTraceR = HigherOrderDerivatives::safeddacosddx((traceR - 1) / 2); - - ddthetaddq = Eigen::MatrixXd::Zero(ddRddq.rows(), ddRddq.cols()); - for (auto i : chain) { - for (auto j : chain) { - ddthetaddq(i, j) = 0.5 * - (0.5 * ddacosddxTraceR * dtraceRdq(i) * dtraceRdq(j) + - dacosdxTraceR * ddtraceRddq(i, j)); - } - } - } - } - - const double st = std::sin(theta); - const double ct = std::cos(theta); - const double xSinxTheta = HigherOrderDerivatives::safexSinx(theta); - const Eigen::Matrix3d RRT = residualMatrix - residualMatrix.transpose(); - - Eigen::Matrix3d logR = 0.5 * xSinxTheta * RRT; - - if (compute_derivatives) { - const double dxSinxTheta = HigherOrderDerivatives::safedxSinxdx(theta); - - if (gradientPtr_ != nullptr) { - Eigen::MatrixXd& gradient = *gradientPtr_; - gradient = Eigen::MatrixXd::Zero(3, dRdq.size()); - - for (auto i : chain) { - Eigen::Matrix3d temp1 = - dxSinxTheta * dthetadq(i) * RRT; - Eigen::Matrix3d temp2 = - xSinxTheta * (dRdq(i) - dRdq(i).transpose()); - gradient.col(i) = Utils::unskew(0.5 * (temp1 + temp2)); - } - } - - if (compute_hessian) { - const double ddxSinxddx = HigherOrderDerivatives::safeddxSinxddx(theta); - - Eigen::Array& hessian = *hessianPtr_; - hessian(0) = Eigen::MatrixXd::Zero(ddRddq.rows(), ddRddq.cols()); - hessian(1) = Eigen::MatrixXd::Zero(ddRddq.rows(), ddRddq.cols()); - hessian(2) = Eigen::MatrixXd::Zero(ddRddq.rows(), ddRddq.cols()); - - for (auto i : chain) { - for (auto j : chain) { - Eigen::Matrix3d temp1_1 = - ddxSinxddx * dthetadq(i) * dthetadq(j) * RRT; - Eigen::Matrix3d temp1_2 = - dxSinxTheta * ddthetaddq(i, j) * RRT; - Eigen::Matrix3d temp2 = - dxSinxTheta * dthetadq(i) * (dRdq(j) - dRdq(j).transpose()); - Eigen::Matrix3d temp3 = - dxSinxTheta * dthetadq(j) * (dRdq(i) - dRdq(i).transpose()); - Eigen::Matrix3d temp4 = - xSinxTheta * (ddRddq(i, j) - ddRddq(i, j).transpose()); - - Eigen::Vector3d h = - Utils::unskew( - 0.5 * ((temp1_1 + temp1_2) + temp2 + temp3 + temp4)); - - hessian(0)(i, j) = h(0); - hessian(1)(i, j) = h(1); - hessian(2)(i, j) = h(2); - } - } - } - } - - return Utils::unskew(logR); -} - -}; // namespace LieSpaceResidual -}; // namespace RAPTOR \ No newline at end of file diff --git a/Constraints/src/SurfaceContactConstraints.cpp b/Constraints/src/SurfaceContactConstraints.cpp deleted file mode 100644 index b2a150e7..00000000 --- a/Constraints/src/SurfaceContactConstraints.cpp +++ /dev/null @@ -1,127 +0,0 @@ -#include "SurfaceContactConstraints.h" - -namespace RAPTOR { - -SurfaceContactConstraints::SurfaceContactConstraints(std::shared_ptr& cidPtr_input, - const frictionParams& fp_input) : - cidPtr_(cidPtr_input), - fp(fp_input) { - // (1) positive contact force - // (2) translation friction cone - // (3) rotation friction cone - // (4, 5) ZMP on one axis - // (6, 7) ZMP on the other axis - m = cidPtr_->trajPtr_->N * 7; - - g = VecX::Zero(m); - g_lb = VecX::Zero(m); - g_ub = VecX::Zero(m); - pg_pz.resize(m, cidPtr_->trajPtr_->varLength); -} - -void SurfaceContactConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("SurfaceContactConstraints does not support hessian computation"); - } - - cidPtr_->compute(z, compute_derivatives); - - for (int i = 0; i < cidPtr_->trajPtr_->N; i++) { - // assume the contact wrench is always located at the end - const VecX& lambda = cidPtr_->lambda(i).tail(6); - - double contact_force = lambda(2); - double friction_force = sqrt(pow(lambda(0), 2) + pow(lambda(1), 2)); - double max_friction_force = fp.mu * contact_force; - double max_moment_z = fp.gamma * contact_force; - double mx_lower_limit = -fp.Lx * contact_force; - double mx_upper_limit = fp.Lx * contact_force; - double my_lower_limit = -fp.Ly * contact_force; - double my_upper_limit = fp.Ly * contact_force; - - // (1) positive contact force - g(i * 7 + 0) = contact_force; - - // (2) translation friction cone - g(i * 7 + 1) = friction_force - max_friction_force; - - // (3) rotation friction cone - g(i * 7 + 2) = abs(lambda(5)) - max_moment_z; - - // (4, 5) ZMP on one axis - g(i * 7 + 3) = lambda(3) - mx_upper_limit; - g(i * 7 + 4) = mx_lower_limit - lambda(3); - - // (6, 7) ZMP on the other axis - g(i * 7 + 5) = lambda(4) - my_upper_limit; - g(i * 7 + 6) = my_lower_limit - lambda(4); - - if (compute_derivatives) { - // assume the contact wrench is always located at the end - const MatX& plambda_pz = cidPtr_->plambda_pz(i).bottomRows(6); - - // (1) positive contact force - pg_pz.row(i * 7 + 0) = plambda_pz.row(2); - - // (2) translation friction cone - if (friction_force <= 1e-8) { - pg_pz.row(i * 7 + 1) = fp.mu * plambda_pz.row(2); - } - else { - pg_pz.row(i * 7 + 1) = (lambda(0) * plambda_pz.row(0) + - lambda(1) * plambda_pz.row(1)) / friction_force - - fp.mu * plambda_pz.row(2); - } - - // (3) rotation friction cone - pg_pz.row(i * 7 + 2) = Utils::sign(lambda(5)) * plambda_pz.row(5) - - fp.gamma * plambda_pz.row(2); - - // (4, 5) ZMP on one axis - pg_pz.row(i * 7 + 3) = plambda_pz.row(3) - fp.Lx * plambda_pz.row(2); - pg_pz.row(i * 7 + 4) = -fp.Lx * plambda_pz.row(2) - plambda_pz.row(3); - - // (6, 7) ZMP on the other axis - pg_pz.row(i * 7 + 5) = plambda_pz.row(4) - fp.Ly * plambda_pz.row(2); - pg_pz.row(i * 7 + 6) = -fp.Ly * plambda_pz.row(2) - plambda_pz.row(4); - } - } -} - -void SurfaceContactConstraints::compute_bounds() { - for (int i = 0; i < cidPtr_->trajPtr_->N; i++) { - // (1) positive contact force - g_lb(i * 7 + 0) = 0; - g_ub(i * 7 + 0) = 1e19; - - // (2) translation friction cone - g_lb(i * 7 + 1) = -1e19; - g_ub(i * 7 + 1) = 0; - - // (3) rotation friction cone - g_lb(i * 7 + 2) = -1e19; - g_ub(i * 7 + 2) = 0; - - // (4, 5) ZMP on one axis - g_lb(i * 7 + 3) = -1e19; - g_ub(i * 7 + 3) = 0; - - g_lb(i * 7 + 4) = -1e19; - g_ub(i * 7 + 4) = 0; - - // (6, 7) ZMP on the other axis - g_lb(i * 7 + 5) = -1e19; - g_ub(i * 7 + 5) = 0; - - g_lb(i * 7 + 6) = -1e19; - g_ub(i * 7 + 6) = 0; - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Constraints/src/TorqueLimits.cpp b/Constraints/src/TorqueLimits.cpp deleted file mode 100644 index 7c0668bf..00000000 --- a/Constraints/src/TorqueLimits.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include "TorqueLimits.h" - -namespace RAPTOR { - -TorqueLimits::TorqueLimits(std::shared_ptr& trajPtr_input, - std::shared_ptr idPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input) : - trajPtr_(trajPtr_input), - idPtr_(idPtr_input) { - lowerLimits = lowerLimits_input; - upperLimits = upperLimits_input; - - initialize_memory(trajPtr_->N * trajPtr_->Nact, - trajPtr_->varLength, - false); -} - -void TorqueLimits::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - if (compute_hessian) { - throw std::invalid_argument("TorqueLimits: Hessian not implemented yet"); - } - - idPtr_->compute(z, compute_derivatives, compute_hessian); - - for (int i = 0; i < trajPtr_->N; i++) { - g.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = idPtr_->tau(i); - - if (compute_derivatives) { - pg_pz.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, trajPtr_->varLength) = idPtr_->ptau_pz(i); - } - } -} - -void TorqueLimits::compute_bounds() { - for (int i = 0; i < trajPtr_->N; i++) { - g_lb.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = lowerLimits; - g_ub.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = upperLimits; - } -} - -void TorqueLimits::print_violation_info() { - for (int i = 0; i < trajPtr_->N; i++) { - for (int j = 0; j < trajPtr_->Nact; j++) { - if (g(i * trajPtr_->Nact + j) <= lowerLimits(j)) { - std::cout << " TorqueLimits.cpp: Joint " - << j - << " at time instance " - << i - << " is below lower limit: " - << g(i * trajPtr_->Nact + j) - << " < " - << lowerLimits(j) - << std::endl; - } - else if (g(i * trajPtr_->Nact + j) >= upperLimits(j)) { - std::cout << " TorqueLimits.cpp: Joint " - << j - << " at time instance " - << i - << " is above upper limit: " - << g(i * trajPtr_->Nact + j) - << " > " - << upperLimits(j) - << std::endl; - } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Constraints/src/VelocityLimits.cpp b/Constraints/src/VelocityLimits.cpp deleted file mode 100644 index a0fc374c..00000000 --- a/Constraints/src/VelocityLimits.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "VelocityLimits.h" - -namespace RAPTOR { - -VelocityLimits::VelocityLimits(std::shared_ptr& trajPtr_input, - const VecX& lowerLimits_input, - const VecX& upperLimits_input) : - lowerLimits(lowerLimits_input), - upperLimits(upperLimits_input) { - trajPtr_ = trajPtr_input; - - if (lowerLimits.size() != upperLimits.size()) { - throw std::invalid_argument("lowerLimits and upperLimits must be the same size"); - } - - if (lowerLimits.size() != trajPtr_->Nact) { - throw std::invalid_argument("lowerLimits and upperLimits must be the same size as the number of actuated joints"); - } - - initialize_memory(trajPtr_->N * trajPtr_->Nact, - trajPtr_->varLength); -} - -void VelocityLimits::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - for (int i = 0; i < trajPtr_->N; i++) { - g.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = trajPtr_->q_d(i).head(trajPtr_->Nact); - - if (compute_derivatives) { - pg_pz.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, trajPtr_->varLength) = trajPtr_->pq_d_pz(i); - } - - if (compute_hessian) { - for (int j = 0; j < trajPtr_->Nact; j++) { - pg_pz_pz(i * trajPtr_->Nact + j) = trajPtr_->pq_d_pz_pz(j, i); - } - } - } -} - -void VelocityLimits::compute_bounds() { - for (int i = 0; i < trajPtr_->N; i++) { - g_lb.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = lowerLimits; - g_ub.block(i * trajPtr_->Nact, 0, trajPtr_->Nact, 1) = upperLimits; - } -} - -void VelocityLimits::print_violation_info() { - for (int i = 0; i < trajPtr_->N; i++) { - for (int j = 0; j < trajPtr_->Nact; j++) { - if (g(i * trajPtr_->Nact + j) <= g_lb(i * trajPtr_->Nact + j)) { - std::cout << " VelocityLimits.cpp: Joint " - << j - << " at time instance " - << i - << " is below lower limit: " - << g(i * trajPtr_->Nact + j) - << " < " - << lowerLimits(j) - << std::endl; - } - if (g(i * trajPtr_->Nact + j) >= g_ub(i * trajPtr_->Nact + j)) { - std::cout << " VelocityLimits.cpp: Joint " - << j - << " at time instance " - << i - << " is above upper limit: " - << g(i * trajPtr_->Nact + j) - << " > " - << upperLimits(j) - << std::endl; - } - } - } -} - -}; // namespace RAPTOR diff --git a/Constraints/src/WaitrContactConstraints.cpp b/Constraints/src/WaitrContactConstraints.cpp deleted file mode 100644 index a9b1d813..00000000 --- a/Constraints/src/WaitrContactConstraints.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include "WaitrContactConstraints.h" - -namespace RAPTOR { - -WaitrContactConstraints::WaitrContactConstraints(std::shared_ptr& idPtr_input, - const contactSurfaceParams& csp_input) : - idPtr_(idPtr_input), - csp(csp_input) { - // (1) positive contact force - // (2) translation friction cone - // (3, 4) ZMP on one axis - // (5, 6) ZMP on the other axis - m = idPtr_->trajPtr_->N * 6; // TODO: this is the number of constraints for all time instances - - g = VecX::Zero(m); - g_lb = VecX::Zero(m); - g_ub = VecX::Zero(m); - pg_pz.resize(m, idPtr_->trajPtr_->varLength); -} - -void WaitrContactConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("WaitrContactConstraints does not support hessian computation"); - } - - idPtr_->compute(z, compute_derivatives); - - for (int i = 0; i < idPtr_->trajPtr_->N; i++) { - // access the last contact wrench, - // which should be the contact wrench between the end effector and the object - const Vec6& lambda = idPtr_->lambda(i); - - const Vec3& rotation_torque = lambda.head(3); - const Vec3& translation_force = lambda.tail(3); - - double contact_force = translation_force(2) + csp.maxSuctionForce; - double friction_force = sqrt(pow(translation_force(0), 2) + pow(translation_force(1), 2)); - double max_friction_force = csp.mu * contact_force; - double mx_lower_limit = -csp.Lx * contact_force; - double mx_upper_limit = csp.Lx * contact_force; - double my_lower_limit = -csp.Ly * contact_force; - double my_upper_limit = csp.Ly * contact_force; - - // (1) positive contact force - g(i * 6 + 0) = contact_force; - - // (2) translation friction cone - g(i * 6 + 1) = friction_force - max_friction_force; - - // (3, 4) ZMP on one axis - g(i * 6 + 2) = rotation_torque(0) - mx_upper_limit; - g(i * 6 + 3) = mx_lower_limit - rotation_torque(0); - - // (5, 6) ZMP on the other axis - g(i * 6 + 4) = rotation_torque(1) - my_upper_limit; - g(i * 6 + 5) = my_lower_limit - rotation_torque(1); - - if (compute_derivatives) { - // assume the contact wrench is always located at the end - const MatX& protation_torque_pz = idPtr_->plambda_pz(i).topRows(3); - const MatX& ptranslation_force_pz = idPtr_->plambda_pz(i).bottomRows(3); - - // (1) positive contact force - pg_pz.row(i * 6 + 0) = ptranslation_force_pz.row(2); - - // (2) translation friction cone - if (friction_force <= 1e-3) { // avoid singularity when friction_force is close to 0 - pg_pz.row(i * 6 + 1) = csp.mu * ptranslation_force_pz.row(2); - } - else { - pg_pz.row(i * 6 + 1) = (translation_force(0) * ptranslation_force_pz.row(0) + - translation_force(1) * ptranslation_force_pz.row(1)) / friction_force - - csp.mu * ptranslation_force_pz.row(2); - } - - // // (3, 4) ZMP on one axis - pg_pz.row(i * 6 + 2) = protation_torque_pz.row(0) - csp.Lx * ptranslation_force_pz.row(2); - pg_pz.row(i * 6 + 3) = -csp.Lx * ptranslation_force_pz.row(2) - protation_torque_pz.row(0); - - // (5, 6) ZMP on the other axis - pg_pz.row(i * 6 + 4) = protation_torque_pz.row(1) - csp.Ly * ptranslation_force_pz.row(2); - pg_pz.row(i * 6 + 5) = -csp.Ly * ptranslation_force_pz.row(2) - protation_torque_pz.row(1); - } - } -} - -void WaitrContactConstraints::compute_bounds() { - for (int i = 0; i < idPtr_->trajPtr_->N; i++) { - // (1) positive contact force - g_lb(i * 6 + 0) = 0; - g_ub(i * 6 + 0) = 1e19; - - // (2) translation friction cone - g_lb(i * 6 + 1) = -1e19; - g_ub(i * 6 + 1) = 0; - - // (3, 4) ZMP on one axis - g_lb(i * 6 + 2) = -1e19; - g_ub(i * 6 + 2) = 0; - - g_lb(i * 6 + 3) = -1e19; - g_ub(i * 6 + 3) = 0; - - // (5, 6) ZMP on the other axis - g_lb(i * 6 + 4) = -1e19; - g_ub(i * 6 + 4) = 0; - - g_lb(i * 6 + 5) = -1e19; - g_ub(i * 6 + 5) = 0; - } -} - -void WaitrContactConstraints::print_violation_info() { - for (int i = 0; i < idPtr_->trajPtr_->N; i++) { - if (g(i * 6 + 0) <= 0) { - std::cout << " WaitrContactConstraints.cpp: Negative contact force at time instance " - << i - << std::endl; - } - if (g(i * 6 + 1) >= 0) { - std::cout << " WaitrContactConstraints.cpp: Translational friction out of friction cone at time instance " - << i - << std::endl; - } - if (g(i * 6 + 2) >= 0) { - std::cout << " WaitrContactConstraints.cpp: Object peel off over x dimension at time instance " - << i - << std::endl; - } - if (g(i * 6 + 3) >= 0) { - std::cout << " WaitrContactConstraints.cpp: Object peel off over x dimension at time instance " - << i - << std::endl; - } - if (g(i * 6 + 4) >= 0) { - std::cout << " WaitrContactConstraints.cpp: Object peel off over y dimension at time instance " - << i - << std::endl; - } - if (g(i * 6 + 5) >= 0) { - std::cout << " WaitrContactConstraints.cpp: Object peel off over y dimension at time instance " - << i - << std::endl; - } - } -} - -}; // namespace RAPTOR - diff --git a/Constraints/src/ZonotopeCollisionAvoidance.cpp b/Constraints/src/ZonotopeCollisionAvoidance.cpp deleted file mode 100644 index d7ed895c..00000000 --- a/Constraints/src/ZonotopeCollisionAvoidance.cpp +++ /dev/null @@ -1,414 +0,0 @@ -#include "ZonotopeCollisionAvoidance.h" - -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED -// DEPRECATED - -namespace RAPTOR { - -ZonotopeCollisionAvoidance::ZonotopeCollisionAvoidance(const std::vector& zonotopeCenters_input, - const Eigen::Array& zonotopeGenerators_input) : - zonotopeCenters(zonotopeCenters_input), - zonotopeGenerators(zonotopeGenerators_input) { - for (int i = 0; i < zonotopeGenerators.size(); i++) { - auto it = zonotopeGenerators[i]; - if (it.rows() != 3 || it.cols() != MAX_OBSTACLE_GENERATOR_NUM) { - throw std::invalid_argument("Zonotope generator matrix should be 3 x " + std::to_string(MAX_OBSTACLE_GENERATOR_NUM)); - } - } - - numObstacles = zonotopeCenters.size(); - distances.resize(numObstacles); - - initialize(); -} - -void ZonotopeCollisionAvoidance::initialize() { - // allocate memory for A, b, v - A = new double[numObstacles * HYPERPLANE_NUM * 3]; - b = new double[numObstacles * HYPERPLANE_NUM]; - v_start_idx = new int[numObstacles]; - v_size = new int[numObstacles]; - v = new double[numObstacles * VERTICES_NUM * 3]; - - // combination index - int combA[COMB_NUM], combB[COMB_NUM]; - int a_id = 0, b_id = 1; - for (int i = 0; i < COMB_NUM; i++) { - combA[i] = a_id; - combB[i] = b_id; - - if (b_id < MAX_OBSTACLE_GENERATOR_NUM - 1) { - b_id++; - } - else { - a_id++; - b_id = a_id + 1; - } - } - - // compute hyperplane representation - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - for (int plane_id = 0; plane_id < COMB_NUM; plane_id++) { - int a_id = combA[plane_id]; - int b_id = combB[plane_id]; - - const Vec3& c = zonotopeCenters[obs_id]; - const MatX& G = zonotopeGenerators[obs_id]; - - double generator_cross[3] = {G(1, a_id) * G(2, b_id) - G(2, a_id) * G(1, b_id), - G(2, a_id) * G(0, b_id) - G(0, a_id) * G(2, b_id), - G(0, a_id) * G(1, b_id) - G(1, a_id) * G(0, b_id)}; - - double norm_cross = sqrt(pow(generator_cross[0], 2) + - pow(generator_cross[1], 2) + - pow(generator_cross[2], 2)); - - double C[3] = {0.0}; - - if (norm_cross > 0) { - C[0] = generator_cross[0] / norm_cross; - C[1] = generator_cross[1] / norm_cross; - C[2] = generator_cross[2] / norm_cross; - } - - // A = C - A[(obs_id * HYPERPLANE_NUM + plane_id) * 3 + 0] = C[0]; - A[(obs_id * HYPERPLANE_NUM + plane_id) * 3 + 1] = C[1]; - A[(obs_id * HYPERPLANE_NUM + plane_id) * 3 + 2] = C[2]; - A[(obs_id * HYPERPLANE_NUM + plane_id + COMB_NUM) * 3 + 0] = -C[0]; - A[(obs_id * HYPERPLANE_NUM + plane_id + COMB_NUM) * 3 + 1] = -C[1]; - A[(obs_id * HYPERPLANE_NUM + plane_id + COMB_NUM) * 3 + 2] = -C[2]; - - // d = C * c - double d = C[0] * c[0] + C[1] * c[1] + C[2] * c[2]; - - // delta = sum(abs(C * G)) - double delta = 0.0; - for (int j = 0; j < MAX_OBSTACLE_GENERATOR_NUM; j++) { - delta += fabs(C[0] * G(0, j) + C[1] * G(1, j) + C[2] * G(2, j)); - } - - b[(obs_id * HYPERPLANE_NUM + plane_id)] = d + delta; - b[(obs_id * HYPERPLANE_NUM + plane_id + COMB_NUM)] = -d + delta; - } - } - - // compute vertices - int dim = 3; - int v_cursor = 0; - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - const Vec3& c = zonotopeCenters[obs_id]; - const MatX& G = zonotopeGenerators[obs_id]; - std::vector vertices; - vertices.push_back(c[0]); - vertices.push_back(c[1]); - vertices.push_back(c[2]); - for (int i = 0; i < MAX_OBSTACLE_GENERATOR_NUM; ++i) { - std::vector vertices_new; - for (int j = 0; j < vertices.size() / dim; ++j) { - for (int k = 0; k < dim; ++k) { - vertices_new.push_back(vertices[j * dim + k] + G(k, i)); - } - for (int k = 0; k < dim; ++k) { - vertices_new.push_back(vertices[j * dim + k] - G(k, i)); - } - } - vertices = vertices_new; - if (i >= dim) { - vertices_new.clear(); - try { - orgQhull::Qhull qhull("", dim, vertices.size() / 3, vertices.data(), ""); - for(const orgQhull::QhullVertex &v: qhull.vertexList()) { - const orgQhull::QhullPoint &qhullPt = v.point(); - auto coords = qhullPt.coordinates(); - vertices_new.push_back(coords[0]); - vertices_new.push_back(coords[1]); - vertices_new.push_back(coords[2]); - } - } - catch (...) { - std::cerr << "Qhull failed!" << std::endl; - vertices_new = vertices; - } - vertices = vertices_new; - } - } - if (obs_id == 0) { - v_start_idx[obs_id] = 0; - } - else { - v_start_idx[obs_id] = v_start_idx[obs_id - 1] + v_size[obs_id - 1]; - } - - v_size[obs_id] = vertices.size() / 3; - - for (auto it : vertices) { - v[v_cursor++] = it; - } - } -} - -void ZonotopeCollisionAvoidance::computeDistance(const Vec3& point) { - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - // compute distances to hyperplanes - double Apprime_minus_b[HYPERPLANE_NUM]; - double perpendicular_distances = 1e19; - int perpendicular_distances_id = 0; - double projected_points[3] = {0.0}; - - for (int p_id = 0; p_id < HYPERPLANE_NUM; p_id++) { - const double A_elt_x = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 0]; - const double A_elt_y = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 1]; - const double A_elt_z = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 2]; - const double b_elt = b[obs_id * HYPERPLANE_NUM + p_id]; - - const double A_elt_norm = fabs(A_elt_x) + fabs(A_elt_y) + fabs(A_elt_z); - - if (A_elt_norm > 0) { - Apprime_minus_b[p_id] = A_elt_x * point(0) + - A_elt_y * point(1) + - A_elt_z * point(2) - b_elt; - } - else { - Apprime_minus_b[p_id] = -1e19; - } - } - - // find the maximum distance to hyperplanes - double max_elt = -1e19; - int max_id = 0; - - for (int i = 0; i < HYPERPLANE_NUM; i++) { - if (Apprime_minus_b[i] > max_elt) { - max_elt = Apprime_minus_b[i]; - max_id = i; - } - } - - if (max_elt <= 0) { - // if the distance is negative, then the sphere is already in collision and directly assign the distance - distances(obs_id) = max_elt; - break; - } - else { - // if the distance is positive, need to compute the distance to the face - perpendicular_distances = max_elt; - perpendicular_distances_id = max_id; - - // compute the projected point - const double* max_A_elt = A + (obs_id * HYPERPLANE_NUM + max_id) * 3; - projected_points[0] = point(0) - max_elt * max_A_elt[0]; - projected_points[1] = point(1) - max_elt * max_A_elt[1]; - projected_points[2] = point(2) - max_elt * max_A_elt[2]; - } - - - // determine whether the projected point is on the face - for (int p_id = 0; p_id < HYPERPLANE_NUM; p_id++) { - const double A_elt_x = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 0]; - const double A_elt_y = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 1]; - const double A_elt_z = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 2]; - const double b_elt = b[obs_id * HYPERPLANE_NUM + p_id]; - - const double A_elt_norm = fabs(A_elt_x) + fabs(A_elt_y) + fabs(A_elt_z); - if (A_elt_norm > 0) { - Apprime_minus_b[p_id] = A_elt_x * projected_points[0] + - A_elt_y * projected_points[1] + - A_elt_z * projected_points[2] - b_elt; - } - else { - Apprime_minus_b[p_id] = -1e19; - } - } - - // check if the projected point is on the face - bool on_zonotope = true; - for (int i = 0; i < HYPERPLANE_NUM; i++) { - if (Apprime_minus_b[i] > 1e-4) { - on_zonotope = false; - break; - } - } - - if (on_zonotope) { - // if projected point is on the face, then the distance is the perpendicular distance - const double max_elt = perpendicular_distances; - const int max_id = perpendicular_distances_id; - - distances(obs_id) = max_elt; - } - else { - // if the projected point is not on the face, then the distance is the minimum distance to the vertices corresponding to the face - const int selected_v_start_idx = v_start_idx[obs_id]; - const int selected_v_size = v_size[obs_id]; - double min_distance = 1e19; - int min_v_id = 0; - - for (int i = 0; i < selected_v_size; i++) { - const double v_x = v[(selected_v_start_idx + i) * 3 + 0]; - const double v_y = v[(selected_v_start_idx + i) * 3 + 1]; - const double v_z = v[(selected_v_start_idx + i) * 3 + 2]; - - const double distance = sqrt(pow(point(0) - v_x, 2) + - pow(point(1) - v_y, 2) + - pow(point(2) - v_z, 2)); - - if (distance < min_distance) { - min_distance = distance; - min_v_id = i; - } - } - - distances(obs_id) = min_distance; - } - } -} - -void ZonotopeCollisionAvoidance::computeDistance(const Vec3& point, const MatX& ppoint_pz) { - if (ppoint_pz.rows() != 3) { - throw std::invalid_argument("ppoint_pz should have 3 rows"); - } - - pdistances_pz.resize(numObstacles, ppoint_pz.cols()); - - for (int obs_id = 0; obs_id < numObstacles; obs_id++) { - // compute distances to hyperplanes - double Apprime_minus_b[HYPERPLANE_NUM]; - double perpendicular_distances = 1e19; - int perpendicular_distances_id = 0; - double projected_points[3] = {0.0}; - - for (int p_id = 0; p_id < HYPERPLANE_NUM; p_id++) { - const double A_elt_x = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 0]; - const double A_elt_y = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 1]; - const double A_elt_z = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 2]; - const double b_elt = b[obs_id * HYPERPLANE_NUM + p_id]; - - const double A_elt_norm = fabs(A_elt_x) + fabs(A_elt_y) + fabs(A_elt_z); - - if (A_elt_norm > 0) { - Apprime_minus_b[p_id] = A_elt_x * point(0) + - A_elt_y * point(1) + - A_elt_z * point(2) - b_elt; - } - else { - Apprime_minus_b[p_id] = -1e19; - } - } - - // find the maximum distance to hyperplanes - double max_elt = -1e19; - int max_id = 0; - - for (int i = 0; i < HYPERPLANE_NUM; i++) { - if (Apprime_minus_b[i] > max_elt) { - max_elt = Apprime_minus_b[i]; - max_id = i; - } - } - - if (max_elt <= 0) { - // if the distance is negative, then the sphere is already in collision and directly assign the distance - distances(obs_id) = max_elt; - - const double* max_A_elt = A + (obs_id * HYPERPLANE_NUM + max_id) * 3; - - pdistances_pz.row(obs_id) = max_A_elt[0] * ppoint_pz.row(0) + - max_A_elt[1] * ppoint_pz.row(1) + - max_A_elt[2] * ppoint_pz.row(2); - - break; - } - else { - // if the distance is positive, need to compute the distance to the face - perpendicular_distances = max_elt; - perpendicular_distances_id = max_id; - - // compute the projected point - const double* max_A_elt = A + (obs_id * HYPERPLANE_NUM + max_id) * 3; - projected_points[0] = point(0) - max_elt * max_A_elt[0]; - projected_points[1] = point(1) - max_elt * max_A_elt[1]; - projected_points[2] = point(2) - max_elt * max_A_elt[2]; - } - - - // determine whether the projected point is on the face - for (int p_id = 0; p_id < HYPERPLANE_NUM; p_id++) { - const double A_elt_x = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 0]; - const double A_elt_y = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 1]; - const double A_elt_z = A[(obs_id * HYPERPLANE_NUM + p_id) * 3 + 2]; - const double b_elt = b[obs_id * HYPERPLANE_NUM + p_id]; - - const double A_elt_norm = fabs(A_elt_x) + fabs(A_elt_y) + fabs(A_elt_z); - if (A_elt_norm > 0) { - Apprime_minus_b[p_id] = A_elt_x * projected_points[0] + - A_elt_y * projected_points[1] + - A_elt_z * projected_points[2] - b_elt; - } - else { - Apprime_minus_b[p_id] = -1e19; - } - } - - // check if the projected point is on the face - bool on_zonotope = true; - for (int i = 0; i < HYPERPLANE_NUM; i++) { - if (Apprime_minus_b[i] > 1e-4) { - on_zonotope = false; - break; - } - } - - if (on_zonotope) { - // if projected point is on the face, then the distance is the perpendicular distance - const double max_elt = perpendicular_distances; - const int max_id = perpendicular_distances_id; - - distances(obs_id) = max_elt; - - const double* max_A_elt = A + (obs_id * HYPERPLANE_NUM + max_id) * 3; - - pdistances_pz.row(obs_id) = max_A_elt[0] * ppoint_pz.row(0) + - max_A_elt[1] * ppoint_pz.row(1) + - max_A_elt[2] * ppoint_pz.row(2); - } - else { - // if the projected point is not on the face, then the distance is the minimum distance to the vertices corresponding to the face - const int selected_v_start_idx = v_start_idx[obs_id]; - const int selected_v_size = v_size[obs_id]; - double min_distance = 1e19; - int min_v_id = 0; - - for (int i = 0; i < selected_v_size; i++) { - const double v_x = v[(selected_v_start_idx + i) * 3 + 0]; - const double v_y = v[(selected_v_start_idx + i) * 3 + 1]; - const double v_z = v[(selected_v_start_idx + i) * 3 + 2]; - - const double distance = sqrt(pow(point(0) - v_x, 2) + - pow(point(1) - v_y, 2) + - pow(point(2) - v_z, 2)); - - if (distance < min_distance) { - min_distance = distance; - min_v_id = i; - } - } - - distances(obs_id) = min_distance; - - const double v_x = v[(selected_v_start_idx + min_v_id) * 3 + 0]; - const double v_y = v[(selected_v_start_idx + min_v_id) * 3 + 1]; - const double v_z = v[(selected_v_start_idx + min_v_id) * 3 + 2]; - - pdistances_pz.row(obs_id) = ((point(0) - v_x) * ppoint_pz.row(0) + - (point(1) - v_y) * ppoint_pz.row(1) + - (point(2) - v_z) * ppoint_pz.row(2)) / min_distance; - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Constraints/src/fclCollisionAvoidance.cpp b/Constraints/src/fclCollisionAvoidance.cpp deleted file mode 100644 index b02c375c..00000000 --- a/Constraints/src/fclCollisionAvoidance.cpp +++ /dev/null @@ -1,283 +0,0 @@ -#include "fclCollisionAvoidance.h" - -namespace RAPTOR { - -bool CustomizedDistanceFunction(fcl::CollisionObjectd* o1, - fcl::CollisionObjectd* o2, - void* cdata_, - double& dist) { - assert(cdata_ != nullptr); - auto* cdata = static_cast(cdata_); - const fcl::DistanceRequestd& request = cdata->request; - fcl::DistanceResultd& result = cdata->result; - - if (cdata->done) { - dist = result.min_distance; - return true; - } - - const fcl::NODE_TYPE o1type = o1->getNodeType(); - const fcl::NODE_TYPE o2type = o2->getNodeType(); - dist = 0; - std::string name1, name2; - - if (o1type == fcl::GEOM_BOX && - o2type == fcl::GEOM_SPHERE) { - const auto sphereGeomtry = static_cast(o2->collisionGeometry().get()); - const double sphere_radius = sphereGeomtry->radius; - - dist = computeBoxPointDistance(o1, o2->getTranslation()) - sphere_radius; - - const auto userData1 = (customizedUserDataForBox*)(o1->getUserData()); - const auto userData2 = (customizedUserDataForSphere*)(o2->getUserData()); - name1 = userData1->name; - name2 = userData2->name; - } - else if (o1type == fcl::GEOM_SPHERE && - o2type == fcl::GEOM_BOX) { - const auto sphereGeomtry = static_cast(o1->collisionGeometry().get()); - const double sphere_radius = sphereGeomtry->radius; - - dist = computeBoxPointDistance(o2, o1->getTranslation()) - sphere_radius; - - const auto userData1 = (customizedUserDataForSphere*)(o1->getUserData()); - const auto userData2 = (customizedUserDataForBox*)(o2->getUserData()); - name1 = userData1->name; - name2 = userData2->name; - } - else if (o1type == fcl::GEOM_SPHERE && - o2type == fcl::GEOM_SPHERE) { - const auto userData1 = (customizedUserDataForSphere*)(o1->getUserData()); - const auto userData2 = (customizedUserDataForSphere*)(o2->getUserData()); - - // we don't check collision between adajacent links - if (std::abs(userData1->linkId - userData2->linkId) <= 1) { - dist = std::numeric_limits::max(); - } - else { - const auto sphereGeomtry1 = static_cast(o1->collisionGeometry().get()); - const auto sphereGeomtry2 = static_cast(o2->collisionGeometry().get()); - const double sphere_radius1 = sphereGeomtry1->radius; - const double sphere_radius2 = sphereGeomtry2->radius; - - dist = (o1->getTranslation() - o2->getTranslation()).norm() - (sphere_radius1 + sphere_radius2); - } - - name1 = userData1->name; - name2 = userData2->name; - } - else { - std::cerr << "The collision objects should be box-sphere or sphere-sphere!\n"; - std::cerr << "Other differentiable collision checking is not supported yet.\n"; - throw std::invalid_argument("The collision objects should be box-sphere or sphere-sphere!"); - } - - if (dist < result.min_distance) { - result.min_distance = dist; - cdata->min_distance = dist; - cdata->name1 = name1; - cdata->name2 = name2; - } - - if (dist <= 0) { - return true; // in collision or in touch - } - - return cdata->done; -} - -bool CustomizedDistanceFunctionDerivative(fcl::CollisionObjectd* o1, - fcl::CollisionObjectd* o2, - void* cdata_, - double& dist) { - assert(cdata_ != nullptr); - auto* cdata = static_cast(cdata_); - const fcl::DistanceRequestd& request = cdata->request; - fcl::DistanceResultd& result = cdata->result; - - if (cdata->done) { - dist = result.min_distance; - return true; - } - - const fcl::NODE_TYPE o1type = o1->getNodeType(); - const fcl::NODE_TYPE o2type = o2->getNodeType(); - dist = 0; - Eigen::VectorXd pdistance_pz; - std::string name1, name2; - - if (o1type == fcl::GEOM_BOX && - o2type == fcl::GEOM_SPHERE) { - const auto userData1 = (customizedUserDataForBox*)(o1->getUserData()); - const auto userData2 = (customizedUserDataForSphere*)(o2->getUserData()); - const Eigen::MatrixXd& ppoint_pz = userData2->ppoint_pz; - const auto sphereGeomtry = static_cast(o2->collisionGeometry().get()); - const double sphere_radius = sphereGeomtry->radius; - - std::pair distRes = - computeBoxPointDistance(o1, o2->getTranslation(), ppoint_pz); - - dist = distRes.first - sphere_radius; - pdistance_pz = distRes.second; - - name1 = userData1->name; - name2 = userData2->name; - } - else if (o1type == fcl::GEOM_SPHERE && - o2type == fcl::GEOM_BOX) { - const auto userData1 = (customizedUserDataForSphere*)(o1->getUserData()); - const auto userData2 = (customizedUserDataForBox*)(o2->getUserData()); - const Eigen::MatrixXd& ppoint_pz = userData1->ppoint_pz; - const auto sphereGeomtry = static_cast(o1->collisionGeometry().get()); - const double sphere_radius = sphereGeomtry->radius; - - std::pair distRes = - computeBoxPointDistance(o2, o1->getTranslation(), ppoint_pz); - - dist = distRes.first - sphere_radius; - pdistance_pz = distRes.second; - - name1 = userData1->name; - name2 = userData2->name; - } - else if (o1type == fcl::GEOM_SPHERE && - o2type == fcl::GEOM_SPHERE) { - const auto userData1 = (customizedUserDataForSphere*)(o1->getUserData()); - const auto userData2 = (customizedUserDataForSphere*)(o2->getUserData()); - - // we don't check collision between adajacent links - if (std::abs(userData1->linkId - userData2->linkId) <= 1) { - dist = std::numeric_limits::max(); - } - else { - const Eigen::MatrixXd& ppoint_pz1 = userData1->ppoint_pz; - const Eigen::MatrixXd& ppoint_pz2 = userData2->ppoint_pz; - const auto sphereGeomtry1 = static_cast(o1->collisionGeometry().get()); - const auto sphereGeomtry2 = static_cast(o2->collisionGeometry().get()); - const double sphere_radius1 = sphereGeomtry1->radius; - const double sphere_radius2 = sphereGeomtry2->radius; - - dist = (o1->getTranslation() - o2->getTranslation()).norm(); - - if (dist > 1e-5) { - pdistance_pz = (o1->getTranslation() - o2->getTranslation()).transpose() * - (ppoint_pz1 - ppoint_pz2) / dist; - } - else { - pdistance_pz.setZero(); - } - - dist -= (sphere_radius1 + sphere_radius2); - } - - name1 = userData1->name; - name2 = userData2->name; - } - else { - std::cerr << "The collision objects should be box-sphere or sphere-sphere!\n"; - std::cerr << "Other differentiable collision checking is not supported yet.\n"; - throw std::invalid_argument("The collision objects should be box-sphere or sphere-sphere!"); - } - - if (dist < result.min_distance) { - result.min_distance = dist; - cdata->min_distance = dist; - cdata->name1 = name1; - cdata->name2 = name2; - cdata->pmin_distance_pz = pdistance_pz; - } - - if (dist <= 0) { - return true; // in collision or in touch - } - - return cdata->done; -} - -void fclCollisionAvoidance::addObstacleBox(const std::string& name, - const Vec3& boxCenter, - const Vec3& boxOrientation, - const Vec3& boxSize) { - std::shared_ptr boxGeomtry(new fcl::Boxd(boxSize(0), - boxSize(1), - boxSize(2))); - - Mat3 R = (Eigen::AngleAxisd(boxOrientation[0], Vec3::UnitX()) - * Eigen::AngleAxisd(boxOrientation[1], Vec3::UnitY()) - * Eigen::AngleAxisd(boxOrientation[2], Vec3::UnitZ())).matrix(); - - fclObjectCollection[name] = new fcl::CollisionObjectd(boxGeomtry, - R, - boxCenter); - - fclObjectCollection[name]->setUserData((void*)(new customizedUserDataForBox(name))); - fclObjectCollection[name]->computeAABB(); - - fclBroadPhaseManagerPtr_->registerObject(fclObjectCollection[name]); - fclBroadPhaseManagerPtr_->setup(); -} - -void fclCollisionAvoidance::addRobotSphere(const std::string& name, - const int linkId, - const Vec3& sphereCenter, - const double sphereRadius, - const MatX& ppoint_pz) { - std::shared_ptr sphereGeomtry(new fcl::Sphered(sphereRadius)); - fclObjectCollection[name] = new fcl::CollisionObjectd(sphereGeomtry, - Mat3::Identity(), - sphereCenter); - - fclObjectCollection[name]->setUserData((void*)(new customizedUserDataForSphere(name, linkId, ppoint_pz))); - fclObjectCollection[name]->computeAABB(); - - fclBroadPhaseManagerPtr_->registerObject(fclObjectCollection[name]); - fclBroadPhaseManagerPtr_->setup(); -} - -void fclCollisionAvoidance::clear() { - fclBroadPhaseManagerPtr_->clear(); - - for (const auto& object : fclObjectCollection) { - if (object.second->getNodeType() == fcl::NODE_TYPE::GEOM_BOX) { - delete (customizedUserDataForBox*)(object.second->getUserData()); - } - else if (object.second->getNodeType() == fcl::NODE_TYPE::GEOM_SPHERE) { - delete (customizedUserDataForSphere*)(object.second->getUserData()); - } - else { - throw std::runtime_error("The collision object should be a box or a sphere!"); - } - delete object.second; - } - - fclObjectCollection.clear(); -} - -void fclCollisionAvoidance::computeDistance(bool compute_derivatives) { - distanceData.reset(); - if (compute_derivatives) { - fclBroadPhaseManagerPtr_->distance(&distanceData, - &CustomizedDistanceFunctionDerivative); - } - else { - fclBroadPhaseManagerPtr_->distance(&distanceData, - &CustomizedDistanceFunction); - } -} - -void fclCollisionAvoidance::computeDistance(const std::shared_ptr& otherCollisionManager, - bool compute_derivatives) { - distanceData.reset(); - if (compute_derivatives) { - fclBroadPhaseManagerPtr_->distance(otherCollisionManager->fclBroadPhaseManagerPtr_.get(), - &distanceData, - &CustomizedDistanceFunctionDerivative); - } - else { - fclBroadPhaseManagerPtr_->distance(otherCollisionManager->fclBroadPhaseManagerPtr_.get(), - &distanceData, - &CustomizedDistanceFunction); - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit-modified/DigitModifiedSingleStep.cpp b/Examples/Digit-modified/DigitModifiedSingleStep.cpp deleted file mode 100644 index 7e2728e7..00000000 --- a/Examples/Digit-modified/DigitModifiedSingleStep.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include "DigitModifiedSingleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include -#include - -using namespace RAPTOR; -using namespace DigitModified; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit-modified/data/"; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - const std::string urdf_filename = "../Robots/digit-v3-modified/digit-v3-modified.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 3, 3, 3, - 3, 3, -3, 3, 3, 3, 3; - - // ignore all motor dynamics - model.rotorInertia.setZero(); - model.damping.setZero(); - model.friction.setZero(); - - // load settings - YAML::Node config; - - const double T = 0.4; - TimeDiscretization time_discretization = Uniform; - int N = 16; - int degree = 5; - - GaitParameters gp; - - try { - config = YAML::LoadFile("../Examples/Digit-modified/singlestep_optimization_settings.yaml"); - - N = config["N"].as(); - degree = config["degree"].as(); - std::string time_discretization_str = config["time_discretization"].as(); - time_discretization = (time_discretization_str == "Uniform") ? Uniform : Chebyshev; - - gp.swingfoot_midstep_z_des = config["swingfoot_midstep_z_des"].as(); - gp.swingfoot_begin_y_des = config["swingfoot_begin_y_des"].as(); - gp.swingfoot_end_y_des = config["swingfoot_end_y_des"].as(); - } - catch (std::exception& e) { - std::cerr << "Error parsing YAML file: " << e.what() << std::endl; - } - - // Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "initial-digit-modified-Bezier.txt"); - - // // add disturbance to initial guess - // Eigen::VectorXd disturbance(z.size()); - // if (argc > 1) { - // char* end = nullptr; - // std::srand((unsigned int)std::strtoul(argv[1], &end, 10)); - // disturbance.setRandom(); - // disturbance = disturbance * (0.2 * z.norm()) / disturbance.norm(); - // } - // else { - // disturbance.setZero(); - // } - - // z = z + disturbance; - - if (argc > 1) { - char* end = nullptr; - std::srand((unsigned int)std::strtoul(argv[1], &end, 10)); - } - else { - std::srand(std::time(nullptr)); - } - Eigen::VectorXd z = 0.2 * Eigen::VectorXd::Random((degree + 1) * NUM_INDEPENDENT_JOINTS + NUM_JOINTS + NUM_DEPENDENT_JOINTS).array() - 0.1; - - SmartPtr mynlp = new DigitModifiedSingleStepOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - time_discretization, - degree, - model, - jtype, - gp); - mynlp->constr_viol_tol = 1e-4; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - try { - app->Options()->SetNumericValue("tol", config["tol"].as()); - app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol); - app->Options()->SetNumericValue("max_wall_time", config["max_wall_time"].as()); - app->Options()->SetIntegerValue("max_iter", config["max_iter"].as()); - app->Options()->SetNumericValue("obj_scaling_factor", config["obj_scaling_factor"].as()); - app->Options()->SetIntegerValue("print_level", config["print_level"].as()); - app->Options()->SetStringValue("mu_strategy", config["mu_strategy"].as().c_str()); - app->Options()->SetStringValue("linear_solver", config["linear_solver"].as().c_str()); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - // app->Options()->SetStringValue("nlp_scaling_method", "none"); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error setting optimization options! Check previous error message!"); - } - - if (config["gredient_check"].as()) { - app->Options()->SetStringValue("output_file", "ipopt.out"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("derivative_test", "second-order"); - } - else { - app->Options()->SetStringValue("derivative_test", "first-order"); - } - app->Options()->SetNumericValue("point_perturbation_radius", 1e-2); - // app->Options()->SetIntegerValue("derivative_test_first_index", 168); - app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - app->Options()->SetNumericValue("derivative_test_tol", 1e-4); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - double solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - std::cout << "Total solve time: " << solve_time << " seconds.\n"; - - std::cout << "Data needed for comparison: " << mynlp->obj_value_copy << ' ' << mynlp->final_constr_violation << ' ' << solve_time << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Print the solution - // if (mynlp->solution.size() == mynlp->numVars) { - // std::ofstream solution(filepath + "solution-digit-modified-initial.txt"); - // solution << std::setprecision(20); - // for (int i = 0; i < mynlp->numVars; i++) { - // solution << mynlp->solution[i] << std::endl; - // } - // solution.close(); - - // std::ofstream trajectory(filepath + "trajectory-digit-modified-Bezier-" + output_name + ".txt"); - // trajectory << std::setprecision(20); - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->q(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->v(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->a(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->tau(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->lambda(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // trajectory.close(); - // } - - return 0; -} diff --git a/Examples/Digit-modified/DigitModifiedSingleStepRobustnessTest.cpp b/Examples/Digit-modified/DigitModifiedSingleStepRobustnessTest.cpp deleted file mode 100644 index debe554c..00000000 --- a/Examples/Digit-modified/DigitModifiedSingleStepRobustnessTest.cpp +++ /dev/null @@ -1,133 +0,0 @@ -#include "DigitModifiedSingleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include - -using namespace RAPTOR; -using namespace DigitModified; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit-modified/data/robustness_test/"; - -const int degree_range[] = {5, 6, 7, 8}; -const int N_range[] = {16, 20, 24, 28}; -const std::string mu_strategy_str[] = {"adaptive", "monotone"}; -const std::string linear_solver_str[] = {"ma27", "ma57", "ma86"}; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // define robot model - const std::string urdf_filename = "../Robots/digit-v3-modified/digit-v3-modified.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 3, 3, 3, - 3, 3, -3, 3, 3, 3, 3; - - // ignore all motor dynamics - model.rotorInertia.setZero(); - model.damping.setZero(); - model.friction.setZero(); - - // load settings - const double T = 0.4; - const TimeDiscretization time_discretization = Chebyshev; - int N = 16; - int degree = 5; - - GaitParameters gp; - - SmartPtr app = IpoptApplicationFactory(); - app->Options()->SetNumericValue("tol", 1e-5); - app->Options()->SetNumericValue("constr_viol_tol", 1e-4); - app->Options()->SetNumericValue("max_wall_time", 200.0); - app->Options()->SetIntegerValue("max_iter", 100); - app->Options()->SetNumericValue("obj_scaling_factor", 1e-3); - app->Options()->SetIntegerValue("print_level", 0); - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - for (int degree_choice = 0; degree_choice < 4; degree_choice++) { - degree = degree_range[degree_choice]; - N = N_range[degree_choice]; - for (int mu_strategy_choice = 0; mu_strategy_choice < 2; mu_strategy_choice++) { - app->Options()->SetStringValue("mu_strategy", mu_strategy_str[mu_strategy_choice].c_str()); - for (int linear_solver_choice = 0; linear_solver_choice < 3; linear_solver_choice++) { - app->Options()->SetStringValue("linear_solver", linear_solver_str[linear_solver_choice].c_str()); - - std::cerr << "EXPERIMENT: Degree: " << degree << ", mu_strategy: " << mu_strategy_str[mu_strategy_choice] << ", linear_solver: " << linear_solver_str[linear_solver_choice] << std::endl; - - std::ofstream experiment_output(filepath + - "robustness_test_" + - std::to_string(degree) + - "_" + - mu_strategy_str[mu_strategy_choice] + - "_" + - linear_solver_str[linear_solver_choice] + - ".txt"); - - for (int test_id = 1; test_id <= 100; test_id++) { - std::srand(test_id); - Eigen::VectorXd z = 0.2 * Eigen::VectorXd::Random((degree + 1) * NUM_INDEPENDENT_JOINTS + NUM_JOINTS + NUM_DEPENDENT_JOINTS).array() - 0.1; - - SmartPtr mynlp = new DigitModifiedSingleStepOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - time_discretization, - degree, - model, - jtype, - gp); - mynlp->constr_viol_tol = 1e-4; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - double solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - - experiment_output << mynlp->obj_value_copy << ' ' << mynlp->final_constr_violation << ' ' << solve_time << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - } - - experiment_output.close(); - } - } - } - - return 0; -} diff --git a/Examples/Digit-modified/DigitModifiedSingleStepSpeedTest.cpp b/Examples/Digit-modified/DigitModifiedSingleStepSpeedTest.cpp deleted file mode 100644 index 3611ca34..00000000 --- a/Examples/Digit-modified/DigitModifiedSingleStepSpeedTest.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include "DigitModifiedSingleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include - -using namespace RAPTOR; -using namespace DigitModified; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit-modified/data/"; - -const int degree_range[] = {5, 6, 7, 8}; -const int N_range[] = {16, 20, 24, 28}; -const std::string mu_strategy_str[] = {"adaptive", "monotone"}; -const TimeDiscretization time_discret[] = {Uniform, Chebyshev}; -const std::string time_discretization_str[] = {"Uniform", "Chebyshev"}; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - const std::string urdf_filename = "../Robots/digit-v3-modified/digit-v3-modified.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 3, 3, 3, - 3, 3, -3, 3, 3, 3, 3; - - // ignore all motor dynamics - model.rotorInertia.setZero(); - model.damping.setZero(); - model.friction.setZero(); - - // load settings - const double T = 0.4; - int N = 16; - int degree = 5; - - GaitParameters gp; - gp.swingfoot_midstep_z_des = 0.30; - gp.swingfoot_begin_y_des = 0.40; - gp.swingfoot_end_y_des = -0.40; - - SmartPtr app = IpoptApplicationFactory(); - app->Options()->SetNumericValue("tol", 1e-5); - app->Options()->SetNumericValue("constr_viol_tol", 1e-5); - app->Options()->SetNumericValue("max_wall_time", 200.0); - app->Options()->SetIntegerValue("max_iter", 100); - app->Options()->SetNumericValue("obj_scaling_factor", 1e-3); - app->Options()->SetIntegerValue("print_level", 0); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - std::ofstream experiment_output(filepath + "speed_test.txt"); - - for (int degree_choice = 0; degree_choice < 4; degree_choice++) { - for (int mu_strategy_choice = 0; mu_strategy_choice < 2; mu_strategy_choice++) { - for (int discretization_choice = 0; discretization_choice < 2; discretization_choice++) { - degree = degree_range[degree_choice]; - N = N_range[degree_choice]; - app->Options()->SetStringValue("mu_strategy", mu_strategy_str[mu_strategy_choice].c_str()); - TimeDiscretization time_discretization = time_discret[discretization_choice]; - - std::cerr << "EXPERIMENT: Degree: " << degree << ", mu_strategy: " << mu_strategy_str[mu_strategy_choice] << ", time discretization: " << time_discretization_str[discretization_choice] << std::endl; - // experiment_output << "EXPERIMENT: Degree: " << degree << ", mu_strategy: " << mu_strategy_str[mu_strategy_choice] << ", time discretization: " << time_discretization_str[discretization_choice] << std::endl; - - Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "robustness_test_solution_" + std::to_string(degree) + ".txt"); - - SmartPtr mynlp = new DigitModifiedSingleStepOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - time_discretization, - degree, - model, - jtype, - gp); - mynlp->constr_viol_tol = 1e-5; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Solve the optimization problem - double solve_time = 0.0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - - experiment_output << " & & & " << mynlp->obj_value_copy << " & " << mynlp->final_constr_violation << " & "; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Evaluate the solution on a finer time discretization - try { - SmartPtr testnlp = new DigitModifiedSingleStepOptimizer(); - testnlp->set_parameters(z, - T, - 2000, - TimeDiscretization::Uniform, - degree, - model, - jtype, - gp); - Index n, m, nnz_jac_g, nnz_h_lag; - TNLP::IndexStyleEnum index_style; - testnlp->get_nlp_info(n, m, nnz_jac_g, nnz_h_lag, index_style); - Number ztry[testnlp->numVars], x_l[testnlp->numVars], x_u[testnlp->numVars]; - Number g[testnlp->numCons], g_lb[testnlp->numCons], g_ub[testnlp->numCons]; - for (int i = 0; i < testnlp->numVars; i++) { - ztry[i] = mynlp->solution[i]; - } - testnlp->get_bounds_info(testnlp->numVars, x_l, x_u, testnlp->numCons, g_lb, g_ub); - testnlp->eval_g(testnlp->numVars, ztry, false, testnlp->numCons, g); - testnlp->summarize_constraints(testnlp->numCons, g, true); - experiment_output << testnlp->final_constr_violation << " & " << solve_time << " \\\\" << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error evaluating the solution on a finer time discretization! Check previous error message!"); - } - } - } - } - - experiment_output.close(); - - return 0; -} diff --git a/Examples/Digit-modified/include/DigitModifiedConstants.h b/Examples/Digit-modified/include/DigitModifiedConstants.h deleted file mode 100644 index 92d535aa..00000000 --- a/Examples/Digit-modified/include/DigitModifiedConstants.h +++ /dev/null @@ -1,98 +0,0 @@ -namespace RAPTOR { -namespace DigitModified { - -// constants related to digit-v3-modified -constexpr int NUM_JOINTS = 20; -constexpr int NUM_DEPENDENT_JOINTS = 6; -constexpr int NUM_INDEPENDENT_JOINTS = 14; - -// pulled out from digit-v3.xml -// This is in degree!!! -constexpr double JOINT_LIMITS_LOWER[NUM_JOINTS] = { - -1000, // Px - -1000, // Py - -1000, // Pz - -1000, // Rx - -1000, // Ry - -1000, // Rz - -60, // left-hip-roll - -40, // left-hip-yaw - -60, // left-hip-pitch - -80, // left-knee - -50.3, // left-tarsus - -44, // left-toe-pitch - -37, // left-toe-roll - -60, // right-hip-roll - -40, // right-hip-yaw - -90, // right-hip-pitch - -58.4, // right-knee - -71.6, // right-tarsus - -34, // right-toe-pitch - -33 // right-toe-roll -}; - -// pulled out from digit-v3.xml -// This is in degree!!! -constexpr double JOINT_LIMITS_UPPER[NUM_JOINTS] = { - 1000, // Px - 1000, // Py - 1000, // Pz - 1000, // Rx - 1000, // Ry - 1000, // Rz - 60, // left-hip-roll - 40, // left-hip-yaw - 90, // left-hip-pitch - 58.4, // left-knee - 71.6, // left-tarsus - 34, // left-toe-pitch - 33, // left-toe-roll - 60, // right-hip-roll - 40, // right-hip-yaw - 60, // right-hip-pitch - 80, // right-knee - 50.3, // right-tarsus - 44, // right-toe-pitch - 37 // right-toe-roll -}; - -// // pulled out from digit-v3.xml -// constexpr double TORQUE_LIMITS_LOWER[NUM_INDEPENDENT_JOINTS] = { -// -126.682, // left-hip-roll -// -79.1765, // left-hip-yaw -// -216.928, // left-hip-pitch -// -231.317, // left-knee -// -41.9759, // left-toe-A -// -41.9759, // left-toe-B -// -126.682, // right-hip-roll -// -79.1765, // right-hip-yaw -// -216.928, // right-hip-pitch -// -231.317, // right-knee -// -41.9759, // right-toe-A -// -41.9759, // right-toe-B -// }; - -// // pulled out from digit-v3.xml -// constexpr double TORQUE_LIMITS_UPPER[NUM_INDEPENDENT_JOINTS] = { -// 126.682, // left-hip-roll -// 79.1765, // left-hip-yaw -// 216.928, // left-hip-pitch -// 231.317, // left-knee -// 41.9759, // left-toe-A -// 41.9759, // left-toe-B -// 126.682, // right-hip-roll -// 79.1765, // right-hip-yaw -// 216.928, // right-hip-pitch -// 231.317, // right-knee -// 41.9759, // right-toe-A -// 41.9759, // right-toe-B -// }; - -// pulled out from digit-v3.xml -constexpr double MU = 0.7; -constexpr double GAMMA = 0.7; -constexpr double FOOT_WIDTH = 0.04; // (m) -constexpr double FOOT_LENGTH = 0.1175; // (m) - -}; // namespace DigitModified -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit-modified/include/DigitModifiedConstrainedInverseDynamics.h b/Examples/Digit-modified/include/DigitModifiedConstrainedInverseDynamics.h deleted file mode 100644 index 87d4e2d0..00000000 --- a/Examples/Digit-modified/include/DigitModifiedConstrainedInverseDynamics.h +++ /dev/null @@ -1,40 +0,0 @@ - -#ifndef DIGIT_MODIFIED_CONSTRAINED_INVERSE_DYNAMICS_H -#define DIGIT_MODIFIED_CONSTRAINED_INVERSE_DYNAMICS_H - -#include "ConstrainedInverseDynamics.h" -#include "DigitModifiedDynamicsConstraints.h" - -namespace RAPTOR { -namespace DigitModified { - -class DigitModifiedConstrainedInverseDynamics : public ConstrainedInverseDynamics { -public: - using Model = pinocchio::Model; - - // Constructor - DigitModifiedConstrainedInverseDynamics() = default; - - // Constructor - DigitModifiedConstrainedInverseDynamics(const Model& model_input, - std::shared_ptr& trajPtr_input, - int numDependentJoints_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg, - const Transform& stance_foot_T_des_input); - - // Destructor - ~DigitModifiedConstrainedInverseDynamics() = default; - - // class members: - // a pointer type of DigitModifiedDynamicsConstraints, - // that shares the same memory with dcPtr_ defined in base class ConstrainedInverseDynamics - // so that other Digit-related class can access specific field in DigitModifiedDynamicsConstraints - // such as stanceLeg, stance_foot_T_des, etc. - std::shared_ptr ddcPtr_; -}; - -}; // namespace DigitModified -}; // namespace RAPTOR - -#endif // DIGIT_MODIFIED_CONSTRAINED_INVERSE_DYNAMICS_H diff --git a/Examples/Digit-modified/include/DigitModifiedCustomizedConstraints.h b/Examples/Digit-modified/include/DigitModifiedCustomizedConstraints.h deleted file mode 100644 index fd955afe..00000000 --- a/Examples/Digit-modified/include/DigitModifiedCustomizedConstraints.h +++ /dev/null @@ -1,88 +0,0 @@ - -#ifndef DIGIT_MODIFIED_CUSTOMIZED_CONSTRAINTS_H -#define DIGIT_MODIFIED_CUSTOMIZED_CONSTRAINTS_H - -#include "Constraints.h" -#include "LieSpaceResidual.h" -#include "Trajectories.h" -#include "DigitModifiedConstrainedInverseDynamics.h" -#include "DigitModifiedDynamicsConstraints.h" -#include "Utils.h" - -namespace RAPTOR { -namespace DigitModified { - -typedef struct GaitParameters_ { - double eps_torso_angle = Utils::deg2rad(3); // 3 degrees - double swingfoot_midstep_z_des = 0.15; // meters - double swingfoot_begin_x_des = -0.22; // meters - double swingfoot_begin_y_des = 0.00; // meters - double swingfoot_end_x_des = -0.22; // meters - double swingfoot_end_y_des = 0.00; // meters -} GaitParameters; - -class DigitModifiedCustomizedConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitModifiedCustomizedConstraints() = default; - - // Constructor - DigitModifiedCustomizedConstraints(const Model& model_input, - const Eigen::VectorXi& jtype_input, - std::shared_ptr& trajPtr_input, - std::shared_ptr& dcPtr_input, - const GaitParameters& gp_input); - - // Destructor - ~DigitModifiedCustomizedConstraints() = default; - - // class methods: - // compute constraints - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - // class variables: - GaitParameters gp; - - std::shared_ptr trajPtr_; - std::shared_ptr dcPtr_; - - std::unique_ptr modelPtr_; - - std::unique_ptr fkPtr_; - - // jtype copy - Eigen::VectorXi jtype; - - // the joint index of the joint we want to constrain - Model::JointIndex swingfoot_id = 0; - - // the transform matrix at the beginning and at the end - Transform swingfoot_endT; - Transform swingfoot_T_des; - - // updated in compute() - // full joint trajectories and derivatives - // including both independent and dependent joints - MatX q; - Eigen::Array pq_pz; - - MatX swingfoot_xyzrpy; - Eigen::Array pswingfoot_xyzrpy_pz; - - // forward kinematics derivatives - std::vector dTdq; -}; - -} // namespace DigitModified -} // namespace RAPTOR - -#endif // DIGIT_MODIFIED_CUSTOMIZED_CONSTRAINTS_H diff --git a/Examples/Digit-modified/include/DigitModifiedDynamicsConstraints.h b/Examples/Digit-modified/include/DigitModifiedDynamicsConstraints.h deleted file mode 100644 index d0af657a..00000000 --- a/Examples/Digit-modified/include/DigitModifiedDynamicsConstraints.h +++ /dev/null @@ -1,168 +0,0 @@ - -#ifndef DIGIT_MODIFIED_CONSTRAINTS_H -#define DIGIT_MODIFIED_CONSTRAINTS_H - -#include -#include - -#include "DigitModifiedConstants.h" -#include "DynamicsConstraints.h" -#include "ForwardKinematics.h" - -namespace RAPTOR { -namespace DigitModified { - -const std::string dependentJointNames[NUM_DEPENDENT_JOINTS] = - {"Px", - "Py", - "Pz", - "Rx", - "Ry", - "Rz"}; - -const std::string independentJointNames[NUM_INDEPENDENT_JOINTS] = - {"left_hip_roll", - "left_hip_yaw", - "left_hip_pitch", - "left_knee", - "left_tarsus", - "left_toe_pitch", - "left_toe_roll", - "right_hip_roll", - "right_hip_yaw", - "right_hip_pitch", - "right_knee", - "right_tarsus", - "right_toe_pitch", - "right_toe_roll"}; - -class DigitModifiedDynamicsConstraints : public DynamicsConstraints { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitModifiedDynamicsConstraints() = default; - - // Constructor - DigitModifiedDynamicsConstraints(const std::shared_ptr& modelPtr_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg, - const Transform& stance_foot_T_des_input); - - // Destructor - ~DigitModifiedDynamicsConstraints() = default; - - // class methods: - // swap the stance leg for reset map constraint evaluation - void reinitialize(); - - // return the index of id th dependent joint - int return_dependent_joint_index(const int id) final override; - - // return the index of id th independent joint - int return_independent_joint_index(const int id) final override; - - // fill in dependent indeces in a vector - void fill_dependent_vector(VecX& r, const VecX& v, const bool setZero = false) final override; - - // fill in independent indeces in a vector - void fill_independent_vector(VecX& r, const VecX& v, const bool setZero = false) final override; - - // fill in dependent columns in a matrix - void fill_dependent_columns(MatX& r, const MatX& m, const bool setZero = false) final override; - - // fill in independent columns in a matrix - void fill_independent_columns(MatX& r, const MatX& m, const bool setZero = false) final override; - - // fill in dependent rows in a matrix - void fill_dependent_rows(MatX& r, const MatX& m, const bool setZero = false) final override; - - // fill in independent rows in a matrix - void fill_independent_rows(MatX& r, const MatX& m, const bool setZero = false) final override; - - // return dependent indeces in a vector - VecX get_dependent_vector(const VecX& v) final override; - - // return independent indeces in a vector - VecX get_independent_vector(const VecX& v) final override; - - // return dependent columns in a matrix - void get_dependent_columns(MatX& r, const MatX& m) final override; - - // return independent columns in a matrix - void get_independent_columns(MatX& r, const MatX& m) final override; - - // return dependent rows in a matrix - void get_dependent_rows(MatX& r, const MatX& m) final override; - - // return independent rows in a matrix - void get_independent_rows(MatX& r, const MatX& m) final override; - - // fill in dependent joint positions in the full joint vector q - // that satisfies the constraints - // This usually involves solving inverse kinematics. - // You need to implement this method in your derived class!!! - void setupJointPosition(VecX& q, bool compute_derivatives = true) final override; - - // constraint c(q) - void get_c(const VecX& q) final override; - - // J = jacobian(c, q) - void get_J(const VecX& q) final override; - - // Jx_partial_dq = jacobian(J * x, q) - // where x is a vector that is not dependent on q - void get_Jx_partial_dq(const VecX& q, const VecX& x) final override; - - // JTx_partial_dq = jacobian(J^T * x, q) - // where x is a vector that is not dependent on q - void get_JTx_partial_dq(const VecX& q, const VecX& x) final override; - - // Jxy_partial_dq = jacobian(jacobian(J * x, q) * y, q) - // where x and y are vectors that are not dependent on q - void get_Jxy_partial_dq(const VecX& q, const VecX& x, const VecX& y) final override; - - // class members: - std::shared_ptr modelPtr_ = nullptr; - - std::unique_ptr fkPtr_ = nullptr; - - // dep/indep joint indeces - int dependentJointIds[NUM_DEPENDENT_JOINTS] = {0}; - int independentJointIds[NUM_INDEPENDENT_JOINTS] = {0}; - - // jtype copy - Eigen::VectorXi jtype; - - // for contact constraints (forward kinematics mainly) - Model::JointIndex contact_joint_id = 0; - - // contact foot transforms - char stanceLeg = 'L'; - - Transform stance_foot_endT; - Transform stance_foot_T_des; - - // intermediate results - Vec3 c_translation; - Vec3 c_rotation; - - MatX J_translation; - MatX J_rotation; - - Eigen::Array H_translation; - Eigen::Array H_rotation; - - Eigen::Array TOx_translation; - Eigen::Array TOx_rotation; -}; - -}; // namespace DigitModified -}; // namespace RAPTOR - -#endif // DIGIT_MODIFIED_CONSTRAINTS_H diff --git a/Examples/Digit-modified/include/DigitModifiedSingleStepOptimizer.h b/Examples/Digit-modified/include/DigitModifiedSingleStepOptimizer.h deleted file mode 100644 index 0f51e47b..00000000 --- a/Examples/Digit-modified/include/DigitModifiedSingleStepOptimizer.h +++ /dev/null @@ -1,104 +0,0 @@ -#ifndef DIGIT_MODIFIEDSINGLESTEPOPTIMIZER_H -#define DIGIT_MODIFIEDSINGLESTEPOPTIMIZER_H - -#include "Optimizer.h" - -#include "BezierCurves.h" - -#include "DigitModifiedConstrainedInverseDynamics.h" -#include "DigitModifiedDynamicsConstraints.h" -#include "Utils.h" - -#include "ConstrainedJointLimits.h" -#include "TorqueLimits.h" -#include "SurfaceContactConstraints.h" -#include "DigitModifiedCustomizedConstraints.h" -#include "DigitModifiedSingleStepPeriodicityConstraints.h" - -namespace RAPTOR { -namespace DigitModified { - -using namespace Ipopt; - -class DigitModifiedSingleStepOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - DigitModifiedSingleStepOptimizer() = default; - - /** Default destructor */ - ~DigitModifiedSingleStepOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const TimeDiscretization time_discretization_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const GaitParameters& gp_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - DigitModifiedSingleStepOptimizer( - const DigitModifiedSingleStepOptimizer& - ); - - DigitModifiedSingleStepOptimizer& operator=( - const DigitModifiedSingleStepOptimizer& - ); - - std::shared_ptr trajPtr_; - - std::shared_ptr dcPtr_; - std::shared_ptr idPtr_; - std::shared_ptr cidPtr_; - std::shared_ptr dcidPtr_; -}; - -}; // namespace DigitModified -}; // namespace RAPTOR - -#endif // DIGIT_MODIFIEDSINGLESTEPOPTIMIZER_H diff --git a/Examples/Digit-modified/include/DigitModifiedSingleStepPeriodicityConstraints.h b/Examples/Digit-modified/include/DigitModifiedSingleStepPeriodicityConstraints.h deleted file mode 100644 index b5a64226..00000000 --- a/Examples/Digit-modified/include/DigitModifiedSingleStepPeriodicityConstraints.h +++ /dev/null @@ -1,95 +0,0 @@ - -#ifndef DIGIT_MODIFIED_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H -#define DIGIT_MODIFIED_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H - -#include "pinocchio/algorithm/crba.hpp" -#include "pinocchio/algorithm/aba-derivatives.hpp" - -#include "Constraints.h" -#include "DigitModifiedConstrainedInverseDynamics.h" -#include "SurfaceContactConstraints.h" - -#include -#include - -namespace RAPTOR { -namespace DigitModified { - -// ... ==> ... -const std::string JOINT_MAP[NUM_INDEPENDENT_JOINTS][2] = { - {"left_hip_roll", "right_hip_roll"}, - {"left_hip_yaw", "right_hip_yaw"}, - {"left_hip_pitch", "right_hip_pitch"}, - {"left_knee", "right_knee"}, - {"left_tarsus", "right_tarsus"}, - {"left_toe_pitch", "right_toe_pitch"}, - {"left_toe_roll", "right_toe_roll"}, - {"right_hip_roll", "left_hip_roll"}, - {"right_hip_yaw", "left_hip_yaw"}, - {"right_hip_pitch", "left_hip_pitch"}, - {"right_knee", "left_knee"}, - {"right_tarsus", "left_tarsus"}, - {"right_toe_pitch", "left_toe_pitch"}, - {"right_toe_roll", "left_toe_roll"}, -}; - -class DigitModifiedSingleStepPeriodicityConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitModifiedSingleStepPeriodicityConstraints() = default; - - // Constructor - DigitModifiedSingleStepPeriodicityConstraints(std::shared_ptr& trajPtr_input, - std::shared_ptr dcidPtr_input, - const frictionParams& fp_input); - - // Destructor - ~DigitModifiedSingleStepPeriodicityConstraints() = default; - - // class methods - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - void compute_bounds() override; - - // class members - std::shared_ptr& trajPtr_; - std::shared_ptr dcidPtr_; - - frictionParams fp; - - // intermediate variables updated in compute() - int joint_id1[NUM_INDEPENDENT_JOINTS]; - int joint_id2[NUM_INDEPENDENT_JOINTS]; - - MatX prnea_pq; - MatX prnea_pv; - MatX prnea_pa; - VecX zeroVec; - - MatX pv_plus_pz; - MatX plambda_pz; - - VecX g1; - VecX g2; - VecX g3; - VecX g4; - VecX g5; - - MatX pg1_pz; - MatX pg2_pz; - MatX pg3_pz; - MatX pg4_pz; - MatX pg5_pz; -}; - -}; // namespace DigitModified -}; // namespace RAPTOR - -#endif // DIGIT_MODIFIED_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H diff --git a/Examples/Digit-modified/matlab_scripts/generate_summary_figure.m b/Examples/Digit-modified/matlab_scripts/generate_summary_figure.m deleted file mode 100644 index bb6615a1..00000000 --- a/Examples/Digit-modified/matlab_scripts/generate_summary_figure.m +++ /dev/null @@ -1,38 +0,0 @@ -close all; clear; clc; - -AA = load('comp-Bezier-N8-50-Chebyshev.mat'); -BB = load('comp-Bezier-N16-50-Chebyshev.mat'); -CC = load('comp-Bezier-N8-50-Uniform.mat'); -DD = load('comp-Bezier-N16-50-Uniform.mat'); -EE = load('comp-TROPIC.mat'); -FF = load('comp-crocoddyl-fwddyn.mat'); -GG = load('comp-crocoddyl-invdyn.mat'); - -% figure; hold on; -% plot(log(sort(AA.data1)); -% plot(log(sort(BB.data1)); -% plot(log(sort(CC.data1)); -% legend('RAPTOR,Bezier,Chebshev,N=8', ... -% 'RAPTOR,Bezier,Chebshev,N=16', ... -% 'TROPIC'); -% xlabel('index of experiment'); -% ylabel('number of iterations to terminate'); - -figure; hold on; -plot(log(sort(AA.data4(2:end))) / log(10)); -plot(log(sort(BB.data4(2:end))) / log(10)); -plot(log(sort(CC.data4(2:end))) / log(10)); -plot(log(sort(DD.data4(2:end))) / log(10)); -plot(log(sort(EE.data4(2:end))) / log(10)); -plot(log(sort(FF.data3)) / log(10)); -plot(log(sort(GG.data3)) / log(10)); -ylim([-12 2]); -legend('RAPTOR,Bezier,Chebshev,N=8', ... - 'RAPTOR,Bezier,Chebshev,N=16', ... - 'RAPTOR,Bezier,Uniform,N=8', ... - 'RAPTOR,Bezier,Uniform,N=16', ... - 'TROPIC', ... - 'crocoddyl forward dynamics', ... - 'crocoddyl inverse dynamics'); -xlabel('index of experiment'); -ylabel('log_{10} constraint violation ratio'); \ No newline at end of file diff --git a/Examples/Digit-modified/matlab_scripts/process_ipopt_outfile.m b/Examples/Digit-modified/matlab_scripts/process_ipopt_outfile.m deleted file mode 100644 index 3c540f99..00000000 --- a/Examples/Digit-modified/matlab_scripts/process_ipopt_outfile.m +++ /dev/null @@ -1,105 +0,0 @@ -close all; clear; - -% Set the print level to 5 in ipopt -% This script reads iterations, constraint violation, objective value -% from ipopt.out and visualizes them. - -num_instances = 8; -disturbance = 20; -discretization = 'Uniform'; - -num_trails = 100; - -data1 = zeros(num_trails,1); -data2 = zeros(num_trails,2001); -data3 = zeros(num_trails,2001); -data4 = zeros(num_trails,1); -data5 = zeros(num_trails,1); -running_time = zeros(num_trails, 1); - -%% -for i = 1:num_trails - ipopt_filename = sprintf('ipopt_digit-modified-Bezier-%d-%d-%d-%s.out', i, num_instances, disturbance, discretization); -% ipopt_filename = sprintf('/home/roahmlab/Documents/TROPIC/examples/digit-modified/data/ipopt-%d-d20.out', i); - - fid = fopen(ipopt_filename, 'r'); - - iterations = []; - constrViolation = []; - objValue = []; - energy = 0; - - while ~feof(fid) - line = fgetl(fid); - line_process = strsplit(line, ' '); - - if isempty(line_process{1}) - line_process = line_process(2:end); - end - - if length(line_process) == 10 - if line_process{1}(end) == 'r' - iterations_text = line_process{1}(1:end-1); - else - iterations_text = line_process{1}; - end - - iteration_number = str2double(iterations_text); - - if isnan(iteration_number) - continue; - end - - iterations = [iterations, iteration_number]; - objValue = [objValue, str2double(line_process{2})]; - constrViolation = [constrViolation, str2double(line_process{3})]; - elseif length(line_process) == 6 && ~isempty(str2double(line_process{end})) && strcmp(line_process{2}, 'seconds') - running_time(i) = str2double(line_process{end}); - elseif length(line_process) == 3 && strcmp(line_process{1}, 'Objective') - energy = str2double(line_process{end}); - end - end - - fclose(fid); - - len = length(iterations); -% data1(i,1:len) = iterations; - data1(i) = len; - data2(i,1:len) = objValue; - data3(i,1:len) = constrViolation; - data4(i) = constrViolation(end); - data5(i) = energy; -end - -%% -% figure; hold on; -% for i = 1:num_trails -% plot(1:2001, log(data2(i,:)) / log(10), 'b'); -% end -% xlim([1,2000]); -% xlabel('iterations'); -% ylabel('log_{10} cost function'); -% title('cost function vs. iterations (TROPIC)'); -% -% figure; hold on; -% for i = 1:num_trails -% plot(1:2001, log(data3(i,:)) / log(10), 'b'); -% end -% xlim([1,2000]); -% xlabel('iterations'); -% ylabel('log_{10} constraint violation'); -% title('constraint violation vs. iterations (TROPIC)'); -% -% figure; -% plot(log(sort(data3(:,end))) / log(10)) -% xlabel('index of the experiment'); -% ylabel('log_{10} of the constraint violation'); -% title('final constraint violation for 100 experiments (TROPIC)'); - -%% -save(sprintf('comp-Bezier-N%d-%d-%s.mat', num_instances, disturbance, discretization), 'data1', "data2", "data3", "data4"); - -disp(vpa([median(data4), max(data4)], 6)); -disp([median(data1), max(data1)]) -disp(vpa([median(data5), max(data5)], 6)); -disp(vpa([median(running_time), max(running_time)], 6)); diff --git a/Examples/Digit-modified/matlab_scripts/visualize_trajectories.m b/Examples/Digit-modified/matlab_scripts/visualize_trajectories.m deleted file mode 100644 index 7d09b9c5..00000000 --- a/Examples/Digit-modified/matlab_scripts/visualize_trajectories.m +++ /dev/null @@ -1,18 +0,0 @@ -close all; clear; clc; - -robot = importrobot('digit-v3-modified.urdf'); -robot.DataFormat = 'col'; - -traj = readmatrix('trajectory-digit-modified-Bezier.txt'); - -q = traj(1:20, :); -qd = traj(21:40, :); -qdd = traj(41:60, :); -u = traj(61:72, :); -lambda = traj(73:end, :); - -for i = 1:size(q,2) - show(robot, q(:,i), 'frames', 'off', 'Collisions','off'); - hold on; -end -axis([-1 1 -1 1 0 2]); \ No newline at end of file diff --git a/Examples/Digit-modified/singlestep_optimization_settings.yaml b/Examples/Digit-modified/singlestep_optimization_settings.yaml deleted file mode 100644 index 38c4b42f..00000000 --- a/Examples/Digit-modified/singlestep_optimization_settings.yaml +++ /dev/null @@ -1,62 +0,0 @@ -name: DigitModifiedSingleStep - -### Trajectory settings - # number of time nodes to evaluate constraints -N: 16 - - # distribution of the time nodes -time_discretization: Chebyshev # Chebyshev, Uniform - - # degree of the Bezier curve (as the desired trajectory) -degree: 5 - -### Gait behavior settings - # step height (in the middle of the gait) -swingfoot_midstep_z_des: 0.15 - - # position with respect to the forward direction of the robot at the beginning -swingfoot_begin_y_des: 0.0 - - # position with respect to the forward direction of the robot at the end -swingfoot_end_y_des: -0.0 - - # examples: - # 1. swingfoot_begin_y_des/swingfoot_end_y_des being positive - # means the swing foot is in front of the robot. - # 2. swingfoot_begin_y_des/swingfoot_end_y_des being negative - # means the swing foot is behind the robot. - # 3. swingfoot_begin_y_des being negative, and swingfoot_end_y_des being positive - # means the robot is walking forward. - # 4. swingfoot_begin_y_des being positive, and swingfoot_end_y_des being negative - # means the robot is walking backward. - # 5. For periodic gaits, the absolute values of swingfoot_begin_y_des and swingfoot_end_y_des should be the same, - # since the gait should be symmetric. - # Otherwise, you need to turn off or change the periodicity reset map constraint. - -### Ipopt Optimization settings - # terminal condition 1: optimality tolerance -tol: 1e-5 - - # terminal condition 2: constraint violation tolerance -constr_viol_tol: 1e-4 - - # terminal condition 3: maximum wall time -max_wall_time: 1000.0 - - # terminal condition 4: maximum number of iterations -max_iter: 200 - - # scale the objective function -obj_scaling_factor: 1e-3 - - # print level -print_level: 5 # 0: no output, 5: detailed output that is usually enough for tuning parameters - - # mu_strategy -mu_strategy: adaptive # adaptive for more aggressive attempts, monotone for more conservative convergence - - # linear solver (HSL) -linear_solver: ma57 # ma27, ma57, ma86, ma97, pardiso - - # turn on gradient check or not -gredient_check: false \ No newline at end of file diff --git a/Examples/Digit-modified/src/DigitModifiedConstrainedInverseDynamics.cpp b/Examples/Digit-modified/src/DigitModifiedConstrainedInverseDynamics.cpp deleted file mode 100644 index 9e10b64f..00000000 --- a/Examples/Digit-modified/src/DigitModifiedConstrainedInverseDynamics.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "DigitModifiedConstrainedInverseDynamics.h" - -namespace RAPTOR { -namespace DigitModified { - -DigitModifiedConstrainedInverseDynamics::DigitModifiedConstrainedInverseDynamics(const Model& model_input, - std::shared_ptr& trajPtr_input, - int NUM_DEPENDENT_JOINTS_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg_input, - const Transform& stance_foot_T_des_input) : - ConstrainedInverseDynamics(model_input, trajPtr_input, NUM_DEPENDENT_JOINTS_input) { - ddcPtr_ = std::make_shared(modelPtr_, - jtype_input, - stanceLeg_input, - stance_foot_T_des_input); - dcPtr_ = ddcPtr_; // convert to base class -} - -}; // namespace DigitModified -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit-modified/src/DigitModifiedCustomizedConstraints.cpp b/Examples/Digit-modified/src/DigitModifiedCustomizedConstraints.cpp deleted file mode 100644 index 293d5a50..00000000 --- a/Examples/Digit-modified/src/DigitModifiedCustomizedConstraints.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include "DigitModifiedCustomizedConstraints.h" - -namespace RAPTOR { -namespace DigitModified { - -DigitModifiedCustomizedConstraints::DigitModifiedCustomizedConstraints(const Model& model_input, - const Eigen::VectorXi& jtype_input, - std::shared_ptr& trajPtr_input, - std::shared_ptr& dcPtr_input, - const GaitParameters& gp_input) : - jtype(jtype_input), - trajPtr_(trajPtr_input), - dcPtr_(dcPtr_input), - gp(gp_input) { - modelPtr_ = std::make_unique(model_input); - fkPtr_ = std::make_unique(modelPtr_.get(), jtype); - - if (modelPtr_->existJointName("right_toe_roll")) { - swingfoot_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: right_toe_roll"); - } - - // This is right foot end transform - // This only applies when stance foot is left foot!!! - swingfoot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - swingfoot_endT.p << 0, 0, 0; - - // desired swing foot orientation - swingfoot_T_des.R << 0, -1, 0, - 1, 0, 0, - 0, 0, 1; - - q = MatX::Zero(modelPtr_->nv, trajPtr_->N); - pq_pz.resize(1, trajPtr_->N); - - swingfoot_xyzrpy = MatX::Zero(6, trajPtr_->N); - pswingfoot_xyzrpy_pz.resize(1, trajPtr_->N); - - m = trajPtr_->N * 8 + 4; - initialize_memory(m, trajPtr_->varLength, false); -} - -void DigitModifiedCustomizedConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("DigitModifiedCustomizedConstraints does not support hessian computation"); - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - const int fk_order = compute_derivatives ? 1 : 0; - - // compute full joint trajectories and swing foot forward kinematics - for (int i = 0; i < trajPtr_->N; i++) { - VecX qi(modelPtr_->nq); - dcPtr_->fill_independent_vector(qi, trajPtr_->q(i)); - dcPtr_->setupJointPosition(qi, compute_derivatives); - q.col(i) = qi; - - fkPtr_->compute(0, swingfoot_id, qi, nullptr, &swingfoot_endT, fk_order); - - swingfoot_xyzrpy.col(i).head(3) = fkPtr_->getTranslation(); - swingfoot_xyzrpy.col(i).tail(3) = fkPtr_->getRPY(); - - if (compute_derivatives) { - pq_pz(i).resize(modelPtr_->nv, trajPtr_->varLength); - - // fill in independent joints derivatives directly - for (int j = 0; j < dcPtr_->numIndependentJoints; j++) { - int indenpendentJointIndex = dcPtr_->return_independent_joint_index(j); - pq_pz(i).row(indenpendentJointIndex) = trajPtr_->pq_pz(i).row(j); - } - // compute and fill in dependent joints derivatives - MatX pq_dep_pz = dcPtr_->pq_dep_pq_indep * trajPtr_->pq_pz(i); - for (int j = 0; j < dcPtr_->numDependentJoints; j++) { - int denpendentJointIndex = dcPtr_->return_dependent_joint_index(j); - pq_pz(i).row(denpendentJointIndex) = pq_dep_pz.row(j); - } - - pswingfoot_xyzrpy_pz(i).resize(6, trajPtr_->varLength); - pswingfoot_xyzrpy_pz(i).topRows(3) = fkPtr_->getTranslationJacobian() * pq_pz(i); - pswingfoot_xyzrpy_pz(i).bottomRows(3) = fkPtr_->getRPYJacobian() * pq_pz(i); - } - } - - // (1) swingfoot height always larger than 0 - // height equal to 0 at the beginning and at the end - // height higher than the desired value in the middle - VecX g1 = swingfoot_xyzrpy.row(2).transpose(); - - // (2) swingfoot always flat and points forward - VecX g2 = swingfoot_xyzrpy.row(3).transpose(); // swingfoot roll - VecX g3 = swingfoot_xyzrpy.row(4).transpose(); // swingfoot pitch - VecX g4 = swingfoot_xyzrpy.row(5).transpose().array() + M_PI / 2; // swingfoot yaw - - // (3) swingfoot xy equal to desired value at the beginning and at the end - VecX g5(4); - g5 << swingfoot_xyzrpy.topLeftCorner(2, 1), swingfoot_xyzrpy.topRightCorner(2, 1); - - // (4) torso height always larger than 1 meter - // roll and pitch always close to 0 - // yaw always close to 0 when walking forward - VecX g6 = q.row(2).transpose(); // torso height - VecX g7 = q.row(3).transpose(); // torso roll - VecX g8 = q.row(4).transpose(); // torso pitch - VecX g9 = q.row(5).transpose().array() + M_PI / 2; // torso yaw - - g << g1, g2, g3, g4, g5, g6, g7, g8, g9; - - if (compute_derivatives) { - int iter = 0; - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(2); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(3); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(4); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(5); - } - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(0).row(0); - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(0).row(1); - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(trajPtr_->N - 1).row(0); - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(trajPtr_->N - 1).row(1); - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(2); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(3); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(4); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(5); - } - } -} - -void DigitModifiedCustomizedConstraints::compute_bounds() { - // (1) swingfoot height always larger than 0 - // height equal to 0 at the beginning and at the end - // height higher than the desired value in the middle - VecX g1_lb = VecX::Zero(trajPtr_->N); - VecX g1_ub = VecX::Constant(trajPtr_->N, 1e19); - g1_lb(trajPtr_->N / 2) = gp.swingfoot_midstep_z_des; - g1_ub(0) = 0; - g1_ub(trajPtr_->N - 1) = 0; - - // (2) swingfoot always flat and points forward - VecX g2_lb = VecX::Zero(trajPtr_->N); - VecX g2_ub = VecX::Zero(trajPtr_->N); - VecX g3_lb = VecX::Zero(trajPtr_->N); - VecX g3_ub = VecX::Zero(trajPtr_->N); - VecX g4_lb = VecX::Zero(trajPtr_->N); - VecX g4_ub = VecX::Zero(trajPtr_->N); - - // (3) swingfoot xy equal to desired value at the beginning and at the end - VecX g5_lb(4); - VecX g5_ub(4); - g5_lb << gp.swingfoot_begin_x_des, gp.swingfoot_begin_y_des, gp.swingfoot_end_x_des, gp.swingfoot_end_y_des; - g5_ub << gp.swingfoot_begin_x_des, gp.swingfoot_begin_y_des, gp.swingfoot_end_x_des, gp.swingfoot_end_y_des; - - // (4) torso height always larger than 1 meter - // roll and pitch always close to 0 - // yaw always close to 0 when walking forward - VecX g6_lb = VecX::Constant(trajPtr_->N, 1); - VecX g6_ub = VecX::Constant(trajPtr_->N, 1e19); - VecX g7_lb = VecX::Constant(trajPtr_->N, -gp.eps_torso_angle); - VecX g7_ub = VecX::Constant(trajPtr_->N, gp.eps_torso_angle); - VecX g8_lb = VecX::Constant(trajPtr_->N, -gp.eps_torso_angle); - VecX g8_ub = VecX::Constant(trajPtr_->N, gp.eps_torso_angle); - VecX g9_lb = VecX::Constant(trajPtr_->N, -gp.eps_torso_angle); - VecX g9_ub = VecX::Constant(trajPtr_->N, gp.eps_torso_angle); - - g_lb << g1_lb, g2_lb, g3_lb, g4_lb, g5_lb, g6_lb, g7_lb, g8_lb, g9_lb; - g_ub << g1_ub, g2_ub, g3_ub, g4_ub, g5_ub, g6_ub, g7_ub, g8_ub, g9_ub; -} - -}; // namespace DigitModified -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit-modified/src/DigitModifiedDynamicsConstraints.cpp b/Examples/Digit-modified/src/DigitModifiedDynamicsConstraints.cpp deleted file mode 100644 index a56e8ed8..00000000 --- a/Examples/Digit-modified/src/DigitModifiedDynamicsConstraints.cpp +++ /dev/null @@ -1,391 +0,0 @@ -#include "DigitModifiedDynamicsConstraints.h" - -namespace RAPTOR { -namespace DigitModified { - -DigitModifiedDynamicsConstraints::DigitModifiedDynamicsConstraints(const std::shared_ptr& modelPtr_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg_input, - const Transform& stance_foot_T_des_input) : - modelPtr_(modelPtr_input), - jtype(jtype_input), - stanceLeg(stanceLeg_input), - DynamicsConstraints(modelPtr_input->nv, NUM_DEPENDENT_JOINTS) { - fkPtr_ = std::make_unique(modelPtr_.get(), jtype); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - if (modelPtr_->existJointName(dependentJointNames[i])) { - dependentJointIds[i] = modelPtr_->getJointId(dependentJointNames[i]) - 1; - } - else { - throw std::runtime_error("Can not find joint: " + dependentJointNames[i]); - } - } - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (modelPtr_->existJointName(independentJointNames[i])) { - independentJointIds[i] = modelPtr_->getJointId(independentJointNames[i]) - 1; - } - else { - throw std::runtime_error("Can not find joint: " + dependentJointNames[i]); - } - } - - if (stanceLeg == 'L' || stanceLeg == 'l') { - if (modelPtr_->existJointName("left_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("left_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: left_toe_roll"); - } - - stance_foot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0, 0; - } - else { - if (modelPtr_->existJointName("right_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: right_toe_roll"); - } - - stance_foot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0, 0; - } - - stance_foot_T_des = stance_foot_T_des_input; -} - -void DigitModifiedDynamicsConstraints::reinitialize() { - // swap the stance leg - if (stanceLeg == 'L' || stanceLeg == 'l') { - stanceLeg = 'R'; - } - else { - stanceLeg = 'L'; - } - - // reinitialize the stance leg end effector transformation matrix - if (stanceLeg == 'L' || stanceLeg == 'l') { - if (modelPtr_->existJointName("left_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("left_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: left_toe_roll"); - } - - stance_foot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0, 0; - } - else { - if (modelPtr_->existJointName("right_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: right_toe_roll"); - } - - stance_foot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0, 0; - } -} - -int DigitModifiedDynamicsConstraints::return_dependent_joint_index(const int id) { - assert(0 <= id && id < NUM_DEPENDENT_JOINTS); - return dependentJointIds[id]; -} - -int DigitModifiedDynamicsConstraints::return_independent_joint_index(const int id) { - assert(0 <= id && id < NUM_INDEPENDENT_JOINTS); - return independentJointIds[id]; -} - -void DigitModifiedDynamicsConstraints::fill_dependent_vector(VecX& r, const VecX& v, const bool setZero) { - assert(r.size() == modelPtr_->nv); - assert(v.size() == NUM_DEPENDENT_JOINTS); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r(dependentJointIds[i]) = v(i); - } -} - -void DigitModifiedDynamicsConstraints::fill_independent_vector(VecX& r, const VecX& v, const bool setZero) { - assert(r.size() == modelPtr_->nv); - assert(v.size() == NUM_INDEPENDENT_JOINTS); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r(independentJointIds[i]) = v(i); - } -} - -void DigitModifiedDynamicsConstraints::fill_dependent_columns(MatX& r, const MatX& m, const bool setZero) { - assert(m.cols() == NUM_DEPENDENT_JOINTS); - assert(r.cols() == modelPtr_->nv); - assert(m.rows() == r.rows()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.col(dependentJointIds[i]) = m.col(i); - } -} - -void DigitModifiedDynamicsConstraints::fill_independent_columns(MatX& r, const MatX& m, const bool setZero) { - assert(m.cols() == NUM_INDEPENDENT_JOINTS); - assert(r.cols() == modelPtr_->nv); - assert(m.rows() == r.rows()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.col(independentJointIds[i]) = m.col(i); - } -} - -void DigitModifiedDynamicsConstraints::fill_dependent_rows(MatX& r, const MatX& m, const bool setZero) { - assert(m.rows() == NUM_DEPENDENT_JOINTS); - assert(r.rows() == modelPtr_->nv); - assert(m.cols() == r.cols()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.row(dependentJointIds[i]) = m.row(i); - } -} - -void DigitModifiedDynamicsConstraints::fill_independent_rows(MatX& r, const MatX& m, const bool setZero) { - assert(m.rows() == NUM_INDEPENDENT_JOINTS); - assert(r.rows() == modelPtr_->nv); - assert(m.cols() == r.cols()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.row(independentJointIds[i]) = m.row(i); - } -} - -Eigen::VectorXd DigitModifiedDynamicsConstraints::get_dependent_vector(const VecX& v) { - assert(v.size() == modelPtr_->nv); - - VecX r(NUM_DEPENDENT_JOINTS); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r(i) = v(dependentJointIds[i]); - } - - return r; -} - -Eigen::VectorXd DigitModifiedDynamicsConstraints::get_independent_vector(const VecX& v) { - assert(v.size() == modelPtr_->nv); - - VecX r(NUM_INDEPENDENT_JOINTS); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r(i) = v(independentJointIds[i]); - } - - return r; -} - -void DigitModifiedDynamicsConstraints::get_dependent_columns(MatX& r, const MatX& m) { - assert(m.cols() == modelPtr_->nv); - assert(r.cols() == NUM_DEPENDENT_JOINTS); - assert(m.rows() == r.rows()); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.col(i) = m.col(dependentJointIds[i]); - } -} - -void DigitModifiedDynamicsConstraints::get_independent_columns(MatX& r, const MatX& m) { - assert(m.cols() == modelPtr_->nv); - assert(r.cols() == NUM_INDEPENDENT_JOINTS); - assert(m.rows() == r.rows()); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.col(i) = m.col(independentJointIds[i]); - } -} - -void DigitModifiedDynamicsConstraints::get_dependent_rows(MatX& r, const MatX& m) { - assert(m.rows() == modelPtr_->nv); - assert(r.rows() == NUM_DEPENDENT_JOINTS); - assert(m.cols() == r.cols()); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.row(i) = m.row(dependentJointIds[i]); - } -} - -void DigitModifiedDynamicsConstraints::get_independent_rows(MatX& r, const MatX& m) { - assert(m.rows() == modelPtr_->nv); - assert(r.rows() == NUM_INDEPENDENT_JOINTS); - assert(m.cols() == r.cols()); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.row(i) = m.row(independentJointIds[i]); - } -} - -void DigitModifiedDynamicsConstraints::setupJointPosition(VecX& q, bool compute_derivatives) { - if (recoverSavedData(q, compute_derivatives)) { - return; - } - - // fill in dependent joint positions - fkPtr_->compute(modelPtr_->getJointId("Rz"), - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 0); - - Transform torso_T = stance_foot_T_des * fkPtr_->getTransform().inverse(); - q.block(0, 0, 6, 1) = torso_T.getXYZRPY(); - - if (compute_derivatives) { - get_c(q); - get_J(q); - - get_dependent_columns(J_dep, J); - get_independent_columns(J_indep, J); - - J_dep_qr = QRSolver(J_dep); - J_dep_T_qr = QRSolver(J_dep.transpose()); - - // sanity check on uniqueness (these two arguments are actually equivalent) - if (J_dep_qr.rank() != J_dep.rows() || - J_dep_qr.rank() != J_dep.cols() || - J_dep_T_qr.rank() != J_dep.rows() || - J_dep_T_qr.rank() != J_dep.cols()) { - throw std::runtime_error("Constraint jacobian is not full rank!"); - } - - P_dep = -J_dep_qr.solve(J_indep); - pq_dep_pq_indep = P_dep; - } - - updateQueue(q, compute_derivatives); -} - -void DigitModifiedDynamicsConstraints::get_c(const VecX& q) { - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 0); - - c_translation = fkPtr_->getTranslation() - stance_foot_T_des.p; - c_rotation = fkPtr_->getRPY() - stance_foot_T_des.getRPY(); - - c = VecX(6); - c << c_translation, c_rotation; -} - -void DigitModifiedDynamicsConstraints::get_J(const VecX& q) { - assert(J.rows() == NUM_DEPENDENT_JOINTS); - assert(J.cols() == modelPtr_->nv); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 1); - - J_translation = fkPtr_->getTranslationJacobian(); - J_rotation = fkPtr_->getRPYJacobian(); - - J << J_translation, J_rotation; -} - -void DigitModifiedDynamicsConstraints::get_Jx_partial_dq(const VecX& q, const VecX& x) { - assert(x.size() == modelPtr_->nv); - assert(Jx_partial_dq.rows() == NUM_DEPENDENT_JOINTS); - assert(Jx_partial_dq.cols() == modelPtr_->nv); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 2); - - fkPtr_->getTranslationHessian(H_translation); - fkPtr_->getRPYHessian(H_rotation); - - Jx_partial_dq.row(0) = H_translation(0) * x; - Jx_partial_dq.row(1) = H_translation(1) * x; - Jx_partial_dq.row(2) = H_translation(2) * x; - Jx_partial_dq.row(3) = H_rotation(0) * x; - Jx_partial_dq.row(4) = H_rotation(1) * x; - Jx_partial_dq.row(5) = H_rotation(2) * x; -} - -void DigitModifiedDynamicsConstraints::get_JTx_partial_dq(const VecX& q, const VecX& x) { - assert(x.size() == NUM_DEPENDENT_JOINTS); - assert(JTx_partial_dq.rows() == modelPtr_->nv); - assert(JTx_partial_dq.cols() == modelPtr_->nv); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 2); - - fkPtr_->getTranslationHessian(H_translation); - fkPtr_->getRPYHessian(H_rotation); - - JTx_partial_dq.setZero(); - JTx_partial_dq += H_translation(0) * x(0); - JTx_partial_dq += H_translation(1) * x(1); - JTx_partial_dq += H_translation(2) * x(2); - JTx_partial_dq += H_rotation(0) * x(3); - JTx_partial_dq += H_rotation(1) * x(4); - JTx_partial_dq += H_rotation(2) * x(5); -} - -void DigitModifiedDynamicsConstraints::get_Jxy_partial_dq(const VecX& q, const VecX& x, const VecX& y) { - assert(x.size() == modelPtr_->nv); - assert(y.size() == modelPtr_->nv); - assert(Jxy_partial_dq.rows() == NUM_DEPENDENT_JOINTS); - assert(Jxy_partial_dq.cols() == modelPtr_->nv); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 3); - - fkPtr_->getTranslationThirdOrderTensor(x, TOx_translation); - fkPtr_->getRPYThirdOrderTensor(x, TOx_rotation); - - Jxy_partial_dq.row(0) = TOx_translation(0) * y; - Jxy_partial_dq.row(1) = TOx_translation(1) * y; - Jxy_partial_dq.row(2) = TOx_translation(2) * y; - Jxy_partial_dq.row(3) = TOx_rotation(0) * y; - Jxy_partial_dq.row(4) = TOx_rotation(1) * y; - Jxy_partial_dq.row(5) = TOx_rotation(2) * y; -} - -}; // namespace DigitModified -}; // namespace RAPTOR diff --git a/Examples/Digit-modified/src/DigitModifiedSingleStepOptimizer.cpp b/Examples/Digit-modified/src/DigitModifiedSingleStepOptimizer.cpp deleted file mode 100644 index b3cd0a4e..00000000 --- a/Examples/Digit-modified/src/DigitModifiedSingleStepOptimizer.cpp +++ /dev/null @@ -1,192 +0,0 @@ -#include "DigitModifiedSingleStepOptimizer.h" - -namespace RAPTOR { -namespace DigitModified { - -// // constructor -// DigitModifiedSingleStepOptimizer::DigitModifiedSingleStepOptimizer() -// { -// } - - -// // destructor -// DigitModifiedSingleStepOptimizer::~DigitModifiedSingleStepOptimizer() -// { -// } - -bool DigitModifiedSingleStepOptimizer::set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const TimeDiscretization time_discretization_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const GaitParameters& gp_input - ) -{ - enable_hessian = false; - x0 = x0_input; - - trajPtr_ = std::make_shared(T_input, - N_input, - NUM_INDEPENDENT_JOINTS, - time_discretization_input, - degree_input); - // add v_reset and lambda_reset to the end of the decision variables - trajPtr_->varLength += NUM_JOINTS + NUM_DEPENDENT_JOINTS; - - // stance foot is left foot by default - char stanceLeg = 'L'; - Transform stance_foot_T_des(3, -M_PI / 2); - dcidPtr_ = std::make_shared(model_input, - trajPtr_, - NUM_DEPENDENT_JOINTS, - jtype_input, - stanceLeg, - stance_foot_T_des); - cidPtr_ = dcidPtr_; // convert to base class - - // convert joint limits from degree to radian - VecX JOINT_LIMITS_LOWER_VEC(NUM_JOINTS); - for (int i = 0; i < NUM_JOINTS; i++) { - JOINT_LIMITS_LOWER_VEC(i) = Utils::deg2rad(JOINT_LIMITS_LOWER[i]); - } - - // convert joint limits from degree to radian - VecX JOINT_LIMITS_UPPER_VEC(NUM_JOINTS); - for (int i = 0; i < NUM_JOINTS; i++) { - JOINT_LIMITS_UPPER_VEC(i) = Utils::deg2rad(JOINT_LIMITS_UPPER[i]); - } - - // Surface contact constraints - const frictionParams FRICTION_PARAMS(MU, GAMMA, FOOT_WIDTH, FOOT_LENGTH); - constraintsPtrVec_.push_back(std::make_unique(cidPtr_, - FRICTION_PARAMS)); - constraintsNameVec_.push_back("contact constraints"); - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - cidPtr_->dcPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // Kinematics constraints related to different gait behavior - constraintsPtrVec_.push_back(std::make_unique(model_input, - jtype_input, - trajPtr_, - cidPtr_->dcPtr_, - gp_input)); - constraintsNameVec_.push_back("customized constraints"); - - // Periodic reset map constraints - // constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - // dcidPtr_, - // FRICTION_PARAMS)); - // constraintsNameVec_.push_back("reset map constraints"); - - return true; -} -// [TNLP_set_parameters] - -// [TNLP_get_nlp_info] -bool DigitModifiedSingleStepOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) -{ - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} -// [TNLP_get_nlp_info] - -// [TNLP_eval_f] -// returns the value of the objective function -bool DigitModifiedSingleStepOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - cidPtr_->compute(z, false); - - obj_value = 0; - for ( Index i = 0; i < cidPtr_->N; i++ ) { - obj_value += sqrt(cidPtr_->tau(i).dot(cidPtr_->tau(i))); - } - - obj_value /= cidPtr_->N; - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} -// [TNLP_eval_f] - -// [TNLP_eval_grad_f] -// return the gradient of the objective function grad_{x} f(x) -bool DigitModifiedSingleStepOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - cidPtr_->compute(z, true); - - for ( Index i = 0; i < n; i++ ) { - grad_f[i] = 0; - } - - for ( Index i = 0; i < cidPtr_->N; i++ ) { - VecX v = cidPtr_->ptau_pz(i).transpose() * cidPtr_->tau(i); - double norm_tau = sqrt(cidPtr_->tau(i).dot(cidPtr_->tau(i))); - - for ( Index j = 0; j < n; j++ ) { - grad_f[j] += v(j) / norm_tau; - } - } - - for ( Index i = 0; i < n; i++ ) { - grad_f[i] /= cidPtr_->N; - } - - return true; -} -// [TNLP_eval_grad_f] - -}; // namespace DigitModified -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit-modified/src/DigitModifiedSingleStepPeriodicityConstraints.cpp b/Examples/Digit-modified/src/DigitModifiedSingleStepPeriodicityConstraints.cpp deleted file mode 100644 index b5340843..00000000 --- a/Examples/Digit-modified/src/DigitModifiedSingleStepPeriodicityConstraints.cpp +++ /dev/null @@ -1,231 +0,0 @@ -#include "DigitModifiedSingleStepPeriodicityConstraints.h" - -namespace RAPTOR { -namespace DigitModified { - -DigitModifiedSingleStepPeriodicityConstraints::DigitModifiedSingleStepPeriodicityConstraints(std::shared_ptr& trajPtr_input, - std::shared_ptr dcidPtr_input, - const frictionParams& fp_input) : - trajPtr_(trajPtr_input), - dcidPtr_(dcidPtr_input), - fp(fp_input) { - m = NUM_JOINTS + // H * (v+ - v-) = J * lambda - NUM_DEPENDENT_JOINTS + // J*v+ = 0 - NUM_INDEPENDENT_JOINTS + // position reset - NUM_INDEPENDENT_JOINTS + // velocity reset - 7; // lambda contact constraints - - initialize_memory(m, trajPtr_->varLength, false); - - // initialize intermediate variables - prnea_pq = MatX::Zero(dcidPtr_->modelPtr_->nv, dcidPtr_->modelPtr_->nv); - prnea_pv = MatX::Zero(dcidPtr_->modelPtr_->nv, dcidPtr_->modelPtr_->nv); - prnea_pa = MatX::Zero(dcidPtr_->modelPtr_->nv, dcidPtr_->modelPtr_->nv); - zeroVec = VecX::Zero(dcidPtr_->modelPtr_->nv); - - g1 = VecX::Zero(NUM_JOINTS); - g2 = VecX::Zero(NUM_DEPENDENT_JOINTS); - g3 = VecX::Zero(NUM_INDEPENDENT_JOINTS); - g4 = VecX::Zero(NUM_INDEPENDENT_JOINTS); - g5 = VecX::Zero(7); - - pg1_pz = MatX::Zero(NUM_JOINTS, trajPtr_->varLength); - pg2_pz = MatX::Zero(NUM_DEPENDENT_JOINTS, trajPtr_->varLength); - pg3_pz = MatX::Zero(NUM_INDEPENDENT_JOINTS, trajPtr_->varLength); - pg4_pz = MatX::Zero(NUM_INDEPENDENT_JOINTS, trajPtr_->varLength); - pg5_pz = MatX::Zero(7, trajPtr_->varLength); - - // the following are going to be constant - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - joint_id1[i] = dcidPtr_->modelPtr_->getJointId(JOINT_MAP[i][0]) - 1; - joint_id2[i] = dcidPtr_->modelPtr_->getJointId(JOINT_MAP[i][1]) - 1; - } - - pv_plus_pz = MatX::Zero(NUM_JOINTS, trajPtr_->varLength); - for (int i = 0; i < NUM_JOINTS; i++) { - pv_plus_pz(i, trajPtr_->varLength - NUM_JOINTS - NUM_DEPENDENT_JOINTS + i) = 1; - } - - plambda_pz = MatX::Zero(NUM_DEPENDENT_JOINTS, trajPtr_->varLength); - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - plambda_pz(i, trajPtr_->varLength - NUM_DEPENDENT_JOINTS + i) = 1; - } -} - -void DigitModifiedSingleStepPeriodicityConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("DigitModifiedSingleStepPeriodicityConstraints does not support hessian computation"); - } - - // We assume that surface contact constraints always come after torque limits constraints for now - // The following line has been called in TorqueLimits::compute() already - // So we directly pull out the lambda values from idPtr_ - // dcidPtr_->compute(z, compute_derivatives); - - const int lastIdx = trajPtr_->N - 1; - - const VecX& q_minus = dcidPtr_->q(lastIdx); - const VecX& v_minus = dcidPtr_->v(lastIdx); - - const VecX& v_plus = z.block(trajPtr_->varLength - NUM_JOINTS - NUM_DEPENDENT_JOINTS, 0, NUM_JOINTS, 1); - - const VecX& q_0 = dcidPtr_->q(0); - const VecX& v_0 = dcidPtr_->v(0); - - const VecX& lambda = z.tail(NUM_DEPENDENT_JOINTS); - - // swap stance leg for reset map - dcidPtr_->ddcPtr_->reinitialize(); - - // re-evaluate constraint jacobian - dcidPtr_->dcPtr_->get_c(q_minus); - dcidPtr_->dcPtr_->get_J(q_minus); - - // (1) H * (v+ - v-) = J * lambda - crba(*(dcidPtr_->modelPtr_), *(dcidPtr_->dataPtr_), q_minus); - - MatX H = dcidPtr_->dataPtr_->M; - for (size_t j = 0; j < dcidPtr_->modelPtr_->nv; j++) { - for (size_t k = j + 1; k < dcidPtr_->modelPtr_->nv; k++) { - H(k, j) = H(j, k); - } - } - - g1 = H * (v_plus - v_minus) - dcidPtr_->dcPtr_->J.transpose() * lambda; - - if (compute_derivatives) { - // compute derivatives with gravity turned off, we just need prnea_pq here - const double original_gravity = dcidPtr_->modelPtr_->gravity.linear()(2); - dcidPtr_->modelPtr_->gravity.linear()(2) = 0; - pinocchio::computeRNEADerivatives(*(dcidPtr_->modelPtr_), *(dcidPtr_->dataPtr_), - q_minus, zeroVec, v_plus - v_minus, - prnea_pq, prnea_pv, prnea_pa); - - // restore gravity - dcidPtr_->modelPtr_->gravity.linear()(2) = original_gravity; - - dcidPtr_->dcPtr_->get_JTx_partial_dq(q_minus, lambda); - const MatX& JTx_partial_dq = dcidPtr_->dcPtr_->JTx_partial_dq; - - pg1_pz = (prnea_pq - JTx_partial_dq) * dcidPtr_->pq_pz(lastIdx) + - H * (pv_plus_pz - dcidPtr_->pv_pz(lastIdx)) - - dcidPtr_->dcPtr_->J.transpose() * plambda_pz; - } - - // (2) J * v+ = 0 - g2 = dcidPtr_->dcPtr_->J * v_plus; - - if (compute_derivatives) { - dcidPtr_->dcPtr_->get_Jx_partial_dq(q_minus, v_plus); - const MatX& Jx_partial_dq = dcidPtr_->dcPtr_->Jx_partial_dq; - - pg2_pz = Jx_partial_dq * dcidPtr_->pq_pz(lastIdx) + - dcidPtr_->dcPtr_->J * pv_plus_pz; - } - - // (3) position reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - g3(i) = q_0(joint_id1[i]) + q_minus(joint_id2[i]); - } - - if (compute_derivatives) { - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - pg3_pz.row(i) = dcidPtr_->pq_pz(0).row(joint_id1[i]) + dcidPtr_->pq_pz(lastIdx).row(joint_id2[i]); - } - } - - // (4) velocity reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - g4(i) = v_0(joint_id1[i]) + v_plus(joint_id2[i]); - } - - if (compute_derivatives) { - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - pg4_pz.row(i) = dcidPtr_->pv_pz(0).row(joint_id1[i]) + pv_plus_pz.row(joint_id2[i]); - } - } - - // (5) contact constraints - VecX contactLambda = lambda.tail(6); - - double contact_force = contactLambda(2); - double friction_force_sq = pow(contactLambda(0), 2) + pow(contactLambda(1), 2); - double max_friction_force_sq = pow(fp.mu * contact_force, 2); - double max_moment_z_sq = pow(fp.gamma * contact_force, 2); - double mx_lower_limit = -fp.Lx * contact_force; - double mx_upper_limit = fp.Lx * contact_force; - double my_lower_limit = -fp.Ly * contact_force; - double my_upper_limit = fp.Ly * contact_force; - - // (1) positive contact force - g5(0) = contact_force; - - // (2) translation friction cone - g5(1) = friction_force_sq - max_friction_force_sq; - - // (3) rotation friction cone - g5(2) = pow(contactLambda(5), 2) - max_moment_z_sq; - - // (4, 5) ZMP on one axis - g5(3) = contactLambda(3) - mx_upper_limit; - g5(4) = mx_lower_limit - contactLambda(3); - - // (6, 7) ZMP on the other axis - g5(5) = contactLambda(4) - my_upper_limit; - g5(6) = my_lower_limit - contactLambda(4); - - if (compute_derivatives) { - // assume the contact wrench is always located at the end - MatX pcontactLambda_pz = plambda_pz.bottomRows(6); - - // (1) positive contact force - pg5_pz.row(0) = pcontactLambda_pz.row(2); - - // (2) translation friction cone - pg5_pz.row(1) = 2 * contactLambda(0) * pcontactLambda_pz.row(0) + - 2 * contactLambda(1) * pcontactLambda_pz.row(1) - - 2 * pow(fp.mu, 2) * contact_force * pcontactLambda_pz.row(2); - - // (3) rotation friction cone - pg5_pz.row(2) = 2 * contactLambda(5) * pcontactLambda_pz.row(5) - - 2 * pow(fp.gamma, 2) * contact_force * pcontactLambda_pz.row(2);; - - // (4, 5) ZMP on one axis - pg5_pz.row(3) = pcontactLambda_pz.row(3) - fp.Lx * pcontactLambda_pz.row(2); - pg5_pz.row(4) = -fp.Lx * pcontactLambda_pz.row(2) - pcontactLambda_pz.row(3); - - // (6, 7) ZMP on the other axis - pg5_pz.row(5) = pcontactLambda_pz.row(4) - fp.Ly * pcontactLambda_pz.row(2); - pg5_pz.row(6) = -fp.Ly * pcontactLambda_pz.row(2) - pcontactLambda_pz.row(4); - } - - // swap back for next round evaluation - dcidPtr_->ddcPtr_->reinitialize(); - - // combine all constraints together - g << g1, g2, g3, g4, g5; - - if (compute_derivatives) { - pg_pz << pg1_pz, pg2_pz, pg3_pz, pg4_pz, pg5_pz; - } -} - -void DigitModifiedSingleStepPeriodicityConstraints::compute_bounds() { - // everything before this are equality constraints, so just zeros - // g_lb and g_ub are already initialized to zeros in the constructor - - // g_lb(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS) = 0; - g_ub(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS) = 1e19; - - g_lb.block(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS + 1, 0, 6, 1).setConstant(-1e19); - // g_ub.block(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS + 1, 0, 6, 1).setZero(); -} - -}; // namespace DigitModified -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/DigitMultipleStep.cpp b/Examples/Digit/DigitMultipleStep.cpp deleted file mode 100644 index 532b26dd..00000000 --- a/Examples/Digit/DigitMultipleStep.cpp +++ /dev/null @@ -1,229 +0,0 @@ -#include "DigitMultipleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include -#include - -using namespace RAPTOR; -using namespace Digit; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit/data/"; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // define robot model - const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3; - - // ignore friction for now - model.friction.setZero(); - - // manually import motor inertia - model.rotorInertia(model.getJointId("left_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("left_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("left_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("left_toe_B") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("right_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("right_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_toe_B") - 1) = 0.036089475; - - // load settings - YAML::Node config; - - const double T = 0.4; - int NSteps = 2; - TimeDiscretization time_discretization = Uniform; - int N = 14; - int degree = 5; - - std::vector gps(NSteps); - - try { - config = YAML::LoadFile("../Examples/Digit/multiplestep_optimization_settings.yaml"); - - NSteps = config["NSteps"].as(); - N = config["N"].as(); - degree = config["degree"].as(); - std::string time_discretization_str = config["time_discretization"].as(); - time_discretization = (time_discretization_str == "Uniform") ? Uniform : Chebyshev; - - auto swingfoot_midstep_z_des = config["swingfoot_midstep_z_des"].as>(); - auto swingfoot_begin_y_des = config["swingfoot_begin_y_des"].as>(); - auto swingfoot_end_y_des = config["swingfoot_end_y_des"].as>(); - - if (swingfoot_midstep_z_des.size() != NSteps || - swingfoot_begin_y_des.size() != NSteps || - swingfoot_end_y_des.size() != NSteps) { - throw std::runtime_error("Error parsing YAML file: Incorrect number of gait parameters!"); - } - - gps.resize(NSteps); - for (int i = 0; i < NSteps; i++) { - gps[i].swingfoot_midstep_z_des = swingfoot_midstep_z_des[i]; - gps[i].swingfoot_begin_y_des = swingfoot_begin_y_des[i]; - gps[i].swingfoot_end_y_des = swingfoot_end_y_des[i]; - } - } - catch (std::exception& e) { - std::cerr << "Error parsing YAML file: " << e.what() << std::endl; - throw std::runtime_error("Error parsing YAML file! Check previous error message!"); - } - - Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "initial-digit-Bezier-Six-5.txt"); - // Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "initial-digit-Bezier-Two-14-5-Uniform.txt"); - // Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "initial-digit-Bezier-14-5-Uniform.txt"); - - SmartPtr mynlp = new DigitMultipleStepOptimizer(); - try { - mynlp->set_parameters(NSteps, - z, - T, - N, - time_discretization, - degree, - model, - jtype, - gps); - mynlp->constr_viol_tol = config["constr_viol_tol"].as(); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - try { - app->Options()->SetNumericValue("tol", config["tol"].as()); - app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol); - app->Options()->SetNumericValue("max_wall_time", config["max_wall_time"].as()); - app->Options()->SetIntegerValue("max_iter", config["max_iter"].as()); - app->Options()->SetNumericValue("obj_scaling_factor", config["obj_scaling_factor"].as()); - app->Options()->SetIntegerValue("print_level", config["print_level"].as()); - app->Options()->SetStringValue("mu_strategy", config["mu_strategy"].as().c_str()); - app->Options()->SetStringValue("linear_solver", config["linear_solver"].as().c_str()); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - // app->Options()->SetStringValue("nlp_scaling_method", "none"); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error setting optimization options! Check previous error message!"); - } - - if (config["gredient_check"].as()) { - app->Options()->SetStringValue("output_file", "ipopt.out"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("derivative_test", "second-order"); - } - else { - app->Options()->SetStringValue("derivative_test", "first-order"); - } - app->Options()->SetNumericValue("point_perturbation_radius", 1e-3); - // app->Options()->SetIntegerValue("derivative_test_first_index", 168); - app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - app->Options()->SetNumericValue("derivative_test_tol", 1e-4); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - double solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - - std::cout << "Data needed for comparison: " << mynlp->obj_value_copy << ' ' << mynlp->final_constr_violation << ' ' << solve_time << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Print the solution - if (mynlp->solution.size() == mynlp->numVars) { - std::ofstream solution(filepath + "solution-digit-Bezier-multiple-step.txt"); - solution << std::setprecision(20); - for (int i = 0; i < mynlp->numVars; i++) { - solution << mynlp->solution[i] << std::endl; - } - solution.close(); - - for (int p = 0; p < NSteps; p++) { - std::ofstream trajectory(filepath + "trajectory-digit-Bezier-multiple-step-" + std::to_string(p) + ".txt"); - trajectory << std::setprecision(20); - const auto& cidPtr_ = mynlp->stepOptVec_[p]->cidPtr_; - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < cidPtr_->N; j++) { - trajectory << cidPtr_->q(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < cidPtr_->N; j++) { - trajectory << cidPtr_->v(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < cidPtr_->N; j++) { - trajectory << cidPtr_->a(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - for (int j = 0; j < cidPtr_->N; j++) { - trajectory << cidPtr_->tau(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - for (int j = 0; j < cidPtr_->N; j++) { - trajectory << cidPtr_->lambda(j)(i) << ' '; - } - trajectory << std::endl; - } - trajectory.close(); - } - } - - return 0; -} diff --git a/Examples/Digit/DigitSingleStep.cpp b/Examples/Digit/DigitSingleStep.cpp deleted file mode 100644 index 83c927d8..00000000 --- a/Examples/Digit/DigitSingleStep.cpp +++ /dev/null @@ -1,221 +0,0 @@ -#include "DigitSingleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include -#include - -using namespace RAPTOR; -using namespace Digit; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit/data/"; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // define robot model - const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3; - - // ignore friction for now - model.friction.setZero(); - - // manually import motor inertia - model.rotorInertia(model.getJointId("left_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("left_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("left_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("left_toe_B") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("right_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("right_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_toe_B") - 1) = 0.036089475; - - // load settings - YAML::Node config; - - const double T = 0.4; - TimeDiscretization time_discretization = Uniform; - int N = 14; - int degree = 5; - - GaitParameters gp; - - try { - config = YAML::LoadFile("../Examples/Digit/singlestep_optimization_settings.yaml"); - - N = config["N"].as(); - degree = config["degree"].as(); - std::string time_discretization_str = config["time_discretization"].as(); - time_discretization = (time_discretization_str == "Uniform") ? Uniform : Chebyshev; - - gp.swingfoot_midstep_z_des = config["swingfoot_midstep_z_des"].as(); - gp.swingfoot_begin_y_des = config["swingfoot_begin_y_des"].as(); - gp.swingfoot_end_y_des = config["swingfoot_end_y_des"].as(); - } - catch (std::exception& e) { - std::cerr << "Error parsing YAML file: " << e.what() << std::endl; - } - - // const std::string output_name = std::string(argv[1]) + "-" + std::string(argv[2]); - - Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "initial-digit-Bezier-14-5-Uniform.txt"); - // if (argc > 1) { - // char* end = nullptr; - // std::srand((unsigned int)std::strtoul(argv[1], &end, 10)); - // } - // else { - // std::srand(std::time(nullptr)); - // } - // Eigen::VectorXd z = 0.2 * Eigen::VectorXd::Random((degree + 1) * NUM_INDEPENDENT_JOINTS + NUM_JOINTS + NUM_DEPENDENT_JOINTS).array() - 0.1; - - SmartPtr mynlp = new DigitSingleStepOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - time_discretization, - degree, - model, - jtype, - gp); - mynlp->constr_viol_tol = config["constr_viol_tol"].as(); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - try { - app->Options()->SetNumericValue("tol", config["tol"].as()); - app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol); - app->Options()->SetNumericValue("max_wall_time", config["max_wall_time"].as()); - app->Options()->SetIntegerValue("max_iter", config["max_iter"].as()); - app->Options()->SetNumericValue("obj_scaling_factor", config["obj_scaling_factor"].as()); - app->Options()->SetIntegerValue("print_level", config["print_level"].as()); - app->Options()->SetStringValue("mu_strategy", config["mu_strategy"].as().c_str()); - app->Options()->SetStringValue("linear_solver", config["linear_solver"].as().c_str()); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - // app->Options()->SetStringValue("nlp_scaling_method", "none"); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error setting optimization options! Check previous error message!"); - } - - if (config["gredient_check"].as()) { - app->Options()->SetStringValue("output_file", "ipopt.out"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("derivative_test", "second-order"); - } - else { - app->Options()->SetStringValue("derivative_test", "first-order"); - } - app->Options()->SetNumericValue("point_perturbation_radius", 1e-3); - // app->Options()->SetIntegerValue("derivative_test_first_index", 168); - app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - app->Options()->SetNumericValue("derivative_test_tol", 1e-4); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - double solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - - std::cout << "Data needed for comparison: " << mynlp->obj_value_copy << ' ' << mynlp->final_constr_violation << ' ' << solve_time << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Print the solution - if (mynlp->solution.size() == mynlp->numVars) { - // std::ofstream solution(filepath + - // "robustness_test_solution_" + - // std::to_string(degree) + - // ".txt"); - - // solution << std::setprecision(20); - // for (int i = 0; i < mynlp->numVars; i++) { - // solution << mynlp->solution[i] << std::endl; - // } - // solution.close(); - - // std::ofstream trajectory(filepath + "trajectory-digit-Bezier-" + output_name + ".txt"); - // trajectory << std::setprecision(20); - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->q(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->v(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->a(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->tau(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->cidPtr_->lambda(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // trajectory.close(); - } - - return 0; -} diff --git a/Examples/Digit/DigitSingleStepRobustnessTest.cpp b/Examples/Digit/DigitSingleStepRobustnessTest.cpp deleted file mode 100644 index f99691d6..00000000 --- a/Examples/Digit/DigitSingleStepRobustnessTest.cpp +++ /dev/null @@ -1,145 +0,0 @@ -#include "DigitSingleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include - -using namespace RAPTOR; -using namespace Digit; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit/data/robustness_test/"; - -const int degree_range[] = {5, 6, 7, 8}; -const int N_range[] = {14, 18, 22, 26}; -const std::string mu_strategy_str[] = {"adaptive", "monotone"}; -const std::string linear_solver_str[] = {"ma27", "ma57", "ma86"}; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // define robot model - const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3; - - // ignore friction for now - model.friction.setZero(); - - // manually import motor inertia - model.rotorInertia(model.getJointId("left_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("left_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("left_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("left_toe_B") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("right_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("right_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_toe_B") - 1) = 0.036089475; - - // load settings - const double T = 0.4; - const TimeDiscretization time_discretization = Chebyshev; - int N = 16; - int degree = 5; - - GaitParameters gp; - - SmartPtr app = IpoptApplicationFactory(); - app->Options()->SetNumericValue("tol", 1e-5); - app->Options()->SetNumericValue("constr_viol_tol", 1e-5); - app->Options()->SetNumericValue("max_wall_time", 200.0); - app->Options()->SetIntegerValue("max_iter", 100); - app->Options()->SetNumericValue("obj_scaling_factor", 1e-3); - app->Options()->SetIntegerValue("print_level", 0); - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - for (int degree_choice = 0; degree_choice < 4; degree_choice++) { - degree = degree_range[degree_choice]; - N = N_range[degree_choice]; - for (int mu_strategy_choice = 0; mu_strategy_choice < 2; mu_strategy_choice++) { - app->Options()->SetStringValue("mu_strategy", mu_strategy_str[mu_strategy_choice].c_str()); - for (int linear_solver_choice = 0; linear_solver_choice < 2; linear_solver_choice++) { - app->Options()->SetStringValue("linear_solver", linear_solver_str[linear_solver_choice].c_str()); - - std::cerr << "EXPERIMENT: Degree: " << degree << ", mu_strategy: " << mu_strategy_str[mu_strategy_choice] << ", linear_solver: " << linear_solver_str[linear_solver_choice] << std::endl; - - std::ofstream experiment_output(filepath + - "robustness_test_" + - std::to_string(degree) + - "_" + - mu_strategy_str[mu_strategy_choice] + - "_" + - linear_solver_str[linear_solver_choice] + - ".txt"); - - for (int test_id = 1; test_id <= 100; test_id++) { - std::srand(test_id); - Eigen::VectorXd z = 0.2 * Eigen::VectorXd::Random((degree + 1) * NUM_INDEPENDENT_JOINTS + NUM_JOINTS + NUM_DEPENDENT_JOINTS).array() - 0.1; - - SmartPtr mynlp = new DigitSingleStepOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - time_discretization, - degree, - model, - jtype, - gp); - mynlp->constr_viol_tol = 1e-5; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - double solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - - experiment_output << mynlp->obj_value_copy << ' ' << mynlp->final_constr_violation << ' ' << solve_time << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - } - - experiment_output.close(); - } - } - } - - return 0; -} diff --git a/Examples/Digit/DigitSingleStepSpeedTest.cpp b/Examples/Digit/DigitSingleStepSpeedTest.cpp deleted file mode 100644 index fdba918d..00000000 --- a/Examples/Digit/DigitSingleStepSpeedTest.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include "DigitSingleStepOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include - -using namespace RAPTOR; -using namespace Digit; -using namespace Ipopt; - -const std::string filepath = "../Examples/Digit/data/"; - -const int degree_range[] = {5, 6, 7, 8}; -const int N_range[] = {14, 18, 22, 26}; -const std::string mu_strategy_str[] = {"adaptive", "monotone"}; -const TimeDiscretization time_discret[] = {Uniform, Chebyshev}; -const std::string time_discretization_str[] = {"Uniform", "Chebyshev"}; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // define robot model - const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.806; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3; - - // ignore friction for now - model.friction.setZero(); - - // manually import motor inertia - model.rotorInertia(model.getJointId("left_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("left_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("left_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("left_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("left_toe_B") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_hip_roll") - 1) = 0.173823936; - model.rotorInertia(model.getJointId("right_hip_yaw") - 1) = 0.067899975; - model.rotorInertia(model.getJointId("right_hip_pitch") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_knee") - 1) = 0.1204731904; - model.rotorInertia(model.getJointId("right_toe_A") - 1) = 0.036089475; - model.rotorInertia(model.getJointId("right_toe_B") - 1) = 0.036089475; - - // load settings - const double T = 0.4; - int N = 16; - int degree = 5; - - GaitParameters gp; - gp.swingfoot_midstep_z_des = 0.30; - gp.swingfoot_begin_y_des = 0.40; - gp.swingfoot_end_y_des = -0.40; - - SmartPtr app = IpoptApplicationFactory(); - app->Options()->SetNumericValue("tol", 1e-5); - app->Options()->SetNumericValue("constr_viol_tol", 1e-5); - app->Options()->SetNumericValue("max_wall_time", 200.0); - app->Options()->SetIntegerValue("max_iter", 100); - app->Options()->SetNumericValue("obj_scaling_factor", 1e-3); - app->Options()->SetIntegerValue("print_level", 0); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - std::ofstream experiment_output(filepath + "speed_test.txt"); - - // char* end = nullptr; - // int degree_choice = std::strtoul(argv[1], &end, 10); - // int mu_strategy_choice = std::strtoul(argv[2], &end, 10); - // int discretization_choice = std::strtoul(argv[3], &end, 10); - - for (int degree_choice = 0; degree_choice < 4; degree_choice++) { - for (int mu_strategy_choice = 0; mu_strategy_choice < 2; mu_strategy_choice++) { - for (int discretization_choice = 0; discretization_choice < 2; discretization_choice++) { - degree = degree_range[degree_choice]; - N = N_range[degree_choice]; - app->Options()->SetStringValue("mu_strategy", mu_strategy_str[mu_strategy_choice].c_str()); - TimeDiscretization time_discretization = time_discret[discretization_choice]; - - std::cerr << "EXPERIMENT: Degree: " << degree << ", mu_strategy: " << mu_strategy_str[mu_strategy_choice] << ", time discretization: " << time_discretization_str[discretization_choice] << std::endl; - // experiment_output << "EXPERIMENT: Degree: " << degree << ", mu_strategy: " << mu_strategy_str[mu_strategy_choice] << ", time discretization: " << time_discretization_str[discretization_choice] << std::endl; - - Eigen::VectorXd z = Utils::initializeEigenMatrixFromFile(filepath + "robustness_test_solution_" + std::to_string(degree) + ".txt"); - - SmartPtr mynlp = new DigitSingleStepOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - time_discretization, - degree, - model, - jtype, - gp); - mynlp->constr_viol_tol = 1e-5; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Solve the optimization problem - double solve_time = 0.0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count() / 1000.0; - - experiment_output << " & & & " << mynlp->obj_value_copy << " & " << mynlp->final_constr_violation << " & "; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Evaluate the solution on a finer time discretization - try { - SmartPtr testnlp = new DigitSingleStepOptimizer(); - testnlp->set_parameters(z, - T, - 1000, - TimeDiscretization::Uniform, - degree, - model, - jtype, - gp); - Index n, m, nnz_jac_g, nnz_h_lag; - TNLP::IndexStyleEnum index_style; - testnlp->get_nlp_info(n, m, nnz_jac_g, nnz_h_lag, index_style); - Number ztry[testnlp->numVars], x_l[testnlp->numVars], x_u[testnlp->numVars]; - Number g[testnlp->numCons], g_lb[testnlp->numCons], g_ub[testnlp->numCons]; - for (int i = 0; i < testnlp->numVars; i++) { - ztry[i] = mynlp->solution[i]; - } - testnlp->get_bounds_info(testnlp->numVars, x_l, x_u, testnlp->numCons, g_lb, g_ub); - testnlp->eval_g(testnlp->numVars, ztry, false, testnlp->numCons, g); - testnlp->summarize_constraints(testnlp->numCons, g, true); - experiment_output << testnlp->final_constr_violation << " & " << solve_time << " \\\\" << std::endl; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error evaluating the solution on a finer time discretization! Check previous error message!"); - } - } - } - } - - experiment_output.close(); - - return 0; -} diff --git a/Examples/Digit/include/DigitConstants.h b/Examples/Digit/include/DigitConstants.h deleted file mode 100644 index e1489213..00000000 --- a/Examples/Digit/include/DigitConstants.h +++ /dev/null @@ -1,130 +0,0 @@ -namespace RAPTOR { -namespace Digit { - -// constants related to digit-v3 -constexpr int NUM_JOINTS = 36; -constexpr int NUM_DEPENDENT_JOINTS = 24; -constexpr int NUM_INDEPENDENT_JOINTS = 12; - -// pulled out from digit-v3.xml -// This is in degree!!! -constexpr double JOINT_LIMITS_LOWER[NUM_JOINTS] = { - -1000, // Px - -1000, // Py - -1000, // Pz - -1000, // Rx - -1000, // Ry - -1000, // Rz - -60, // left-hip-roll - -40, // left-hip-yaw - -60, // left-hip-pitch - -180, // left-achilles-rod - -10, // left-ach2 - -16.5480 - 15, // -80, // left-knee - -50.3, // left-tarsus - -26.8765 - 15, // left-toe-A - -180, // left-toe-A-rod - -10, // left-A2 - -1.1295 - 15, // left-toe-B - -180, // left-toe-B-rod - -10, // left-B2 - -44, // left-toe-pitch - -37, // left-toe-roll - -60, // right-hip-roll - -40, // right-hip-yaw - -90, // right-hip-pitch - -180, // right-achilles-rod - -10, // right-ach2 - -46.2821 - 15, // 58.4 // right-knee - -71.6, // right-tarsus - -18.2531 - 15, // -44.9815, // right-toe-A - -180, // right-toe-A-rod - -10, // right-A2 - -26.2724 - 15, // -45.5476, // right-toe-B - -180, // right-toe-B-rod - -10, // right-B2 - -34, // right-toe-pitch - -33 // right-toe-roll -}; - -// pulled out from digit-v3.xml -// This is in degree!!! -constexpr double JOINT_LIMITS_UPPER[NUM_JOINTS] = { - 1000, // Px - 1000, // Py - 1000, // Pz - 1000, // Rx - 1000, // Ry - 1000, // Rz - 60, // left-hip-roll - 40, // left-hip-yaw - 90, // left-hip-pitch - 180, // left-achilles-rod - 10, // left-ach2 - 46.2821 + 15, // 58.4, // left-knee - 71.6, // left-tarsus - 0.4032 + 15, // 44.9815, // left-toe-A - 180, // left-toe-A-rod - 10, // left-A2 - 26.2183 + 15, // 45.5476, // left-toe-B - 180, // left-toe-B-rod - 10, // left-B2 - 34, // left-toe-pitch - 33, // left-toe-roll - 60, // right-hip-roll - 40, // right-hip-yaw - 60, // right-hip-pitch - 180, // right-achilles-rod - 10, // right-ach2 - 30.7455 + 15, // 80, // right-knee - 50.3, // right-tarsus - 27.0323 + 15, // 46.2755, // right-toe-A - 180, // right-toe-A-rod - 10, // right-A2 - 14.6109 + 15, // 45.8918, // right-toe-B - 180, // right-toe-B-rod - 10, // right-B2 - 44, // right-toe-pitch - 37 // right-toe-roll -}; - -// pulled out from digit-v3.xml -constexpr double TORQUE_LIMITS_LOWER[NUM_INDEPENDENT_JOINTS] = { - -126.682, // left-hip-roll - -79.1765, // left-hip-yaw - -216.928, // left-hip-pitch - -231.317, // left-knee - -41.9759, // left-toe-A - -41.9759, // left-toe-B - -126.682, // right-hip-roll - -79.1765, // right-hip-yaw - -216.928, // right-hip-pitch - -231.317, // right-knee - -41.9759, // right-toe-A - -41.9759, // right-toe-B -}; - -// pulled out from digit-v3.xml -constexpr double TORQUE_LIMITS_UPPER[NUM_INDEPENDENT_JOINTS] = { - 126.682, // left-hip-roll - 79.1765, // left-hip-yaw - 216.928, // left-hip-pitch - 231.317, // left-knee - 41.9759, // left-toe-A - 41.9759, // left-toe-B - 126.682, // right-hip-roll - 79.1765, // right-hip-yaw - 216.928, // right-hip-pitch - 231.317, // right-knee - 41.9759, // right-toe-A - 41.9759, // right-toe-B -}; - -// pulled out from digit-v3.xml -constexpr double MU = 0.7; -constexpr double GAMMA = 0.7; -constexpr double FOOT_WIDTH = 0.04; // (m) -constexpr double FOOT_LENGTH = 0.1175; // (m) - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/include/DigitConstrainedInverseDynamics.h b/Examples/Digit/include/DigitConstrainedInverseDynamics.h deleted file mode 100644 index 655aff29..00000000 --- a/Examples/Digit/include/DigitConstrainedInverseDynamics.h +++ /dev/null @@ -1,41 +0,0 @@ - -#ifndef DIGIT_CONSTRAINED_INVERSE_DYNAMICS_H -#define DIGIT_CONSTRAINED_INVERSE_DYNAMICS_H - -#include "ConstrainedInverseDynamics.h" - -#include "DigitDynamicsConstraints.h" - -namespace RAPTOR { -namespace Digit { - -class DigitConstrainedInverseDynamics : public ConstrainedInverseDynamics { -public: - using Model = pinocchio::Model; - - // Constructor - DigitConstrainedInverseDynamics() = default; - - // Constructor (for single step optimization) - DigitConstrainedInverseDynamics(const Model& model_input, - std::shared_ptr& trajPtr_input, - int numDependentJoints_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg, - const Transform& stance_foot_T_des_input); - - // Destructor - ~DigitConstrainedInverseDynamics() = default; - - // class members: - // a pointer type of DigitDynamicsConstraints, - // that shares the same memory with dcPtr_ defined in base class ConstrainedInverseDynamics - // so that other Digit-related class can access specific field in DigitDynamicsConstraints - // such as stanceLeg, stance_foot_T_des, etc. - std::shared_ptr ddcPtr_; -}; - -}; // namespace Digit -}; // namespace RAPTOR - -#endif // DIGIT_CONSTRAINED_INVERSE_DYNAMICS_H diff --git a/Examples/Digit/include/DigitCustomizedConstraints.h b/Examples/Digit/include/DigitCustomizedConstraints.h deleted file mode 100644 index 916f0e13..00000000 --- a/Examples/Digit/include/DigitCustomizedConstraints.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef DIGIT_CUSTOMIZED_CONSTRAINTS_H -#define DIGIT_CUSTOMIZED_CONSTRAINTS_H - -#include "Constraints.h" -#include "FourierCurves.h" -#include "DigitConstrainedInverseDynamics.h" -#include "DigitDynamicsConstraints.h" -#include "Utils.h" - -namespace RAPTOR { -namespace Digit { - -typedef struct GaitParameters_ { - double eps_torso_angle = Utils::deg2rad(3); // 3 degrees - double swingfoot_midstep_z_des = 0.15; // meters - double swingfoot_begin_x_des = -0.22; // meters (negative if left stance, positive if right stance) - double swingfoot_begin_y_des = 0.00; // meters - double swingfoot_end_x_des = -0.22; // meters (negative if left stance, positive if right stance) - double swingfoot_end_y_des = 0.00; // meters -} GaitParameters; - -class DigitCustomizedConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitCustomizedConstraints() = default; - - // Constructor - DigitCustomizedConstraints(const Model& model_input, - const Eigen::VectorXi& jtype_input, - std::shared_ptr& trajPtr_input, - std::shared_ptr& dcPtr_input, - const GaitParameters& gp_input); - - // Destructor - ~DigitCustomizedConstraints() = default; - - // class methods: - // compute constraints - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - virtual void compute_bounds() override; - - // class variables: - GaitParameters gp; - - std::shared_ptr trajPtr_; - std::shared_ptr ddcPtr_; - - std::unique_ptr modelPtr_; - - std::unique_ptr fkPtr_; - - // jtype copy - Eigen::VectorXi jtype; - - // swing foot information - Model::JointIndex swingfoot_id = 0; - - Transform swingfoot_endT; - - Transform leftfoot_endT; - Transform rightfoot_endT; - - // updated in compute() - MatX q; - MatX swingfoot_xyzrpy; - - Eigen::Array pq_pz; - Eigen::Array pswingfoot_xyzrpy_pz; - - // forward kinematics derivatives - std::vector dTdq; -}; - -} // namespace Digit -} // namespace RAPTOR - -#endif // DIGIT_CUSTOMIZED_CONSTRAINTS_H diff --git a/Examples/Digit/include/DigitDynamicsConstraints.h b/Examples/Digit/include/DigitDynamicsConstraints.h deleted file mode 100644 index 7c9ccdba..00000000 --- a/Examples/Digit/include/DigitDynamicsConstraints.h +++ /dev/null @@ -1,216 +0,0 @@ - -#ifndef DIGIT_CONSTRAINTS_H -#define DIGIT_CONSTRAINTS_H - -#include -#include - -#include -#include - -#include "Utils.h" -#include "DigitConstants.h" -#include "DynamicsConstraints.h" -#include "ForwardKinematics.h" - -namespace RAPTOR { -namespace Digit { - -const std::string dependentJointNames[NUM_DEPENDENT_JOINTS] = - {"Px", - "Py", - "Pz", - "Rx", - "Ry", - "Rz", - "left_achilles_rod", - "left_ach2", - "left_tarsus", - "left_toe_A_rod", - "left_A2", - "left_toe_B_rod", - "left_B2", - "left_toe_pitch", - "left_toe_roll", - "right_achilles_rod", - "right_ach2", - "right_tarsus", - "right_toe_A_rod", - "right_A2", - "right_toe_B_rod", - "right_B2", - "right_toe_pitch", - "right_toe_roll"}; - -const std::string independentJointNames[NUM_INDEPENDENT_JOINTS] = - {"left_hip_roll", - "left_hip_yaw", - "left_hip_pitch", - "left_knee", - "left_toe_A", - "left_toe_B", - "right_hip_roll", - "right_hip_yaw", - "right_hip_pitch", - "right_knee", - "right_toe_A", - "right_toe_B"}; - -class DigitDynamicsConstraints : public DynamicsConstraints { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitDynamicsConstraints() = default; - - // Constructor - DigitDynamicsConstraints(const std::shared_ptr& modelPtr_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg, - const Transform& stance_foot_T_des_input); - - // Destructor - ~DigitDynamicsConstraints() = default; - - // class methods: - // swap the stance leg for reset map constraint evaluation - void reinitialize(const char stanceLeg_input = 0); - - // return the index of id th dependent joint - virtual int return_dependent_joint_index(const int id) final override; - - // return the index of id th independent joint - virtual int return_independent_joint_index(const int id) final override; - - // fill in dependent indeces in a vector - virtual void fill_dependent_vector(VecX& r, const VecX& v, const bool setZero = false) final override; - - // fill in independent indeces in a vector - virtual void fill_independent_vector(VecX& r, const VecX& v, const bool setZero = false) final override; - - // fill in dependent columns in a matrix - virtual void fill_dependent_columns(MatX& r, const MatX& m, const bool setZero = false) final override; - - // fill in independent columns in a matrix - virtual void fill_independent_columns(MatX& r, const MatX& m, const bool setZero = false) final override; - - // fill in dependent rows in a matrix - virtual void fill_dependent_rows(MatX& r, const MatX& m, const bool setZero = false) final override; - - // fill in independent rows in a matrix - virtual void fill_independent_rows(MatX& r, const MatX& m, const bool setZero = false) final override; - - // return dependent indeces in a vector - virtual VecX get_dependent_vector(const VecX& v) final override; - - // return independent indeces in a vector - virtual VecX get_independent_vector(const VecX& v) final override; - - // return dependent columns in a matrix - virtual void get_dependent_columns(MatX& r, const MatX& m) final override; - - // return independent columns in a matrix - virtual void get_independent_columns(MatX& r, const MatX& m) final override; - - // return dependent rows in a matrix - virtual void get_dependent_rows(MatX& r, const MatX& m) final override; - - // return independent rows in a matrix - virtual void get_independent_rows(MatX& r, const MatX& m) final override; - - // fill in dependent joint positions in the full joint vector q - // that satisfies the constraints - // This usually involves solving inverse kinematics. - // You need to implement this method in your derived class!!! - virtual void setupJointPosition(VecX& q, bool compute_derivatives = true) override; - - // constraint c(q) - virtual void get_c(const VecX& q) final override; - - // J = jacobian(c, q) - virtual void get_J(const VecX& q) final override; - - // Jx_partial_dq = jacobian(J * x, q) - // where x is a vector that is not dependent on q - virtual void get_Jx_partial_dq(const VecX& q, const VecX& x) final override; - - // JTx_partial_dq = jacobian(J^T * x, q) - // where x is a vector that is not dependent on q - virtual void get_JTx_partial_dq(const VecX& q, const VecX& x) final override; - - // Jxy_partial_dq = jacobian(jacobian(J * x, q) * y, q) - // where x and y are vectors that are not dependent on q - virtual void get_Jxy_partial_dq(const VecX& q, const VecX& x, const VecX& y) final override; - - // class members: - std::shared_ptr modelPtr_ = nullptr; - - std::unique_ptr fkPtr_ = nullptr; - - // dep/indep joint indeces - int dependentJointIds[NUM_DEPENDENT_JOINTS] = {0}; - int independentJointIds[NUM_INDEPENDENT_JOINTS] = {0}; - - // jtype copy - Eigen::VectorXi jtype; - - // for contact constraints (forward kinematics mainly) - Model::JointIndex contact_joint_id = 0; - - // forward kinematics derivatives - Eigen::Array H_translation; - Eigen::Array H_rotation; - - Eigen::Array TOx_translation; - Eigen::Array TOx_rotation; - - // contact foot transforms - char stanceLeg = 'L'; - - Transform stance_foot_T; - Transform stance_foot_endT; - - Transform stance_foot_T_des; - - // closed loop related transforms - // you can find all of these numbers in digit-v3.xml - const Transform left_toeA_rod_endT = Transform(Vec3(0.17 * 2, 0, 0)); - const Transform left_toeA_anchor_endT = Transform(Vec3(0.0179, -0.009551, -0.054164)); - - const Transform left_toeB_rod_endT = Transform(Vec3(0.144 * 2, 0, 0)); - const Transform left_toeB_anchor_endT = Transform(Vec3(-0.0181, -0.009551, -0.054164)); - - const Transform left_knee_rod_endT = Transform(Vec3(0.25 * 2, 0, 0)); - const Transform left_knee_anchor_endT = Transform(Utils::deg2rad(Vec3(4.47, 0.32, 155.8)), - Vec3(-0.01766, -0.029456, 0.00104)) * - Transform(Vec3(0.113789, -0.011056, 0)); - - const Transform right_toeA_rod_endT = Transform(Vec3(0.17 * 2, 0, 0)); - const Transform right_toeA_anchor_endT = Transform(Vec3(0.0179, 0.009551, -0.054164)); - - const Transform right_toeB_rod_endT = Transform(Vec3(0.144 * 2, 0, 0)); - const Transform right_toeB_anchor_endT = Transform(Vec3(-0.0181, 0.009551, -0.054164)); - - const Transform right_knee_rod_endT = Transform(Vec3(0.25 * 2, 0, 0)); - const Transform right_knee_anchor_endT = Transform(Utils::deg2rad(Vec3(-4.47, 0.32, -155.8)), - Vec3(-0.01766, 0.029456, 0.00104)) * - Transform(Vec3(0.113789, 0.011056, 0)); - - VecX qcopy; -}; - -int fillDependent_f(const gsl_vector* x, void *params, gsl_vector* f); - -int fillDependent_df(const gsl_vector* x, void *params, gsl_matrix* J); - -int fillDependent_fdf(const gsl_vector* x, void *params, gsl_vector* f, gsl_matrix* J); - -}; // namespace Digit -}; // namespace RAPTOR - -#endif // DIGIT_CONSTRAINTS_H diff --git a/Examples/Digit/include/DigitMultipleStepOptimizer.h b/Examples/Digit/include/DigitMultipleStepOptimizer.h deleted file mode 100644 index a711be9d..00000000 --- a/Examples/Digit/include/DigitMultipleStepOptimizer.h +++ /dev/null @@ -1,136 +0,0 @@ -#ifndef DIGITMULTIPLESTEPOPTIMIZER_H -#define DIGITMULTIPLESTEPOPTIMIZER_H - -#include "DigitSingleStepOptimizer.h" -#include "DigitMultipleStepPeriodicityConstraints.h" - -namespace RAPTOR { -namespace Digit { - -using namespace Ipopt; - -class DigitMultipleStepOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - DigitMultipleStepOptimizer() = default; - - /** Default destructor */ - ~DigitMultipleStepOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const int NSteps_input, - const VecX& x0_input, - const double T_input, - const int N_input, - const TimeDiscretization time_discretization_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::vector& gps_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the bounds for my problem */ - bool get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /** Method to return the constraint residuals */ - bool eval_g( - Index n, - const Number* x, - bool new_x, - Index m, - Number* g - ) final override; - - /** Method to return: - * 1) The structure of the jacobian (if "values" is NULL) - * 2) The values of the jacobian (if "values" is not NULL) - */ - bool eval_jac_g( - Index n, - const Number* x, - bool new_x, - Index m, - Index nele_jac, - Index* iRow, - Index* jCol, - Number* values - ) final override; - - /** This method summarizes constraint violation for each type of constraints */ - void summarize_constraints( - Index m, - const Number* g, - const bool verbose = true - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - DigitMultipleStepOptimizer( - const DigitMultipleStepOptimizer& - ); - - DigitMultipleStepOptimizer& operator=( - const DigitMultipleStepOptimizer& - ); - - std::vector> stepOptVec_; - std::vector> periodConsVec_; - std::vector n_local; - std::vector n_position; // record the position of decision variables of each walking step in the full decision vector - std::vector m_local; - std::vector m_position; // record the position of constraints of each walking step in the full constraint vector - bool ifFeasibleCurrIter = false; -}; - -}; // namespace Digit -}; // namespace RAPTOR - -#endif // DIGITMULTIPLESTEPOPTIMIZER_H diff --git a/Examples/Digit/include/DigitMultipleStepPeriodicityConstraints.h b/Examples/Digit/include/DigitMultipleStepPeriodicityConstraints.h deleted file mode 100644 index 87bbd5a3..00000000 --- a/Examples/Digit/include/DigitMultipleStepPeriodicityConstraints.h +++ /dev/null @@ -1,74 +0,0 @@ - -#ifndef DIGIT_MULTIPLE_STEP_PERIODICITY_CONSTRAINTS_H -#define DIGIT_MULTIPLE_STEP_PERIODICITY_CONSTRAINTS_H - -#include "DigitSingleStepOptimizer.h" - -namespace RAPTOR { -namespace Digit { - -class DigitMultipleStepPeriodicityConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitMultipleStepPeriodicityConstraints() = default; - - // Constructor - DigitMultipleStepPeriodicityConstraints(std::shared_ptr& currTrajPtr_input, - std::shared_ptr& nextTrajPtr_input, - std::shared_ptr currDcidPtr_input, - std::shared_ptr nextDcidPtr_input, - const frictionParams& fp_input); - - // Destructor - ~DigitMultipleStepPeriodicityConstraints() = default; - - // class methods - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) final override; - - void compute_bounds() final override; - - void print_violation_info() final override; - - // class members - std::shared_ptr& currTrajPtr_; - std::shared_ptr& nextTrajPtr_; - std::shared_ptr currDcidPtr_; - std::shared_ptr nextDcidPtr_; - - frictionParams fp; - - // intermediate variables updated in compute() - MatX prnea_pq; - MatX prnea_pv; - MatX prnea_pa; - VecX zeroVec; - - MatX pv_plus_pz; - MatX plambda_pz; - - VecX g1; - VecX g2; - VecX g3; - VecX g4; - VecX g5; - - MatX pg1_pz; - MatX pg2_pz; - MatX pg3_pz; - MatX pg3_pz2; - MatX pg4_pz; - MatX pg4_pz2; - MatX pg5_pz; -}; - -}; // namespace Digit -}; // namespace RAPTOR - -#endif // DIGIT_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H diff --git a/Examples/Digit/include/DigitSingleStepOptimizer.h b/Examples/Digit/include/DigitSingleStepOptimizer.h deleted file mode 100644 index 82f5b454..00000000 --- a/Examples/Digit/include/DigitSingleStepOptimizer.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef DIGITSINGLESTEPOPTIMIZER_H -#define DIGITSINGLESTEPOPTIMIZER_H - -#include "Optimizer.h" - -#include "BezierCurves.h" - -#include "DigitConstrainedInverseDynamics.h" -#include "DigitDynamicsConstraints.h" -#include "Utils.h" - -#include "ConstrainedJointLimits.h" -#include "TorqueLimits.h" -#include "SurfaceContactConstraints.h" -#include "DigitCustomizedConstraints.h" -#include "DigitSingleStepPeriodicityConstraints.h" - -namespace RAPTOR { -namespace Digit { - -using namespace Ipopt; - -class DigitSingleStepOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - DigitSingleStepOptimizer() = default; - - /** Default destructor */ - ~DigitSingleStepOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const TimeDiscretization time_discretization_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const GaitParameters& gp_input, - const char stanceLeg = 'L', // stance foot is left foot by default - const Transform& stance_foot_T_des = Transform(3, -M_PI / 2), - bool periodic = true - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - DigitSingleStepOptimizer( - const DigitSingleStepOptimizer& - ); - - DigitSingleStepOptimizer& operator=( - const DigitSingleStepOptimizer& - ); - - std::shared_ptr trajPtr_; - - std::shared_ptr dcidPtr_; - std::shared_ptr cidPtr_; -}; - -}; // namespace Digit -}; // namespace RAPTOR - -#endif // DIGITSINGLESTEPOPTIMIZER_H diff --git a/Examples/Digit/include/DigitSingleStepPeriodicityConstraints.h b/Examples/Digit/include/DigitSingleStepPeriodicityConstraints.h deleted file mode 100644 index b1535b72..00000000 --- a/Examples/Digit/include/DigitSingleStepPeriodicityConstraints.h +++ /dev/null @@ -1,95 +0,0 @@ - -#ifndef DIGIT_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H -#define DIGIT_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H - -#include "pinocchio/algorithm/crba.hpp" -#include "pinocchio/algorithm/aba-derivatives.hpp" - -#include "Constraints.h" -#include "DigitConstrainedInverseDynamics.h" -#include "SurfaceContactConstraints.h" - -#include -#include - -namespace RAPTOR { -namespace Digit { - -// ... ==> ... -const std::string JOINT_MAP[NUM_INDEPENDENT_JOINTS][2] = { - {"left_hip_roll", "right_hip_roll"}, - {"left_hip_yaw", "right_hip_yaw"}, - {"left_hip_pitch", "right_hip_pitch"}, - {"left_knee", "right_knee"}, - {"left_toe_A", "right_toe_A"}, - {"left_toe_B", "right_toe_B"}, - {"right_hip_roll", "left_hip_roll"}, - {"right_hip_yaw", "left_hip_yaw"}, - {"right_hip_pitch", "left_hip_pitch"}, - {"right_knee", "left_knee"}, - {"right_toe_A", "left_toe_A"}, - {"right_toe_B", "left_toe_B"}, -}; - -class DigitSingleStepPeriodicityConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - DigitSingleStepPeriodicityConstraints() = default; - - // Constructor - DigitSingleStepPeriodicityConstraints(std::shared_ptr& trajPtr_input, - std::shared_ptr dcidPtr_input, - const frictionParams& fp_input); - - // Destructor - ~DigitSingleStepPeriodicityConstraints() = default; - - // class methods - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) final override; - - void compute_bounds() final override; - - void print_violation_info() final override; - - // class members - std::shared_ptr& trajPtr_; - std::shared_ptr dcidPtr_; - - frictionParams fp; - - // intermediate variables updated in compute() - int joint_id1[NUM_INDEPENDENT_JOINTS]; - int joint_id2[NUM_INDEPENDENT_JOINTS]; - - MatX prnea_pq; - MatX prnea_pv; - MatX prnea_pa; - VecX zeroVec; - - MatX pv_plus_pz; - MatX plambda_pz; - - VecX g1; - VecX g2; - VecX g3; - VecX g4; - VecX g5; - - MatX pg1_pz; - MatX pg2_pz; - MatX pg3_pz; - MatX pg4_pz; - MatX pg5_pz; -}; - -}; // namespace Digit -}; // namespace RAPTOR - -#endif // DIGIT_SINGLE_STEP_PERIODICITY_CONSTRAINTS_H diff --git a/Examples/Digit/multiplestep_optimization_settings.yaml b/Examples/Digit/multiplestep_optimization_settings.yaml deleted file mode 100644 index e24eee77..00000000 --- a/Examples/Digit/multiplestep_optimization_settings.yaml +++ /dev/null @@ -1,65 +0,0 @@ -name: DigitMultipleStep - -### Trajectory settings - # number of steps in the gait -NSteps: 6 - - # number of time nodes to evaluate constraints -N: 16 - - # distribution of the time nodes -time_discretization: Chebyshev # Chebyshev, Uniform - - # degree of the Bezier curve (as the desired trajectory) -degree: 5 - -### Gait behavior settings - # step height (in the middle of the gait) -swingfoot_midstep_z_des: [0.15, 0.225, 0.30, 0.30, 0.225, 0.15] - - # position with respect to the forward direction of the robot at the beginning -swingfoot_begin_y_des: [0.20, -0.30, 0.30, -0.40, 0.30, -0.20] - - # position with respect to the forward direction of the robot at the end -swingfoot_end_y_des: [-0.30, 0.30, -0.40, 0.30, -0.20, 0.20] - - # examples: - # 1. swingfoot_begin_y_des/swingfoot_end_y_des being positive - # means the swing foot is in front of the robot. - # 2. swingfoot_begin_y_des/swingfoot_end_y_des being negative - # means the swing foot is behind the robot. - # 3. swingfoot_begin_y_des being negative, and swingfoot_end_y_des being positive - # means the robot is walking forward. - # 4. swingfoot_begin_y_des being positive, and swingfoot_end_y_des being negative - # means the robot is walking backward. - # 5. For periodic gaits, the absolute values of swingfoot_begin_y_des and swingfoot_end_y_des should be the same, - # since the gait should be symmetric. - # Otherwise, you need to turn off or change the periodicity reset map constraint. - -### Ipopt Optimization settings - # terminal condition 1: optimality tolerance -tol: 1e-5 - - # terminal condition 2: constraint violation tolerance -constr_viol_tol: 1e-3 - - # terminal condition 3: maximum wall time -max_wall_time: 100.0 - - # terminal condition 4: maximum number of iterations -max_iter: 100 - - # scale the objective function -obj_scaling_factor: 1e-3 - - # print level -print_level: 5 # 0: no output, 5: detailed output that is usually enough for tuning parameters - - # mu_strategy -mu_strategy: monotone # adaptive for more aggressive attempts, monotone for more conservative convergence - - # linear solver (HSL) -linear_solver: ma57 # ma27, ma57, ma86, ma97, pardiso - - # turn on gradient check or not -gredient_check: false \ No newline at end of file diff --git a/Examples/Digit/singlestep_optimization_settings.yaml b/Examples/Digit/singlestep_optimization_settings.yaml deleted file mode 100644 index 758a3558..00000000 --- a/Examples/Digit/singlestep_optimization_settings.yaml +++ /dev/null @@ -1,62 +0,0 @@ -name: DigitSingleStep - -### Trajectory settings - # number of time nodes to evaluate constraints -N: 14 - - # distribution of the time nodes -time_discretization: Chebyshev # Chebyshev, Uniform - - # degree of the Bezier curve (as the desired trajectory) -degree: 5 - -### Gait behavior settings - # step height (in the middle of the gait) -swingfoot_midstep_z_des: 0.15 - - # position with respect to the forward direction of the robot at the beginning -swingfoot_begin_y_des: 0.20 - - # position with respect to the forward direction of the robot at the end -swingfoot_end_y_des: -0.20 - - # examples: - # 1. swingfoot_begin_y_des/swingfoot_end_y_des being positive - # means the swing foot is in front of the robot. - # 2. swingfoot_begin_y_des/swingfoot_end_y_des being negative - # means the swing foot is behind the robot. - # 3. swingfoot_begin_y_des being negative, and swingfoot_end_y_des being positive - # means the robot is walking forward. - # 4. swingfoot_begin_y_des being positive, and swingfoot_end_y_des being negative - # means the robot is walking backward. - # 5. For periodic gaits, the absolute values of swingfoot_begin_y_des and swingfoot_end_y_des should be the same, - # since the gait should be symmetric. - # Otherwise, you need to turn off or change the periodicity reset map constraint. - -### Ipopt Optimization settings - # terminal condition 1: optimality tolerance -tol: 1e-5 - - # terminal condition 2: constraint violation tolerance -constr_viol_tol: 1e-4 - - # terminal condition 3: maximum wall time -max_wall_time: 1000.0 - - # terminal condition 4: maximum number of iterations -max_iter: 500 - - # scale the objective function -obj_scaling_factor: 1e-3 - - # print level -print_level: 5 # 0: no output, 5: detailed output that is usually enough for tuning parameters - - # mu_strategy -mu_strategy: adaptive # "adaptive" for more aggressive attempts, "monotone" for more conservative convergence - - # linear solver (HSL) -linear_solver: ma57 # ma27, ma57, ma86, ma97, pardiso - - # turn on gradient check or not -gredient_check: false \ No newline at end of file diff --git a/Examples/Digit/src/DigitConstrainedInverseDynamics.cpp b/Examples/Digit/src/DigitConstrainedInverseDynamics.cpp deleted file mode 100644 index 1a52681c..00000000 --- a/Examples/Digit/src/DigitConstrainedInverseDynamics.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "DigitConstrainedInverseDynamics.h" - -namespace RAPTOR { -namespace Digit { - -DigitConstrainedInverseDynamics::DigitConstrainedInverseDynamics(const Model& model_input, - std::shared_ptr& trajPtr_input, - int NUM_DEPENDENT_JOINTS_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg_input, - const Transform& stance_foot_T_des_input) : - ConstrainedInverseDynamics(model_input, trajPtr_input, NUM_DEPENDENT_JOINTS_input) { - ddcPtr_ = std::make_shared(modelPtr_, - jtype_input, - stanceLeg_input, - stance_foot_T_des_input); - dcPtr_ = ddcPtr_; -} - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/src/DigitCustomizedConstraints.cpp b/Examples/Digit/src/DigitCustomizedConstraints.cpp deleted file mode 100644 index e7f94a49..00000000 --- a/Examples/Digit/src/DigitCustomizedConstraints.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include "DigitCustomizedConstraints.h" - -namespace RAPTOR { -namespace Digit { - -DigitCustomizedConstraints::DigitCustomizedConstraints(const Model& model_input, - const Eigen::VectorXi& jtype_input, - std::shared_ptr& trajPtr_input, - std::shared_ptr& ddcPtr_input, - const GaitParameters& gp_input) : - jtype(jtype_input), - trajPtr_(trajPtr_input), - ddcPtr_(ddcPtr_input), - gp(gp_input) { - modelPtr_ = std::make_unique(model_input); - fkPtr_ = std::make_unique(modelPtr_.get(), jtype); - - leftfoot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - leftfoot_endT.p << 0, -0.05456, -0.0315; - - rightfoot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - rightfoot_endT.p << 0, 0.05456, -0.0315; - - q = MatX::Zero(modelPtr_->nv, trajPtr_->N); - pq_pz.resize(1, trajPtr_->N); - swingfoot_xyzrpy = MatX::Zero(6, trajPtr_->N); - pswingfoot_xyzrpy_pz.resize(1, trajPtr_->N); - - m = trajPtr_->N * 8 + 4; - initialize_memory(m, trajPtr_->varLength, false); -} - -void DigitCustomizedConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (compute_hessian) { - throw std::invalid_argument("DigitCustomizedConstraints does not support hessian computation"); - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - const int fk_order = compute_derivatives ? 1 : 0; - - // compute full joint trajectories and swing foot forward kinematics - for (int i = 0; i < trajPtr_->N; i++) { - if (ddcPtr_->stanceLeg == 'L') { - swingfoot_endT = rightfoot_endT; - swingfoot_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - swingfoot_endT = leftfoot_endT; - swingfoot_id = modelPtr_->getJointId("left_toe_roll"); - } - - VecX qi(modelPtr_->nq); - ddcPtr_->fill_independent_vector(qi, trajPtr_->q(i)); - ddcPtr_->setupJointPosition(qi, compute_derivatives); - q.col(i) = qi; - - fkPtr_->compute(0, swingfoot_id, qi, nullptr, &swingfoot_endT, fk_order); - - swingfoot_xyzrpy.col(i).head(3) = fkPtr_->getTranslation(); - swingfoot_xyzrpy.col(i).tail(3) = fkPtr_->getRPY(); - - if (compute_derivatives) { - pq_pz(i).resize(modelPtr_->nv, trajPtr_->varLength); - - // fill in independent joints derivatives directly - for (int j = 0; j < ddcPtr_->numIndependentJoints; j++) { - int indenpendentJointIndex = ddcPtr_->return_independent_joint_index(j); - pq_pz(i).row(indenpendentJointIndex) = trajPtr_->pq_pz(i).row(j); - } - // compute and fill in dependent joints derivatives - MatX pq_dep_pz = ddcPtr_->pq_dep_pq_indep * trajPtr_->pq_pz(i); - for (int j = 0; j < ddcPtr_->numDependentJoints; j++) { - int denpendentJointIndex = ddcPtr_->return_dependent_joint_index(j); - pq_pz(i).row(denpendentJointIndex) = pq_dep_pz.row(j); - } - - pswingfoot_xyzrpy_pz(i).resize(6, trajPtr_->varLength); - pswingfoot_xyzrpy_pz(i).topRows(3) = fkPtr_->getTranslationJacobian() * pq_pz(i); - pswingfoot_xyzrpy_pz(i).bottomRows(3) = fkPtr_->getRPYJacobian() * pq_pz(i); - } - } - - // (1) swingfoot height always larger than 0 - // height equal to 0 at the beginning and at the end - // height higher than the desired value in the middle - VecX g1 = swingfoot_xyzrpy.row(2); - - // (2) swingfoot always flat and points forward - VecX g2 = Utils::wrapToPi(swingfoot_xyzrpy.row(3)); // swingfoot roll - VecX g3 = Utils::wrapToPi(swingfoot_xyzrpy.row(4)); // swingfoot pitch - VecX g4 = (ddcPtr_->stanceLeg == 'L') ? - Utils::wrapToPi(swingfoot_xyzrpy.row(5).array() + M_PI / 2) : - Utils::wrapToPi(swingfoot_xyzrpy.row(5).array() - M_PI / 2); // swingfoot yaw - - // (3) swingfoot xy equal to desired value - // at the beginning and at the end of each walking step - VecX g5(4); - g5 << swingfoot_xyzrpy.topLeftCorner(2, 1), swingfoot_xyzrpy.topRightCorner(2, 1); - - // (4) torso height always larger than 1 meter - // roll and pitch always close to 0 - // yaw always close to 0 when walking forward - VecX g6 = q.row(2); // torso height - VecX g7 = Utils::wrapToPi(q.row(3)); // torso roll - VecX g8 = Utils::wrapToPi(q.row(4)); // torso pitch - VecX g9 = (ddcPtr_->stanceLeg == 'L') ? - Utils::wrapToPi(q.row(5).array() + M_PI / 2) : - Utils::wrapToPi(q.row(5).array() - M_PI / 2); // torso yaw - - g << g1, g2, g3, g4, g5, g6, g7, g8, g9; - - if (compute_derivatives) { - int iter = 0; - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(2); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(3); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(4); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(i).row(5); - } - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(0).row(0); - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(0).row(1); - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(trajPtr_->N - 1).row(0); - pg_pz.row(iter++) = pswingfoot_xyzrpy_pz(trajPtr_->N - 1).row(1); - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(2); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(3); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(4); - } - for (int i = 0; i < trajPtr_->N; i++) { - pg_pz.row(iter++) = pq_pz(i).row(5); - } - } -} - -void DigitCustomizedConstraints::compute_bounds() { - // (1) swingfoot height always larger than 0 - // height equal to 0 at the beginning and at the end - // height higher than the desired value in the middle - VecX g1_lb = VecX::Zero(trajPtr_->N); - VecX g1_ub = VecX::Constant(trajPtr_->N, 1e19); - g1_lb(trajPtr_->N / 2) = gp.swingfoot_midstep_z_des; - g1_ub(0) = 0; - g1_ub(trajPtr_->N - 1) = 0; - - // (2) swingfoot always flat and points forward - VecX g2_lb = VecX::Zero(trajPtr_->N); - VecX g2_ub = VecX::Zero(trajPtr_->N); - VecX g3_lb = VecX::Zero(trajPtr_->N); - VecX g3_ub = VecX::Zero(trajPtr_->N); - VecX g4_lb = VecX::Zero(trajPtr_->N); - VecX g4_ub = VecX::Zero(trajPtr_->N); - - // (3) swingfoot xy equal to desired value at the beginning and at the end - VecX g5_lb(4); - VecX g5_ub(4); - g5_lb << gp.swingfoot_begin_x_des, gp.swingfoot_begin_y_des, gp.swingfoot_end_x_des, gp.swingfoot_end_y_des; - g5_ub << gp.swingfoot_begin_x_des, gp.swingfoot_begin_y_des, gp.swingfoot_end_x_des, gp.swingfoot_end_y_des; - - // (4) torso height always larger than 1 meter - // roll and pitch always close to 0 - // yaw always close to 0 when walking forward - VecX g6_lb = VecX::Constant(trajPtr_->N, 1); - VecX g6_ub = VecX::Constant(trajPtr_->N, 1e19); - VecX g7_lb = VecX::Constant(trajPtr_->N, -gp.eps_torso_angle); - VecX g7_ub = VecX::Constant(trajPtr_->N, gp.eps_torso_angle); - VecX g8_lb = VecX::Constant(trajPtr_->N, -gp.eps_torso_angle); - VecX g8_ub = VecX::Constant(trajPtr_->N, gp.eps_torso_angle); - VecX g9_lb = VecX::Constant(trajPtr_->N, -gp.eps_torso_angle); - VecX g9_ub = VecX::Constant(trajPtr_->N, gp.eps_torso_angle); - - g_lb << g1_lb, g2_lb, g3_lb, g4_lb, g5_lb, g6_lb, g7_lb, g8_lb, g9_lb; - g_ub << g1_ub, g2_ub, g3_ub, g4_ub, g5_ub, g6_ub, g7_ub, g8_ub, g9_ub; -} - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/src/DigitDynamicsConstraints.cpp b/Examples/Digit/src/DigitDynamicsConstraints.cpp deleted file mode 100644 index b01c760b..00000000 --- a/Examples/Digit/src/DigitDynamicsConstraints.cpp +++ /dev/null @@ -1,3021 +0,0 @@ -#include "DigitDynamicsConstraints.h" - -namespace RAPTOR { -namespace Digit { - -DigitDynamicsConstraints::DigitDynamicsConstraints(const std::shared_ptr& modelPtr_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg_input, - const Transform& stance_foot_T_des_input) : - modelPtr_(modelPtr_input), - jtype(jtype_input), - stanceLeg(stanceLeg_input), - DynamicsConstraints(modelPtr_input->nv, NUM_DEPENDENT_JOINTS) { - fkPtr_ = std::make_unique(modelPtr_.get(), jtype); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - if (modelPtr_->existJointName(dependentJointNames[i])) { - dependentJointIds[i] = modelPtr_->getJointId(dependentJointNames[i]) - 1; - } - else { - throw std::runtime_error("Can not find joint: " + dependentJointNames[i]); - } - } - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (modelPtr_->existJointName(independentJointNames[i])) { - independentJointIds[i] = modelPtr_->getJointId(independentJointNames[i]) - 1; - } - else { - throw std::runtime_error("Can not find joint: " + dependentJointNames[i]); - } - } - - if (stanceLeg == 'L' || stanceLeg == 'l') { - if (modelPtr_->existJointName("left_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("left_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: left_toe_roll"); - } - - stance_foot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, -0.05456, -0.0315; - } - else { - if (modelPtr_->existJointName("right_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: right_toe_roll"); - } - - stance_foot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0.05456, -0.0315; - } - - stance_foot_T_des = stance_foot_T_des_input; -} - -void DigitDynamicsConstraints::reinitialize(const char stanceLeg_input) { - if (stanceLeg_input == 0) { // swap the stance leg if there's no input - if (stanceLeg == 'L' || stanceLeg == 'l') { - stanceLeg = 'R'; - } - else { - stanceLeg = 'L'; - } - } - else { - if (stanceLeg_input != 'L' && - stanceLeg_input != 'R' && - stanceLeg_input != 'l' && - stanceLeg_input != 'r') { - throw std::runtime_error("Invalid stance leg input"); - } - stanceLeg = stanceLeg_input; - } - - // reinitialize the stance leg end effector transformation matrix - if (stanceLeg == 'L' || stanceLeg == 'l') { - contact_joint_id = modelPtr_->getJointId("left_toe_roll"); - stance_foot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, -0.05456, -0.0315; - } - else { - contact_joint_id = modelPtr_->getJointId("right_toe_roll"); - stance_foot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0.05456, -0.0315; - } -} - -int DigitDynamicsConstraints::return_dependent_joint_index(const int id) { - assert(0 <= id && id < NUM_DEPENDENT_JOINTS); - return dependentJointIds[id]; -} - -int DigitDynamicsConstraints::return_independent_joint_index(const int id) { - assert(0 <= id && id < NUM_INDEPENDENT_JOINTS); - return independentJointIds[id]; -} - -void DigitDynamicsConstraints::fill_dependent_vector(VecX& r, const VecX& v, const bool setZero) { - assert(r.size() == modelPtr_->nv); - assert(v.size() == NUM_DEPENDENT_JOINTS); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r(dependentJointIds[i]) = v(i); - } -} - -void DigitDynamicsConstraints::fill_independent_vector(VecX& r, const VecX& v, const bool setZero) { - assert(r.size() == modelPtr_->nv); - assert(v.size() == NUM_INDEPENDENT_JOINTS); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r(independentJointIds[i]) = v(i); - } -} - -void DigitDynamicsConstraints::fill_dependent_columns(MatX& r, const MatX& m, const bool setZero) { - assert(m.cols() == NUM_DEPENDENT_JOINTS); - assert(r.cols() == modelPtr_->nv); - assert(m.rows() == r.rows()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.col(dependentJointIds[i]) = m.col(i); - } -} - -void DigitDynamicsConstraints::fill_independent_columns(MatX& r, const MatX& m, const bool setZero) { - assert(m.cols() == NUM_INDEPENDENT_JOINTS); - assert(r.cols() == modelPtr_->nv); - assert(m.rows() == r.rows()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.col(independentJointIds[i]) = m.col(i); - } -} - -void DigitDynamicsConstraints::fill_dependent_rows(MatX& r, const MatX& m, const bool setZero) { - assert(m.rows() == NUM_DEPENDENT_JOINTS); - assert(r.rows() == modelPtr_->nv); - assert(m.cols() == r.cols()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.row(dependentJointIds[i]) = m.row(i); - } -} - -void DigitDynamicsConstraints::fill_independent_rows(MatX& r, const MatX& m, const bool setZero) { - assert(m.rows() == NUM_INDEPENDENT_JOINTS); - assert(r.rows() == modelPtr_->nv); - assert(m.cols() == r.cols()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.row(independentJointIds[i]) = m.row(i); - } -} - -Eigen::VectorXd DigitDynamicsConstraints::get_dependent_vector(const VecX& v) { - assert(v.size() == modelPtr_->nv); - - VecX r(NUM_DEPENDENT_JOINTS); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r(i) = v(dependentJointIds[i]); - } - - return r; -} - -Eigen::VectorXd DigitDynamicsConstraints::get_independent_vector(const VecX& v) { - assert(v.size() == modelPtr_->nv); - - VecX r(NUM_INDEPENDENT_JOINTS); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r(i) = v(independentJointIds[i]); - } - - return r; -} - -void DigitDynamicsConstraints::get_dependent_columns(MatX& r, const MatX& m) { - assert(m.cols() == modelPtr_->nv); - - r.resize(m.rows(), NUM_DEPENDENT_JOINTS); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.col(i) = m.col(dependentJointIds[i]); - } -} - -void DigitDynamicsConstraints::get_independent_columns(MatX& r, const MatX& m) { - assert(m.cols() == modelPtr_->nv); - - r.resize(m.rows(), NUM_INDEPENDENT_JOINTS); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.col(i) = m.col(independentJointIds[i]); - } -} - -void DigitDynamicsConstraints::get_dependent_rows(MatX& r, const MatX& m) { - assert(m.rows() == modelPtr_->nv); - - r.resize(NUM_DEPENDENT_JOINTS, m.cols()); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.row(i) = m.row(dependentJointIds[i]); - } -} - -void DigitDynamicsConstraints::get_independent_rows(MatX& r, const MatX& m) { - assert(m.rows() == modelPtr_->nv); - - r.resize(NUM_INDEPENDENT_JOINTS, m.cols()); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.row(i) = m.row(independentJointIds[i]); - } -} - -void DigitDynamicsConstraints::setupJointPosition(VecX& q, bool compute_derivatives) { - if (recoverSavedData(q, compute_derivatives)) { - return; - } - - qcopy = q; - - // fill in dependent joint positions - // we first use approximation to give an initial guess for dependent joints - // then we use gsl multidimensional root-finding to provide a more accurate solution - - // a Fourier based approximation. the code is modified from code generation by Matlab symbolic toolbox - // right toe close loop - double q1 = q(modelPtr_->getJointId("right_toe_A") - 1); - double q2 = q(modelPtr_->getJointId("right_toe_B") - 1); - double t2 = cos(q1); - double t3 = cos(q2); - double t4 = sin(q1); - double t5 = sin(q2); - double t6 = q1*2.0; - double t7 = q2*2.0; - double t8 = cos(t6); - double t9 = cos(t7); - double t10 = sin(t6); - double t11 = sin(t7); - qcopy(modelPtr_->getJointId("right_toe_A_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*5.114967905744713E-1+t3*4.072902857734452E-1+t4*5.15019991000208E-1-t5*7.189052210514619E-1-t8*1.914308797309167E-1+t9*1.020482430826409E-1+t10*2.762037569148706E-1+t11*4.461078267300372E-1-t2*t3*5.669643819573642E-1+t2*t5*9.826313004441752E-1+t3*t4*7.066611122892024E-1-t4*t5*8.751268427354268E-2-t2*t9*1.526013089230614E-1+t3*t8*2.146475284950957E-1-t2*t11*6.373722738059316E-1-t3*t10*4.061233553924697E-1-t4*t9*2.591685269591633E-1-t5*t8*3.141536266927853E-1+t4*t11*1.154141052196728E-1+t5*t10*5.111686735370716E-2+t8*t9*2.629326893467769E-2+t8*t11*2.261999172185061E-1+t9*t10*1.714455893661597E-1-t10*t11*5.322298539488757E-2-3.496118766033006E-1); - qcopy(modelPtr_->getJointId("right_A2") - 1) = HigherOrderDerivatives::safeasin(t2*8.08508988653757+t3*8.033204820099098+t4*1.820663506981432+t5*9.760946687487468E-1-t8*2.143120822774579-t9*1.998293355526279-t10*1.095961671709748-t11*4.953901287885355E-1-t2*t3*1.273036938982479E+1-t2*t5*1.117825053271672-t3*t4*2.420405429552186+t4*t5*1.170037217867019+t2*t9*2.835751824751641+t3*t8*3.021210259875125+t2*t11*5.805706025081291E-1+t3*t10*1.468854423683356+t4*t9*5.420356015832145E-1+t5*t8*2.326217949887465E-1+t4*t11*1.600391744429722E-1+t5*t10*1.170908991628156E-1-t8*t9*4.40337591097512E-1-t8*t11*1.065731420572547E-1-t9*t10*3.400548068856727E-1-t10*t11*4.121086576936894E-1-4.663948776295725); - qcopy(modelPtr_->getJointId("right_toe_B_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*(-6.277469604269263E-1)-t3*6.676022839778379E-1-t4*8.399423316628958E-1+t5*1.529719504893023E-1+t8*1.44381731007234E-1+t9*4.870768316848186E-1+t10*5.196004288358185E-1+t11*4.899787657102767E-1+t2*t3*1.038356653790214+t2*t5*1.224128324227442+t3*t4*1.136249345040226-t4*t5*2.962244825741616E-1-t2*t9*7.061936994304631E-1-t3*t8*2.508516890107657E-1-t2*t11*7.147313228957868E-1-t3*t10*7.361945978788478E-1-t4*t9*3.638713473685915E-1-t5*t8*4.137119943797418E-1+t4*t11*2.198247731883075E-1+t5*t10*1.190720324216782E-1+t8*t9*1.91374695742122E-1+t8*t11*2.674233531253601E-1+t9*t10*2.592073581826719E-1-t10*t11*1.033333326963321E-1+3.90361533934657E-1); - qcopy(modelPtr_->getJointId("right_B2") - 1) = HigherOrderDerivatives::safeasin(t2*9.834460864042423+t3*1.012210878062529E+1+t4*4.114199768067971+t5*2.918615505121678-t8*2.33635398066763-t9*2.824506662141518-t10*2.436915399445716-t11*1.657161229950764-t2*t3*1.56824564483544E+1-t2*t5*4.071020737587724-t3*t4*5.741194151827515+t4*t5*1.412990674523749+t2*t9*3.933187185458805+t3*t8*3.362495208957303+t2*t11*2.232327688060054+t3*t10*3.377470435585635+t4*t9*1.500870022094839+t5*t8*1.104866050029796+t4*t11*1.111537011807542E-1+t5*t10*1.885808892070127E-1-t8*t9*5.75161230009884E-1-t8*t11*6.064451339600673E-1-t9*t10*9.059551372991537E-1-t10*t11*4.795029892364048E-1-5.829415122169579); - qcopy(modelPtr_->getJointId("right_toe_pitch") - 1) = -HigherOrderDerivatives::safeasin(t2*1.893989559445235E+1+t3*1.920869550741533E+1+t4*6.868951098405778+t5*3.51561215609706-t8*4.080342460091422-t9*4.456343821978674-t10*3.407846365996183-t11*2.503419572692307-t2*t3*2.929974638198693E+1-t2*t5*5.157359318839895-t3*t4*8.730366732237334+t4*t5*1.967066798625298+t2*t9*5.895002722892592+t3*t8*5.481616764469075+t2*t11*3.209642327668312+t3*t10*4.724342470672166+t4*t9*2.235861962074205+t5*t8*1.242934577497499+t4*t11*1.119458771782081+t5*t10*1.139096075125015-t8*t9*3.306304563913454E-1-t8*t11*7.650487856200098E-1-t9*t10*1.240832663118515-t10*t11*1.549291982370939-1.135688467845833E+1); - qcopy(modelPtr_->getJointId("right_toe_roll") - 1) = -HigherOrderDerivatives::safeasin(t2*1.925184467728597+t3*7.345366777287639+t4*2.843699508105022E+1+t5*2.720900626308319E+1+t8*3.57433462778918-t9*6.332457856504018-t10*1.691966583575934E+1-t11*1.60681725794845E+1-t2*t3*6.299462363896048-t2*t5*4.117199083152367E+1-t3*t4*4.282360516873334E+1+t4*t5*7.910198316638063E-2+t2*t9*7.854243405249483-t3*t8*4.011841131466795+t2*t11*2.320584908063003E+1+t3*t10*2.435545111239733E+1+t4*t9*1.284334673751474E+1+t5*t8*1.242519321726289E+1-t4*t11*1.106503053903957+t5*t10*8.20592115289103E-1-t8*t9*6.089534825702931E-1-t8*t11*7.206363286390523-t9*t10*7.502224690460121+t10*t11*1.255468934813467E-1-3.446675672448973); - - // left toe close loop - q1 = q(modelPtr_->getJointId("left_toe_A") - 1); - q2 = q(modelPtr_->getJointId("left_toe_B") - 1); - t2 = cos(q1); - t3 = cos(q2); - t4 = sin(q1); - t5 = sin(q2); - t6 = q1*2.0; - t7 = q2*2.0; - t8 = cos(t6); - t9 = cos(t7); - t10 = sin(t6); - t11 = sin(t7); - qcopy(modelPtr_->getJointId("left_toe_A_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*(-5.114926838701811E-1)-t3*4.072861778028993E-1+t4*5.150198314974532E-1-t5*7.189053388109297E-1+t8*1.914297694169048E-1-t9*1.020493568564005E-1+t10*2.762038362432678E-1+t11*4.461078824742524E-1+t2*t3*5.669587551620541E-1+t2*t5*9.826314482751646E-1+t3*t4*7.066613185154341E-1+t4*t5*8.751254943270917E-2+t2*t9*1.526028367712922E-1-t3*t8*2.146460048968412E-1-t2*t11*6.3737234338624E-1-t3*t10*4.061234579946822E-1-t4*t9*2.591685735898548E-1-t5*t8*3.141536565807085E-1-t4*t11*1.15414032157577E-1-t5*t10*5.111679397817798E-2-t8*t9*2.629368451925844E-2+t8*t11*2.261999309581469E-1+t9*t10*1.714456126031132E-1+t10*t11*5.322294548977349E-2+3.49608876912165E-1); - qcopy(modelPtr_->getJointId("left_A2") - 1) = HigherOrderDerivatives::safeasin(t2*8.085018160207163+t3*8.033133064086231-t4*1.820662389025083-t5*9.760941431166779E-1-t8*2.143101587807895-t9*1.998274051537184+t10*1.095961134647075+t11*4.953899201857669E-1-t2*t3*1.273027136495291E+1+t2*t5*1.11782444127773+t3*t4*2.420403987960523+t4*t5*1.17003991016308+t2*t9*2.835725404415498+t3*t8*3.021183922843985-t2*t11*5.805703711334091E-1-t3*t10*1.468853731639082-t4*t9*5.420352775054091E-1-t5*t8*2.326217084069311E-1+t4*t11*1.600377315075955E-1+t5*t10*1.170894522681423E-1-t8*t9*4.403304524513951E-1+t8*t11*1.065731191784157E-1+t9*t10*3.400546515442943E-1-t10*t11*4.121078791939159E-1-4.663896238435751); - qcopy(modelPtr_->getJointId("left_toe_B_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*6.277394162562625E-1+t3*6.6759473381467E-1-t4*8.399421448959555E-1+t5*1.52972128181329E-1-t8*1.443797201746081E-1-t9*4.870748185116889E-1+t10*5.196003431996498E-1+t11*4.899786813403554E-1-t2*t3*1.038346357873659+t2*t5*1.224128102824049+t3*t4*1.13624910964581+t4*t5*2.962247949009167E-1+t2*t9*7.06190949266457E-1+t3*t8*2.508489399062072E-1-t2*t11*7.147312184882999E-1-t3*t10*7.361944907642444E-1-t4*t9*3.638712999144287E-1-t5*t8*4.137119506580644E-1-t4*t11*2.198249429188003E-1-t5*t10*1.190722007592886E-1-t8*t9*1.913739574570494E-1+t8*t11*2.67423333074767E-1+t9*t10*2.592073373292207E-1+t10*t11*1.033334244581574E-1-3.903559984881447E-1); - qcopy(modelPtr_->getJointId("left_B2") - 1) = HigherOrderDerivatives::safeasin(t2*9.83436298937519+t3*1.012201086685809E+1-t4*4.114198671144258-t5*2.918615149671792-t8*2.3363277301986-t9*2.824480323210008+t10*2.436914889769644+t11*1.657161129062592-t2*t3*1.568232268107518E+1+t2*t5*4.071020370775729+t3*t4*5.741192746046224+t4*t5*1.412994342972937+t2*t9*3.933151135492005+t3*t8*3.362459265404445-t2*t11*2.232327610328257-t3*t10*3.377469784128673-t4*t9*1.500869712732554-t5*t8*1.104866037480153+t4*t11*1.111517321363341E-1+t5*t10*1.885789156342841E-1-t8*t9*5.751514893646594E-1+t8*t11*6.064451565041576E-1+t9*t10*9.05954995114355E-1-t10*t11*4.795019258370027E-1-5.829343436697216); - qcopy(modelPtr_->getJointId("left_toe_pitch") - 1) = HigherOrderDerivatives::safeasin(t2*1.893971200186481E+1+t3*1.920851184673561E+1-t4*6.868948549897667-t5*3.515611050123151-t8*4.080293186785779-t9*4.456294384490135+t10*3.407845158508307+t11*2.503419165594914-t2*t3*2.92994954151725E+1+t2*t5*5.157358065307949+t3*t4*8.730363458834317+t4*t5*1.967073609987012+t2*t9*5.894935044834759+t3*t8*5.481549285471837-t2*t11*3.209641901519396-t3*t10*4.72434092223045-t4*t9*2.235861236273534-t5*t8*1.242934428625482+t4*t11*1.119455114838831+t5*t10*1.139092408157223-t8*t9*3.306121591996909E-1+t8*t11*7.650487658943779E-1+t9*t10*1.240832321393588-t10*t11*1.549290006003741-1.135675024134487E+1); - qcopy(modelPtr_->getJointId("left_toe_roll") - 1) = HigherOrderDerivatives::safeasin(t2*1.92500223858939+t3*7.345184505784512-t4*2.843699897601034E+1-t5*2.720901076343499E+1+t8*3.574383636815427-t9*6.332408732694388+t10*1.691966794969343E+1+t11*1.606817500398276E+1-t2*t3*6.299213096460508+t2*t5*4.117199681273369E+1+t3*t4*4.282361029947926E+1+t4*t5*7.910853681196074E-2+t2*t9*7.854176116345911-t3*t8*4.011908279005063-t2*t11*2.320585230931768E+1-t3*t10*2.435545390288711E+1-t4*t9*1.284334796900915E+1-t5*t8*1.242519468406818E+1-t4*t11*1.106506602910529+t5*t10*8.205885613072367E-1-t8*t9*6.089352667535674E-1+t8*t11*7.206364083227697+t9*t10*7.502225364895965+t10*t11*1.255488247882333E-1-3.446542349892159); - - // right knee close loop - q1 = q(modelPtr_->getJointId("right_knee") - 1); - t2 = cos(q1); - t3 = sin(q1); - t4 = q1*2.0; - t5 = cos(t4); - t6 = sin(t4); - qcopy(modelPtr_->getJointId("right_tarsus") - 1) = -HigherOrderDerivatives::safeasin(t2*1.155848969647063E-3+t3*1.004686948291003+t5*1.274417498011625E-4-t6*1.785981355062532E-3-1.132590494159057E-2); - qcopy(modelPtr_->getJointId("right_achilles_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*(-1.587289102030986E-3)-t3*1.001736520672665+t5*3.407131509821247E-4+t6*9.646678263881318E-4+1.539911054998293E-3); - qcopy(modelPtr_->getJointId("right_ach2") - 1) = HigherOrderDerivatives::safeasin(t2*(-7.197863326636346E-2)-t3*8.929579539511397E-3+t5*2.669904889172627E-4+t6*8.46571305589265E-5+7.18964949007849E-2); - - // left knee close loop - q1 = q(modelPtr_->getJointId("left_knee") - 1); - t2 = cos(q1); - t3 = sin(q1); - t4 = q1*2.0; - t5 = cos(t4); - t6 = sin(t4); - qcopy(modelPtr_->getJointId("left_tarsus") - 1) = HigherOrderDerivatives::safeasin(t2*1.155848972188414E-3-t3*1.004686948291033+t5*1.274417489907877E-4+t6*1.785981355072367E-3-1.132590494335349E-2); - qcopy(modelPtr_->getJointId("left_achilles_rod") - 1) = HigherOrderDerivatives::safeasin(t2*(-1.587289102219775E-3)+t3*1.001736520672708+t5*3.407131510426615E-4-t6*9.646678264174077E-4+1.539911055129276E-3); - qcopy(modelPtr_->getJointId("left_ach2") - 1) = HigherOrderDerivatives::safeasin(t2*(-7.197863326651638E-2)+t3*8.929579539517018E-3+t5*2.669904889661342E-4-t6*8.465713056222183E-5+7.189649490089099E-2); - - fkPtr_->compute(modelPtr_->getJointId("Rz"), - contact_joint_id, - qcopy, - nullptr, - &stance_foot_endT, - 0); - Transform torso_T = stance_foot_T_des * fkPtr_->getTransform().inverse(); - qcopy.block(0, 0, 6, 1) = torso_T.getXYZRPY(); - - // gsl multidimensional root-finding - const gsl_multiroot_fdfsolver_type *T; - gsl_multiroot_fdfsolver *s; - - int status; - size_t i, iter = 0; - - const size_t n = NUM_DEPENDENT_JOINTS; - gsl_multiroot_function_fdf f = {&fillDependent_f, - &fillDependent_df, - &fillDependent_fdf, - n, this}; - - gsl_vector *x = gsl_vector_alloc(n); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - gsl_vector_set(x, i, qcopy(dependentJointIds[i])); - } - - T = gsl_multiroot_fdfsolver_hybridsj; - s = gsl_multiroot_fdfsolver_alloc(T, n); - gsl_multiroot_fdfsolver_set(s, &f, x); - - // printf("\nstart gsl iterations:\n"); - - do { - iter++; - status = gsl_multiroot_fdfsolver_iterate(s); - - if (status) break; - - status = gsl_multiroot_test_residual(s->f, 1e-14); - - // printf ("iter = %ld, status = %s\n", iter, gsl_strerror(status)); - // for (int i = 0; i < 6; i++) { - // printf("%f, ", gsl_vector_get(s->x, i)) ; - // } - // for (int i = 0; i < 6; i++) { - // printf("%f, ", gsl_vector_get(s->f, 18 + i)) ; - // } - // printf("\n"); - } - while (status == GSL_CONTINUE && iter < 50); - - // printf ("total iter = %ld, status = %s\n\n", iter, gsl_strerror(status)); - - // the optimal solution found by gsl! - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - qcopy(dependentJointIds[i]) = gsl_vector_get(s->x, i); - } - - gsl_multiroot_fdfsolver_free(s); - gsl_vector_free(x); - - q = qcopy; - - if (compute_derivatives) { - get_c(q); - get_J(q); - - get_dependent_columns(J_dep, J); - get_independent_columns(J_indep, J); - - J_dep_qr = QRSolver(J_dep); - J_dep_T_qr = QRSolver(J_dep.transpose()); - - // sanity check on uniqueness (these two arguments are actually equivalent) - assert(J_dep_qr.rank() == J_dep.rows() && J_dep_qr.rank() == J_dep.cols()); - assert(J_dep_T_qr.rank() == J_dep.rows() && J_dep_T_qr.rank() == J_dep.cols()); - - P_dep = -J_dep_qr.solve(J_indep); - pq_dep_pq_indep = P_dep; - } - - updateQueue(q, compute_derivatives); -} - -int fillDependent_f(const gsl_vector* x, void *params, gsl_vector* f) { - DigitDynamicsConstraints* constraintsData = (DigitDynamicsConstraints*)params; - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - constraintsData->qcopy(constraintsData->dependentJointIds[i]) = gsl_vector_get(x, i); - } - - constraintsData->get_c(constraintsData->qcopy); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - gsl_vector_set(f, i, constraintsData->c(i)); - } - - return GSL_SUCCESS; -} - -int fillDependent_df(const gsl_vector* x, void *params, gsl_matrix* J) { - DigitDynamicsConstraints* constraintsData = (DigitDynamicsConstraints*)params; - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - constraintsData->qcopy(constraintsData->dependentJointIds[i]) = gsl_vector_get(x, i); - } - - constraintsData->get_J(constraintsData->qcopy); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - for (int j = 0; j < NUM_DEPENDENT_JOINTS; j++) { - gsl_matrix_set(J, i, j, constraintsData->J(i, constraintsData->dependentJointIds[j])); - } - } - - return GSL_SUCCESS; -} - -int fillDependent_fdf(const gsl_vector* x, void *params, gsl_vector* f, gsl_matrix* J) { - fillDependent_f(x, params, f); - fillDependent_df(x, params, J); - - return GSL_SUCCESS; -} - -void DigitDynamicsConstraints::get_c(const VecX& q) { - double t2 = cos(q(9)); - double t3 = cos(q(10)); - double t4 = cos(q(11)); - double t5 = cos(q(12)); - double t6 = cos(q(13)); - double t7 = cos(q(14)); - double t8 = cos(q(15)); - double t9 = cos(q(16)); - double t10 = cos(q(17)); - double t11 = cos(q(18)); - double t12 = cos(q(19)); - double t13 = cos(q(20)); - double t14 = cos(q(24)); - double t15 = cos(q(25)); - double t16 = cos(q(26)); - double t17 = cos(q(27)); - double t18 = cos(q(28)); - double t19 = cos(q(29)); - double t20 = cos(q(30)); - double t21 = cos(q(31)); - double t22 = cos(q(32)); - double t23 = cos(q(33)); - double t24 = cos(q(34)); - double t25 = cos(q(35)); - double t26 = sin(q(9)); - double t27 = sin(q(10)); - double t28 = sin(q(11)); - double t29 = sin(q(12)); - double t30 = sin(q(13)); - double t31 = sin(q(14)); - double t32 = sin(q(15)); - double t33 = sin(q(16)); - double t34 = sin(q(17)); - double t35 = sin(q(18)); - double t36 = sin(q(19)); - double t37 = sin(q(20)); - double t38 = sin(q(24)); - double t39 = sin(q(25)); - double t40 = sin(q(26)); - double t41 = sin(q(27)); - double t42 = sin(q(28)); - double t43 = sin(q(29)); - double t44 = sin(q(30)); - double t45 = sin(q(31)); - double t46 = sin(q(32)); - double t47 = sin(q(33)); - double t48 = sin(q(34)); - double t49 = sin(q(35)); - double t50 = t2*t2; - double t51 = t3*t3; - double t52 = t4*t4; - double t53 = t5*t5; - double t54 = t6*t6; - double t55 = t7*t7; - double t56 = t8*t8; - double t57 = t9*t9; - double t58 = t10*t10; - double t59 = t11*t11; - double t60 = t12*t12; - double t61 = t13*t13; - double t62 = t14*t14; - double t63 = t15*t15; - double t64 = t16*t16; - double t65 = t17*t17; - double t66 = t26*t26; - double t67 = t27*t27; - double t68 = t28*t28; - double t69 = t29*t29; - double t70 = t30*t30; - double t71 = t31*t31; - double t72 = t32*t32; - double t73 = t33*t33; - double t74 = t34*t34; - double t75 = t35*t35; - double t76 = t36*t36; - double t77 = t37*t37; - double t78 = t38*t38; - double t79 = t39*t39; - double t80 = t40*t40; - double t81 = t41*t41; - double t82 = t24*1.696216709330505E-2; - double t83 = t48*1.696216709330505E-2; - double t84 = t49*-9.551E-3; - double t85 = t49*9.551E-3; - double t86 = t24*-5.143951577823025E-2; - double t87 = t24*5.143951577823025E-2; - double t88 = t48*5.143951577823025E-2; - double t97 = t12*t36*-1.758834080497775E-1; - double t98 = t12*t36*1.758834080497775E-1; - double t99 = t12*t36*-1.052664560664766E-1; - double t100 = t12*t36*1.052664560664766E-1; - double t89 = t76*-4.263322803323831E-2; - double t90 = t76*4.263322803323831E-2; - double t91 = t61*-2.04E-1; - double t92 = t61*2.04E-1; - double t93 = t77*-2.04E-1; - double t94 = t77*2.04E-1; - double t95 = t76*-1.405829597511123E-2; - double t96 = t76*1.405829597511123E-2; - double t101 = t60*6.263322803323831E-2; - double t102 = t61*2.0E-2; - double t103 = t60*-1.899417040248888E-1; - double t104 = t60*1.899417040248888E-1; - double t105 = t77*2.0E-2; - double t106 = t12*t61*1.696216709330505E-2; - double t107 = t12*t77*1.696216709330505E-2; - double t108 = t36*t61*1.696216709330505E-2; - double t109 = t37*t60*9.551E-3; - double t110 = t36*t77*1.696216709330505E-2; - double t111 = t12*t61*5.143951577823025E-2; - double t112 = t37*t76*9.551E-3; - double t113 = t12*t77*5.143951577823025E-2; - double t114 = t36*t61*-5.143951577823025E-2; - double t115 = t36*t61*5.143951577823025E-2; - double t116 = t36*t77*-5.143951577823025E-2; - double t117 = t36*t77*5.143951577823025E-2; - double t118 = t61*t76*6.263322803323831E-2; - double t119 = t60*t61*-4.263322803323831E-2; - double t120 = t60*t61*4.263322803323831E-2; - double t121 = t76*t77*6.263322803323831E-2; - double t122 = t60*t77*-4.263322803323831E-2; - double t123 = t60*t77*4.263322803323831E-2; - double t124 = t61*t76*-1.899417040248888E-1; - double t125 = t61*t76*1.899417040248888E-1; - double t126 = t60*t61*-1.405829597511123E-2; - double t127 = t60*t61*1.405829597511123E-2; - double t128 = t76*t77*-1.899417040248888E-1; - double t129 = t76*t77*1.899417040248888E-1; - double t130 = t60*t77*-1.405829597511123E-2; - double t131 = t60*t77*1.405829597511123E-2; - double t132 = t61*t98; - double t133 = t77*t98; - double t134 = t61*t100; - double t135 = t77*t100; - - c[0] = t18*1.034589188110661E-3+t42*5.699060997402858E-2+t82+t88-t24*t25*9.070578524442013E-3-t18*t44*2.246221860400801E-3-t24*t49*1.69996184260823E-2+t25*t48*2.991020934719675E-3-t42*t44*1.26575449899492E-2+t48*t49*5.605619802270151E-3+t18*t19*t20*3.399783924207052E-1+t18*t20*t43*3.105990081579729E-3+t19*t20*t42*3.020283789547073E-3-t20*t42*t43*3.397508858570615E-1-3.49E-1; - c[1] = t18*5.699060997402858E-2-t42*1.034589188110661E-3+t83+t86-t24*t25*2.991020934719675E-3-t18*t44*1.26575449899492E-2-t24*t49*5.605619802270151E-3-t25*t48*9.070578524442013E-3+t42*t44*2.246221860400801E-3-t48*t49*1.69996184260823E-2+t18*t19*t20*3.020283789547073E-3-t18*t20*t43*3.397508858570615E-1-t19*t20*t42*3.399783924207052E-1-t20*t42*t43*3.105990081579729E-3-6.0E-3; - c[2] = t25*1.79E-2+t44*3.39756885202024E-1+t84+t19*t20*2.360206106172353E-3-t20*t43*1.263678697118524E-2-1.96E-2; - c[3] = t21*1.034589188110661E-3-t45*5.699060997402856E-2+t82+t88-t24*t25*9.070578524442012E-3-t21*t47*8.234792310892687E-4+t24*t49*1.718955829676478E-2+t25*t48*2.991020934719677E-3-t45*t47*1.549180159908108E-2-t48*t49*5.668252425759201E-3+t21*t22*t23*2.879825211612492E-1-t21*t23*t46*3.064210757541298E-3-t22*t23*t45*3.104080344633556E-3-t23*t45*t46*2.87566285868891E-1-2.97E-1; - c[4] = t21*5.699060997402856E-2+t45*1.034589188110661E-3+t83+t86-t24*t25*2.991020934719677E-3+t21*t47*1.549180159908108E-2+t24*t49*5.668252425759201E-3-t25*t48*9.070578524442012E-3-t45*t47*8.234792310892687E-4+t48*t49*1.718955829676478E-2+t21*t22*t23*3.104080344633556E-3+t21*t23*t46*2.87566285868891E-1+t22*t23*t45*2.879825211612492E-1-t23*t45*t46*3.064210757541298E-3-6.0E-3; - c[5] = t25*-1.81E-2-t47*2.875818595898751E-1+t84-t22*t23*6.574122182670539E-4+t23*t46*1.549973690114125E-2+1.96E-2; - c[6] = t55*4.075092868664323E-5+t56*2.945782751508952E-2+t71*1.421556223845944E-6+t72*2.945782751508952E-2+t91+t93+t95+t99+t103+t106+t107+t114+t116+t124+t126+t128+t130+t134+t135-t12*t13*9.070578524442013E-3-t7*t31*1.522231734027378E-5+t12*t37*1.69996184260823E-2-t13*t36*2.991020934719675E-3+t6*t55*7.74045457557333E-4+t6*t56*4.455658896021977E-4+t36*t37*5.605619802270151E-3-t6*t71*1.8502215904887E-4+t6*t72*4.455658896021977E-4-t30*t55*4.104331657223103E-4-t30*t56*2.809111102928818E-2-t30*t71*2.848906577901809E-2-t30*t72*2.809111102928818E-2+t54*t55*2.961197292377137E-2+t54*t56*3.670379334125467E-5+t55*t56*1.421556223845944E-6-t54*t71*1.486767171126281E-4-t55*t70*1.527238524580169E-4+t54*t72*3.670379334125467E-5+t56*t70*5.468691569234733E-6+t55*t72*1.421556223845944E-6+t56*t71*4.075092868664323E-5+t70*t71*2.964725516088879E-2+t70*t72*5.468691569234733E-6+t71*t72*4.075092868664323E-5+t6*t7*t8*3.399783924207052E-1+t6*t7*t31*2.809726196867663E-2-t6*t8*t31*3.105990081579729E-3-t7*t8*t30*3.020283789547073E-3-t7*t30*t31*9.515128902424641E-4-t8*t30*t31*3.397508858570615E-1+t6*t30*t55*1.643509328293678E-2+t6*t30*t56*3.732977901401361E-5+t7*t31*t54*1.646136108951249E-2-t6*t32*t55*2.246221860400801E-3+t7*t31*t56*1.522231734027378E-5-t6*t30*t71*1.64724230619508E-2+t6*t30*t72*3.732977901401361E-5-t7*t31*t70*1.644613877217222E-2-t6*t32*t71*2.246221860400801E-3+t7*t31*t72*1.522231734027378E-5-t6*t55*t56*1.8502215904887E-4+t30*t32*t55*1.26575449899492E-2-t6*t55*t72*1.8502215904887E-4+t6*t56*t71*7.74045457557333E-4+t30*t32*t71*1.26575449899492E-2-t30*t55*t56*2.848906577901809E-2+t6*t71*t72*7.74045457557333E-4-t30*t55*t72*2.848906577901809E-2-t30*t56*t71*4.104331657223103E-4-t54*t55*t56*1.486767171126281E-4-t30*t71*t72*4.104331657223103E-4-t54*t55*t72*1.486767171126281E-4+t54*t56*t71*2.961197292377137E-2+t55*t56*t70*2.964725516088879E-2+t54*t71*t72*2.961197292377137E-2+t55*t70*t72*2.964725516088879E-2-t56*t70*t71*1.527238524580169E-4-t70*t71*t72*1.527238524580169E-4-t6*t7*t30*t31*5.956062305521773E-2-t6*t7*t31*t56*2.809726196867663E-2-t6*t7*t31*t72*2.809726196867663E-2+t7*t30*t31*t56*9.515128902424641E-4+t7*t30*t31*t72*9.515128902424641E-4-t6*t30*t55*t56*1.64724230619508E-2-t7*t31*t54*t56*1.646136108951249E-2-t6*t30*t55*t72*1.64724230619508E-2+t6*t30*t56*t71*1.643509328293678E-2-t7*t31*t54*t72*1.646136108951249E-2+t7*t31*t56*t70*1.644613877217222E-2+t6*t30*t71*t72*1.643509328293678E-2+t7*t31*t70*t72*1.644613877217222E-2+t6*t7*t30*t31*t56*5.956062305521773E-2+t6*t7*t30*t31*t72*5.956062305521773E-2; - c[7] = t55*-2.348358602281135E-5-t56*1.697569721208548E-2-t71*8.192018917078324E-7-t72*1.697569721208548E-2+t89+t97+t101+t102+t105+t108+t110+t111+t113+t118+t119+t121+t122+t132+t133+t12*t13*2.991020934719675E-3+t7*t31*8.772182874056074E-6-t12*t37*5.605619802270151E-3-t13*t36*9.070578524442013E-3-t6*t55*4.104331657223105E-4-t6*t56*2.809111102928818E-2+t36*t37*1.69996184260823E-2-t6*t71*2.848906577901809E-2-t6*t72*2.809111102928818E-2-t30*t55*7.74045457557333E-4-t30*t56*4.455658896021977E-4+t30*t71*1.8502215904887E-4-t30*t72*4.455658896021977E-4-t54*t55*2.707115655202057E-4+t54*t56*6.513495549747348E-6-t55*t56*8.192018917078324E-7-t54*t71*1.673580193002955E-2-t55*t70*1.670580484845698E-2+t54*t72*6.513495549747348E-6-t56*t70*3.081628346426626E-5-t55*t72*8.192018917078324E-7-t56*t71*2.348358602281135E-5-t70*t71*2.633788680787505E-4-t70*t72*3.081628346426626E-5-t71*t72*2.348358602281135E-5-t6*t7*t8*3.020283789547073E-3-t6*t7*t31*9.51512890242464E-4-t6*t8*t31*3.397508858570615E-1-t7*t8*t30*3.399783924207052E-1-t7*t30*t31*2.809726196867663E-2+t8*t30*t31*3.105990081579729E-3-t6*t30*t55*2.976469677622939E-2-t6*t30*t56*3.123510177201994E-5-t7*t31*t54*2.978469761904589E-2+t6*t32*t55*1.26575449899492E-2-t7*t31*t56*8.772182874056074E-6+t6*t30*t71*2.979593187800142E-2-t6*t30*t72*3.123510177201994E-5+t7*t31*t70*2.977592543617184E-2+t6*t32*t71*1.26575449899492E-2-t7*t31*t72*8.772182874056074E-6-t6*t55*t56*2.848906577901809E-2+t30*t32*t55*2.246221860400801E-3-t6*t55*t72*2.848906577901809E-2-t6*t56*t71*4.104331657223105E-4+t30*t32*t71*2.246221860400801E-3+t30*t55*t56*1.8502215904887E-4-t6*t71*t72*4.104331657223105E-4+t30*t55*t72*1.8502215904887E-4-t30*t56*t71*7.74045457557333E-4-t54*t55*t56*1.673580193002955E-2-t30*t71*t72*7.74045457557333E-4-t54*t55*t72*1.673580193002955E-2-t54*t56*t71*2.707115655202057E-4-t55*t56*t70*2.633788680787505E-4-t54*t71*t72*2.707115655202057E-4-t55*t70*t72*2.633788680787505E-4-t56*t70*t71*1.670580484845698E-2-t70*t71*t72*1.670580484845698E-2-t6*t7*t30*t31*3.290749986168471E-2+t6*t7*t31*t56*9.51512890242464E-4+t6*t7*t31*t72*9.51512890242464E-4+t7*t30*t31*t56*2.809726196867663E-2+t7*t30*t31*t72*2.809726196867663E-2+t6*t30*t55*t56*2.979593187800142E-2+t7*t31*t54*t56*2.978469761904589E-2+t6*t30*t55*t72*2.979593187800142E-2-t6*t30*t56*t71*2.976469677622939E-2+t7*t31*t54*t72*2.978469761904589E-2-t7*t31*t56*t70*2.977592543617184E-2-t6*t30*t71*t72*2.976469677622939E-2-t7*t31*t70*t72*2.977592543617184E-2+t6*t7*t30*t31*t56*3.290749986168471E-2+t6*t7*t30*t31*t72*3.290749986168471E-2; - c[8] = t109+t112-t6*t55*6.213602549397351E-4+t6*t56*8.271781369638137E-4+t13*t60*1.79E-2-t6*t71*2.058178820240719E-4+t6*t72*8.271781369638137E-4+t30*t55*1.101395785003593E-3-t30*t56*9.852121016062406E-4+t13*t76*1.79E-2-t30*t71*1.161836833973452E-4-t30*t72*9.852121016062406E-4-t54*t55*1.084459637073036E-2+t54*t56*1.04947201084778E-3-t54*t71*9.804875640117422E-3-t55*t70*1.084459637073035E-2+t54*t72*1.04947201084778E-3+t56*t70*1.04947201084778E-3-t70*t71*9.804875640117427E-3+t70*t72*1.04947201084778E-3+t6*t7*t31*1.218023269812134E-3+t7*t30*t31*4.163488242625716E-4+t7*t8*t54*2.360206106172353E-3+t7*t8*t70*2.360206106172353E-3+t7*t31*t54*2.212067055639549E-4+t8*t31*t54*1.263678697118524E-2+t7*t31*t70*2.212067055639517E-4+t8*t31*t70*1.263678697118524E-2-t6*t55*t56*2.058178820240719E-4-t6*t55*t72*2.058178820240719E-4-t6*t56*t71*6.213602549397351E-4-t30*t55*t56*1.161836833973452E-4+t32*t54*t55*3.39756885202024E-1-t6*t71*t72*6.213602549397351E-4-t30*t55*t72*1.161836833973452E-4+t30*t56*t71*1.101395785003593E-3+t32*t54*t71*3.39756885202024E-1+t32*t55*t70*3.39756885202024E-1-t54*t55*t56*9.804875640117422E-3+t30*t71*t72*1.101395785003593E-3+t32*t70*t71*3.39756885202024E-1-t54*t55*t72*9.804875640117422E-3-t54*t56*t71*1.084459637073036E-2-t55*t56*t70*9.804875640117427E-3-t54*t71*t72*1.084459637073036E-2-t55*t70*t72*9.804875640117427E-3-t56*t70*t71*1.084459637073035E-2-t70*t71*t72*1.084459637073035E-2-t6*t7*t31*t56*1.218023269812134E-3-t6*t7*t31*t72*1.218023269812134E-3-t7*t30*t31*t56*4.163488242625716E-4-t7*t30*t31*t72*4.163488242625716E-4-t7*t31*t54*t56*2.212067055639549E-4-t7*t31*t54*t72*2.212067055639549E-4-t7*t31*t56*t70*2.212067055639517E-4-t7*t31*t70*t72*2.212067055639517E-4; - c[9] = t58*1.607521019272677E-4+t59*5.533895870788692E-2+t74*2.891901858161877E-7+t75*5.533895870788692E-2+t91+t93+t95+t99+t103+t106+t107+t114+t116+t124+t126+t128+t130+t134+t135-t12*t13*9.070578524442012E-3-t10*t34*1.363641158467861E-5-t12*t37*1.718955829676478E-2-t13*t36*2.991020934719677E-3+t9*t58*8.255702643738717E-4+t9*t59*4.936925916256938E-4-t36*t37*5.668252425759201E-3-t9*t74*2.846736678889049E-4+t9*t75*4.936925916256938E-4-t33*t58*4.353713116283091E-4+t33*t59*2.893932048270239E-2+t33*t74*2.848666080295449E-2+t33*t75*2.893932048270239E-2+t57*t58*5.551356637805161E-2+t57*t59*1.632022254576177E-4+t58*t59*2.891901858161877E-7-t57*t74*1.767686035092276E-4-t58*t73*1.743184799788775E-4+t57*t75*1.632022254576177E-4-t59*t73*2.160933344533858E-6+t58*t75*2.891901858161877E-7+t59*t74*1.607521019272677E-4+t73*t74*5.567647941332341E-2-t73*t75*2.160933344533858E-6+t74*t75*1.607521019272677E-4+t9*t10*t11*2.879825211612492E-1-t9*t10*t34*2.896401859571867E-2+t9*t11*t34*3.064210757541298E-3+t10*t11*t33*3.104080344633556E-3-t10*t33*t34*1.113095897646181E-3-t11*t33*t34*2.87566285868891E-1-t9*t33*t58*1.576769239528801E-2-t9*t33*t59*3.197767103277491E-5-t10*t34*t57*1.577685653751059E-2-t9*t35*t58*8.234792310892687E-4+t10*t34*t59*1.363641158467861E-5+t9*t33*t74*1.579967006632079E-2-t9*t33*t75*3.197767103277491E-5+t10*t34*t73*1.579049294909527E-2-t9*t35*t74*8.234792310892687E-4+t10*t34*t75*1.363641158467861E-5-t9*t58*t59*2.846736678889049E-4+t33*t35*t58*1.549180159908108E-2-t9*t58*t75*2.846736678889049E-4+t9*t59*t74*8.255702643738717E-4+t33*t35*t74*1.549180159908108E-2+t33*t58*t59*2.848666080295449E-2+t9*t74*t75*8.255702643738717E-4+t33*t58*t75*2.848666080295449E-2-t33*t59*t74*4.353713116283091E-4-t57*t58*t59*1.767686035092276E-4-t33*t74*t75*4.353713116283091E-4-t57*t58*t75*1.767686035092276E-4+t57*t59*t74*5.551356637805161E-2+t58*t59*t73*5.567647941332341E-2+t57*t74*t75*5.551356637805161E-2+t58*t73*t75*5.567647941332341E-2-t59*t73*t74*1.743184799788775E-4-t73*t74*t75*1.743184799788775E-4-t9*t10*t33*t34*1.115410112085772E-1+t9*t10*t34*t59*2.896401859571867E-2+t9*t10*t34*t75*2.896401859571867E-2+t10*t33*t34*t59*1.113095897646181E-3+t10*t33*t34*t75*1.113095897646181E-3+t9*t33*t58*t59*1.579967006632079E-2+t10*t34*t57*t59*1.577685653751059E-2+t9*t33*t58*t75*1.579967006632079E-2-t9*t33*t59*t74*1.576769239528801E-2+t10*t34*t57*t75*1.577685653751059E-2-t10*t34*t59*t73*1.579049294909527E-2-t9*t33*t74*t75*1.576769239528801E-2-t10*t34*t73*t75*1.579049294909527E-2+t9*t10*t33*t34*t59*1.115410112085772E-1+t9*t10*t33*t34*t75*1.115410112085772E-1; - c[10] = t58*-4.923938257231622E-5-t59*1.695067203665005E-2-t74*8.85807776373908E-8-t75*1.695067203665005E-2+t89+t97+t101+t102+t105+t108+t110+t111+t113+t118+t119+t121+t122+t132+t133+t12*t13*2.991020934719677E-3+t10*t34*4.176918863775431E-6+t12*t37*5.668252425759201E-3-t13*t36*9.070578524442012E-3+t9*t58*4.353713116283091E-4-t9*t59*2.893932048270239E-2-t36*t37*1.718955829676478E-2-t9*t74*2.848666080295449E-2-t9*t75*2.893932048270239E-2+t33*t58*8.255702643738717E-4+t33*t59*4.936925916256938E-4-t33*t74*2.846736678889049E-4+t33*t75*4.936925916256938E-4-t57*t58*5.915341110698354E-4-t57*t59*8.675146158589342E-6-t58*t59*8.85807776373908E-8-t57*t74*1.639979074277157E-2-t58*t73*1.635922650635785E-2-t57*t75*8.675146158589342E-6-t59*t73*4.065281719136426E-5-t58*t75*8.85807776373908E-8-t59*t74*4.923938257231622E-5-t73*t74*6.001206764507874E-4-t73*t75*4.065281719136426E-5-t74*t75*4.923938257231622E-5-t9*t10*t11*3.104080344633556E-3+t9*t10*t34*1.113095897646181E-3+t9*t11*t34*2.87566285868891E-1+t10*t11*t33*2.879825211612492E-1-t10*t33*t34*2.896401859571867E-2+t11*t33*t34*3.064210757541298E-3+t9*t33*t58*5.568788485803049E-2+t9*t33*t59*1.653631588021515E-4+t10*t34*t57*5.576841714485674E-2-t9*t35*t58*1.549180159908108E-2-t10*t34*t59*4.176918863775431E-6-t9*t33*t74*5.585324801683264E-2+t9*t33*t75*1.653631588021515E-4-t10*t34*t73*5.577259406372051E-2-t9*t35*t74*1.549180159908108E-2-t10*t34*t75*4.176918863775431E-6-t9*t58*t59*2.848666080295449E-2-t33*t35*t58*8.234792310892687E-4-t9*t58*t75*2.848666080295449E-2+t9*t59*t74*4.353713116283091E-4-t33*t35*t74*8.234792310892687E-4-t33*t58*t59*2.846736678889049E-4+t9*t74*t75*4.353713116283091E-4-t33*t58*t75*2.846736678889049E-4+t33*t59*t74*8.255702643738717E-4-t57*t58*t59*1.639979074277157E-2+t33*t74*t75*8.255702643738717E-4-t57*t58*t75*1.639979074277157E-2-t57*t59*t74*5.915341110698354E-4-t58*t59*t73*6.001206764507874E-4-t57*t74*t75*5.915341110698354E-4-t58*t73*t75*6.001206764507874E-4-t59*t73*t74*1.635922650635785E-2-t73*t74*t75*1.635922650635785E-2-t9*t10*t33*t34*3.156734948660587E-2-t9*t10*t34*t59*1.113095897646181E-3-t9*t10*t34*t75*1.113095897646181E-3+t10*t33*t34*t59*2.896401859571867E-2+t10*t33*t34*t75*2.896401859571867E-2-t9*t33*t58*t59*5.585324801683264E-2-t10*t34*t57*t59*5.576841714485674E-2-t9*t33*t58*t75*5.585324801683264E-2+t9*t33*t59*t74*5.568788485803049E-2-t10*t34*t57*t75*5.576841714485674E-2+t10*t34*t59*t73*5.577259406372051E-2+t9*t33*t74*t75*5.568788485803049E-2+t10*t34*t73*t75*5.577259406372051E-2+t9*t10*t33*t34*t59*3.156734948660587E-2+t9*t10*t33*t34*t75*3.156734948660587E-2; - c[11] = t109+t112-t9*t58*8.817574215578565E-4+t9*t59*7.546580486902988E-4-t13*t60*1.81E-2+t9*t74*1.270993728675576E-4+t9*t75*7.546580486902988E-4-t13*t76*1.81E-2-t33*t58*2.99216464372371E-3+t33*t59*3.029602411709927E-3-t33*t74*3.743776798621798E-5+t33*t75*3.029602411709927E-3+t57*t58*8.240645190217156E-3+t57*t59*1.557524060708425E-3+t57*t74*9.801830749074416E-3+t58*t73*8.240645190217156E-3+t57*t75*1.557524060708425E-3+t59*t73*1.557524060708425E-3+t73*t74*9.801830749074416E-3+t73*t75*1.557524060708425E-3-t9*t10*t34*2.959208971395655E-3+t10*t33*t34*1.009577116650697E-3-t10*t11*t57*6.574122182670539E-4-t10*t11*t73*6.574122182670539E-4+t10*t34*t57*2.297574320158763E-5-t11*t34*t57*1.549973690114125E-2+t10*t34*t73*2.297574320158763E-5-t11*t34*t73*1.549973690114125E-2+t9*t58*t59*1.270993728675576E-4+t9*t58*t75*1.270993728675576E-4-t9*t59*t74*8.817574215578565E-4-t33*t58*t59*3.743776798621798E-5-t35*t57*t58*2.875818595898751E-1-t9*t74*t75*8.817574215578565E-4-t33*t58*t75*3.743776798621798E-5-t33*t59*t74*2.99216464372371E-3-t35*t57*t74*2.875818595898751E-1-t35*t58*t73*2.875818595898751E-1+t57*t58*t59*9.801830749074416E-3-t33*t74*t75*2.99216464372371E-3-t35*t73*t74*2.875818595898751E-1+t57*t58*t75*9.801830749074416E-3+t57*t59*t74*8.240645190217156E-3+t58*t59*t73*9.801830749074416E-3+t57*t74*t75*8.240645190217156E-3+t58*t73*t75*9.801830749074416E-3+t59*t73*t74*8.240645190217156E-3+t73*t74*t75*8.240645190217156E-3+t9*t10*t34*t59*2.959208971395655E-3+t9*t10*t34*t75*2.959208971395655E-3-t10*t33*t34*t59*1.009577116650697E-3-t10*t33*t34*t75*1.009577116650697E-3-t10*t34*t57*t59*2.297574320158763E-5-t10*t34*t57*t75*2.297574320158763E-5-t10*t34*t59*t73*2.297574320158763E-5-t10*t34*t73*t75*2.297574320158763E-5; - c[12] = t16*-3.370300000000001E-2+t40*2.47718E-1-t62*1.98970623391096E-4-t63*2.003180563211872E-5+t78*2.190024290232146E-4-t79*2.003180563211872E-5+t14*t15*6.725214316796237E-2+t16*t17*1.20005624647208E-1+t14*t38*1.586414893789752E-3-t15*t38*4.954563135453205E-1-t16*t41*6.370345907739961E-5-t17*t40*6.370345907739961E-5-t16*t65*3.370300000000001E-2-t40*t41*1.20005624647208E-1-t16*t81*3.3703E-2-t39*t62*4.365860704565494E-4+t40*t65*2.47718E-1-t39*t78*4.365860704565494E-4+t40*t81*2.47718E-1+t62*t63*2.190024290232146E-4-t64*t65*6.0E-2+t62*t79*2.190024290232146E-4-t63*t78*1.98970623391096E-4-t64*t81*6.0E-2-t65*t80*6.0E-2-t78*t79*1.98970623391096E-4-t80*t81*6.0E-2-t14*t38*t63*1.586414893789752E-3-t14*t38*t79*1.586414893789752E-3-6.0E-2; - c[13] = t16*-2.47718E-1-t40*3.370300000000001E-2+t62*2.695145178952449E-5-t63*1.636252192216505E-3+t78*1.60930074042698E-3-t79*1.636252192216505E-3+t14*t15*4.941905177865888E-1+t16*t17*6.370345907739961E-5-t14*t38*4.170075425381788E-4+t15*t38*6.711175107535902E-2+t16*t41*1.20005624647208E-1+t17*t40*1.20005624647208E-1-t16*t65*2.47718E-1-t40*t41*6.370345907739961E-5-t16*t81*2.47718E-1-t39*t62*3.566153386244494E-2-t40*t65*3.370300000000001E-2-t39*t78*3.566153386244494E-2-t40*t81*3.3703E-2+t62*t63*1.60930074042698E-3+t62*t79*1.60930074042698E-3+t63*t78*2.695145178952449E-5+t78*t79*2.695145178952449E-5+t14*t38*t63*4.170075425381788E-4+t14*t38*t79*4.170075425381788E-4; - c[14] = t62*2.299824701031743E-2+t63*1.170180756433687E-4+t78*2.288473491403919E-2+t79*1.170180756433687E-4-t14*t15*3.539606431708408E-2+t14*t38*2.84294570084937E-5-t15*t38*4.365115769377904E-3-t39*t62*4.987264424463382E-1-t39*t78*4.987264424463382E-1+t62*t63*2.288473491403919E-2-t64*t65*1.051397136175053E-2+t62*t79*2.288473491403919E-2+t63*t78*2.299824701031743E-2-t64*t81*1.051397136175053E-2-t65*t80*1.051397136175053E-2+t78*t79*2.299824701031743E-2-t80*t81*1.051397136175053E-2-t14*t38*t63*2.84294570084937E-5-t14*t38*t79*2.84294570084937E-5; - c[15] = t4*-3.370300000000001E-2-t28*2.47718E-1-t50*1.98970623391096E-4-t51*2.003180563211872E-5+t66*2.190024290232146E-4-t67*2.003180563211872E-5+t2*t3*6.725214316796237E-2+t4*t5*1.20005624647208E-1-t2*t26*1.586414893789752E-3+t3*t26*4.954563135453205E-1+t4*t29*6.370345907739961E-5+t5*t28*6.370345907739961E-5-t4*t53*3.370300000000001E-2-t28*t29*1.20005624647208E-1-t4*t69*3.3703E-2-t27*t50*4.365860704565494E-4-t28*t53*2.47718E-1-t27*t66*4.365860704565494E-4-t28*t69*2.47718E-1+t50*t51*2.190024290232146E-4-t52*t53*6.0E-2+t50*t67*2.190024290232146E-4-t51*t66*1.98970623391096E-4-t52*t69*6.0E-2-t53*t68*6.0E-2-t66*t67*1.98970623391096E-4-t68*t69*6.0E-2+t2*t26*t51*1.586414893789752E-3+t2*t26*t67*1.586414893789752E-3-6.0E-2; - c[16] = t4*2.47718E-1-t28*3.370300000000001E-2-t50*2.695145178952449E-5+t51*1.636252192216505E-3-t66*1.60930074042698E-3+t67*1.636252192216505E-3-t2*t3*4.941905177865888E-1-t4*t5*6.370345907739961E-5-t2*t26*4.170075425381788E-4+t3*t26*6.711175107535902E-2+t4*t29*1.20005624647208E-1+t5*t28*1.20005624647208E-1+t4*t53*2.47718E-1+t28*t29*6.370345907739961E-5+t4*t69*2.47718E-1+t27*t50*3.566153386244494E-2-t28*t53*3.370300000000001E-2+t27*t66*3.566153386244494E-2-t28*t69*3.3703E-2-t50*t51*1.60930074042698E-3-t50*t67*1.60930074042698E-3-t51*t66*2.695145178952449E-5-t66*t67*2.695145178952449E-5+t2*t26*t51*4.170075425381788E-4+t2*t26*t67*4.170075425381788E-4; - c[17] = t50*2.299824701031743E-2+t51*1.170180756433687E-4+t66*2.288473491403919E-2+t67*1.170180756433687E-4-t2*t3*3.539606431708408E-2-t2*t26*2.84294570084937E-5+t3*t26*4.365115769377904E-3-t27*t50*4.987264424463382E-1-t27*t66*4.987264424463382E-1+t50*t51*2.288473491403919E-2-t52*t53*1.051397136175053E-2+t50*t67*2.288473491403919E-2+t51*t66*2.299824701031743E-2-t52*t69*1.051397136175053E-2-t53*t68*1.051397136175053E-2+t66*t67*2.299824701031743E-2-t68*t69*1.051397136175053E-2+t2*t26*t51*2.84294570084937E-5+t2*t26*t67*2.84294570084937E-5; - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 0); - - c.block(18, 0, 3, 1) = fkPtr_->getTranslation() - stance_foot_T_des.p; - c.block(21, 0, 3, 1) = fkPtr_->getRPY() - stance_foot_T_des.getRPY(); -} - -void DigitDynamicsConstraints::get_J(const VecX& q) { - assert(J.rows() == NUM_DEPENDENT_JOINTS); - assert(J.cols() == modelPtr_->nv); - - double t2 = cos(q(9)); - double t3 = cos(q(10)); - double t4 = cos(q(11)); - double t5 = cos(q(12)); - double t6 = cos(q(13)); - double t7 = cos(q(14)); - double t8 = cos(q(15)); - double t9 = cos(q(16)); - double t10 = cos(q(17)); - double t11 = cos(q(18)); - double t12 = cos(q(19)); - double t13 = cos(q(20)); - double t14 = cos(q(24)); - double t15 = cos(q(25)); - double t16 = cos(q(26)); - double t17 = cos(q(27)); - double t18 = cos(q(28)); - double t19 = cos(q(29)); - double t20 = cos(q(30)); - double t21 = cos(q(31)); - double t22 = cos(q(32)); - double t23 = cos(q(33)); - double t24 = cos(q(34)); - double t25 = cos(q(35)); - double t26 = sin(q(9)); - double t27 = sin(q(10)); - double t28 = sin(q(11)); - double t29 = sin(q(12)); - double t30 = sin(q(13)); - double t31 = sin(q(14)); - double t32 = sin(q(15)); - double t33 = sin(q(16)); - double t34 = sin(q(17)); - double t35 = sin(q(18)); - double t36 = sin(q(19)); - double t37 = sin(q(20)); - double t38 = sin(q(24)); - double t39 = sin(q(25)); - double t40 = sin(q(26)); - double t41 = sin(q(27)); - double t42 = sin(q(28)); - double t43 = sin(q(29)); - double t44 = sin(q(30)); - double t45 = sin(q(31)); - double t46 = sin(q(32)); - double t47 = sin(q(33)); - double t48 = sin(q(34)); - double t49 = sin(q(35)); - double t50 = t2*t2; - double t51 = t3*t3; - double t52 = t5*t5; - double t53 = t6*t6; - double t54 = t7*t7; - double t55 = t8*t8; - double t56 = t9*t9; - double t57 = t10*t10; - double t58 = t11*t11; - double t59 = t12*t12; - double t60 = t13*t13; - double t61 = t14*t14; - double t62 = t15*t15; - double t63 = t17*t17; - double t64 = t26*t26; - double t65 = t27*t27; - double t66 = t29*t29; - double t67 = t30*t30; - double t68 = t31*t31; - double t69 = t32*t32; - double t70 = t33*t33; - double t71 = t34*t34; - double t72 = t35*t35; - double t73 = t36*t36; - double t74 = t37*t37; - double t75 = t38*t38; - double t76 = t39*t39; - double t77 = t41*t41; - double t78 = t24*1.696216709330505E-2; - double t79 = t25*-9.551E-3; - double t80 = t25*9.551E-3; - double t81 = t48*-1.696216709330505E-2; - double t82 = t48*1.696216709330505E-2; - double t83 = t24*5.143951577823025E-2; - double t84 = t48*5.143951577823025E-2; - double t89 = t4*t5*6.370345907739961E-5; - double t90 = t16*t17*-6.370345907739961E-5; - double t91 = t16*t17*6.370345907739961E-5; - double t92 = t4*t29*6.370345907739961E-5; - double t93 = t5*t28*6.370345907739961E-5; - double t94 = t16*t41*-6.370345907739961E-5; - double t95 = t16*t41*6.370345907739961E-5; - double t96 = t17*t40*-6.370345907739961E-5; - double t97 = t17*t40*6.370345907739961E-5; - double t98 = t28*t29*-6.370345907739961E-5; - double t99 = t28*t29*6.370345907739961E-5; - double t100 = t40*t41*6.370345907739961E-5; - double t101 = t12*t36*3.517668160995551E-1; - double t102 = t4*t5*1.20005624647208E-1; - double t103 = t16*t17*1.20005624647208E-1; - double t104 = t12*t36*-2.105329121329532E-1; - double t105 = t12*t36*2.105329121329532E-1; - double t106 = t4*t29*-1.20005624647208E-1; - double t107 = t4*t29*1.20005624647208E-1; - double t108 = t5*t28*-1.20005624647208E-1; - double t109 = t5*t28*1.20005624647208E-1; - double t110 = t16*t41*-1.20005624647208E-1; - double t111 = t16*t41*1.20005624647208E-1; - double t112 = t17*t40*-1.20005624647208E-1; - double t113 = t17*t40*1.20005624647208E-1; - double t114 = t28*t29*-1.20005624647208E-1; - double t115 = t28*t29*1.20005624647208E-1; - double t116 = t40*t41*-1.20005624647208E-1; - double t117 = t40*t41*1.20005624647208E-1; - double t85 = t73*1.758834080497775E-1; - double t86 = t59*-1.052664560664766E-1; - double t87 = t59*1.052664560664766E-1; - double t88 = t73*1.052664560664766E-1; - double t118 = t59*-1.758834080497775E-1; - double t119 = t59*1.758834080497775E-1; - double t120 = t12*t60*1.696216709330505E-2; - double t121 = t13*t59*9.551E-3; - double t122 = t12*t74*1.696216709330505E-2; - double t123 = t36*t60*-1.696216709330505E-2; - double t124 = t36*t60*1.696216709330505E-2; - double t125 = t13*t73*9.551E-3; - double t126 = t36*t74*-1.696216709330505E-2; - double t127 = t36*t74*1.696216709330505E-2; - double t128 = t12*t60*-5.143951577823025E-2; - double t129 = t12*t60*5.143951577823025E-2; - double t130 = t12*t74*-5.143951577823025E-2; - double t131 = t12*t74*5.143951577823025E-2; - double t132 = t36*t60*-5.143951577823025E-2; - double t133 = t36*t60*5.143951577823025E-2; - double t134 = t36*t74*-5.143951577823025E-2; - double t135 = t36*t74*5.143951577823025E-2; - double t138 = t60*t73*-1.758834080497775E-1; - double t141 = t73*t74*-1.758834080497775E-1; - double t144 = t60*t73*-1.052664560664766E-1; - double t146 = t73*t74*-1.052664560664766E-1; - double t148 = t12*t36*t60*-3.517668160995551E-1; - double t149 = t60*t101; - double t150 = t12*t36*t74*-3.517668160995551E-1; - double t151 = t74*t101; - double t152 = t60*t105; - double t153 = t74*t105; - double t136 = t60*t119; - double t137 = t74*t119; - double t139 = t60*t85; - double t140 = t60*t87; - double t142 = t74*t85; - double t143 = t74*t87; - double t145 = t60*t88; - double t147 = t74*t88; - - // assume J(NUM_DEPENDENT_JOINTS, modelPtr_->nv) has been allocated outside - J.setZero(); - J(0, 28) = t18*5.699060997402858E-2-t42*1.034589188110661E-3-t18*t44*1.26575449899492E-2+t42*t44*2.246221860400801E-3+t18*t19*t20*3.020283789547073E-3-t18*t20*t43*3.397508858570615E-1-t19*t20*t42*3.399783924207052E-1-t20*t42*t43*3.105990081579729E-3; - J(0, 29) = t18*t19*t20*3.105990081579729E-3-t18*t20*t43*3.399783924207052E-1-t19*t20*t42*3.397508858570615E-1-t20*t42*t43*3.020283789547073E-3; - J(0, 30) = t18*t20*-2.246221860400801E-3-t20*t42*1.26575449899492E-2-t18*t19*t44*3.399783924207052E-1-t18*t43*t44*3.105990081579729E-3-t19*t42*t44*3.020283789547073E-3+t42*t43*t44*3.397508858570615E-1; - J(0, 34) = t81+t83+t24*t25*2.991020934719675E-3+t24*t49*5.605619802270151E-3+t25*t48*9.070578524442013E-3+t48*t49*1.69996184260823E-2; - J(0, 35) = t24*t25*-1.69996184260823E-2+t24*t49*9.070578524442013E-3+t25*t48*5.605619802270151E-3-t48*t49*2.991020934719675E-3; - J(1, 28) = t18*-1.034589188110661E-3-t42*5.699060997402858E-2+t18*t44*2.246221860400801E-3+t42*t44*1.26575449899492E-2-t18*t19*t20*3.399783924207052E-1-t18*t20*t43*3.105990081579729E-3-t19*t20*t42*3.020283789547073E-3+t20*t42*t43*3.397508858570615E-1; - J(1, 29) = t18*t19*t20*-3.397508858570615E-1-t18*t20*t43*3.020283789547073E-3-t19*t20*t42*3.105990081579729E-3+t20*t42*t43*3.399783924207052E-1; - J(1, 30) = t18*t20*-1.26575449899492E-2+t20*t42*2.246221860400801E-3-t18*t19*t44*3.020283789547073E-3+t18*t43*t44*3.397508858570615E-1+t19*t42*t44*3.399783924207052E-1+t42*t43*t44*3.105990081579729E-3; - J(1, 34) = t78+t84-t24*t25*9.070578524442013E-3-t24*t49*1.69996184260823E-2+t25*t48*2.991020934719675E-3+t48*t49*5.605619802270151E-3; - J(1, 35) = t24*t25*-5.605619802270151E-3+t24*t49*2.991020934719675E-3-t25*t48*1.69996184260823E-2+t48*t49*9.070578524442013E-3; - J(2, 29) = t19*t20*-1.263678697118524E-2-t20*t43*2.360206106172353E-3; - J(2, 30) = t20*3.39756885202024E-1-t19*t44*2.360206106172353E-3+t43*t44*1.263678697118524E-2; - J(2, 35) = t49*-1.79E-2+t79; - J(3, 31) = t21*-5.699060997402856E-2-t45*1.034589188110661E-3-t21*t47*1.549180159908108E-2+t45*t47*8.234792310892687E-4-t21*t22*t23*3.104080344633556E-3-t21*t23*t46*2.87566285868891E-1-t22*t23*t45*2.879825211612492E-1+t23*t45*t46*3.064210757541298E-3; - J(3, 32) = t21*t22*t23*-3.064210757541298E-3-t21*t23*t46*2.879825211612492E-1-t22*t23*t45*2.87566285868891E-1+t23*t45*t46*3.104080344633556E-3; - J(3, 33) = t21*t23*-8.234792310892687E-4-t23*t45*1.549180159908108E-2-t21*t22*t47*2.879825211612492E-1+t21*t46*t47*3.064210757541298E-3+t22*t45*t47*3.104080344633556E-3+t45*t46*t47*2.87566285868891E-1; - J(3, 34) = t81+t83+t24*t25*2.991020934719677E-3-t24*t49*5.668252425759201E-3+t25*t48*9.070578524442012E-3-t48*t49*1.718955829676478E-2; - J(3, 35) = t24*t25*1.718955829676478E-2+t24*t49*9.070578524442012E-3-t25*t48*5.668252425759201E-3-t48*t49*2.991020934719677E-3; - J(4, 31) = t21*1.034589188110661E-3-t45*5.699060997402856E-2-t21*t47*8.234792310892687E-4-t45*t47*1.549180159908108E-2+t21*t22*t23*2.879825211612492E-1-t21*t23*t46*3.064210757541298E-3-t22*t23*t45*3.104080344633556E-3-t23*t45*t46*2.87566285868891E-1; - J(4, 32) = t21*t22*t23*2.87566285868891E-1-t21*t23*t46*3.104080344633556E-3-t22*t23*t45*3.064210757541298E-3-t23*t45*t46*2.879825211612492E-1; - J(4, 33) = t21*t23*1.549180159908108E-2-t23*t45*8.234792310892687E-4-t21*t22*t47*3.104080344633556E-3-t21*t46*t47*2.87566285868891E-1-t22*t45*t47*2.879825211612492E-1+t45*t46*t47*3.064210757541298E-3; - J(4, 34) = t78+t84-t24*t25*9.070578524442012E-3+t24*t49*1.718955829676478E-2+t25*t48*2.991020934719677E-3-t48*t49*5.668252425759201E-3; - J(4, 35) = t24*t25*5.668252425759201E-3+t24*t49*2.991020934719677E-3+t25*t48*1.718955829676478E-2+t48*t49*9.070578524442012E-3; - J(5, 32) = t22*t23*1.549973690114125E-2+t23*t46*6.574122182670539E-4; - J(5, 33) = t23*-2.875818595898751E-1+t22*t47*6.574122182670539E-4-t46*t47*1.549973690114125E-2; - J(5, 35) = t49*1.81E-2+t79; - J(6, 13) = t6*t54*-4.104331657223103E-4-t6*t55*2.809111102928818E-2-t6*t68*2.848906577901809E-2-t6*t69*2.809111102928818E-2-t30*t54*7.74045457557333E-4-t30*t55*4.455658896021977E-4+t30*t68*1.8502215904887E-4-t30*t69*4.455658896021977E-4+t53*t54*1.643509328293678E-2+t53*t55*3.732977901401361E-5-t53*t68*1.64724230619508E-2-t54*t67*1.643509328293678E-2+t53*t69*3.732977901401361E-5-t55*t67*3.732977901401361E-5+t67*t68*1.64724230619508E-2-t67*t69*3.732977901401361E-5-t6*t7*t8*3.020283789547073E-3-t6*t7*t31*9.515128902424641E-4-t6*t8*t31*3.397508858570615E-1-t7*t8*t30*3.399783924207052E-1-t7*t30*t31*2.809726196867663E-2+t8*t30*t31*3.105990081579729E-3-t6*t30*t54*5.952939355245877E-2-t6*t30*t55*6.247020354403988E-5-t7*t31*t53*5.956062305521773E-2+t6*t32*t54*1.26575449899492E-2+t6*t30*t68*5.959186375600283E-2-t6*t30*t69*6.247020354403988E-5+t7*t31*t67*5.956062305521773E-2+t6*t32*t68*1.26575449899492E-2-t6*t54*t55*2.848906577901809E-2+t30*t32*t54*2.246221860400801E-3-t6*t54*t69*2.848906577901809E-2-t6*t55*t68*4.104331657223103E-4+t30*t32*t68*2.246221860400801E-3+t30*t54*t55*1.8502215904887E-4-t6*t68*t69*4.104331657223103E-4+t30*t54*t69*1.8502215904887E-4-t30*t55*t68*7.74045457557333E-4-t53*t54*t55*1.64724230619508E-2-t30*t68*t69*7.74045457557333E-4-t53*t54*t69*1.64724230619508E-2+t53*t55*t68*1.643509328293678E-2+t54*t55*t67*1.64724230619508E-2+t53*t68*t69*1.643509328293678E-2+t54*t67*t69*1.64724230619508E-2-t55*t67*t68*1.643509328293678E-2-t67*t68*t69*1.643509328293678E-2-t6*t7*t30*t31*6.581499972336942E-2+t6*t7*t31*t55*9.515128902424641E-4+t6*t7*t31*t69*9.515128902424641E-4+t7*t30*t31*t55*2.809726196867663E-2+t7*t30*t31*t69*2.809726196867663E-2+t6*t30*t54*t55*5.959186375600283E-2+t7*t31*t53*t55*5.956062305521773E-2+t6*t30*t54*t69*5.959186375600283E-2-t6*t30*t55*t68*5.952939355245877E-2+t7*t31*t53*t69*5.956062305521773E-2-t7*t31*t55*t67*5.956062305521773E-2-t6*t30*t68*t69*5.952939355245877E-2-t7*t31*t67*t69*5.956062305521773E-2+t6*t7*t30*t31*t55*6.581499972336942E-2+t6*t7*t30*t31*t69*6.581499972336942E-2; - J(6, 14) = t54*-1.522231734027378E-5+t68*1.522231734027378E-5-t7*t31*7.865874492559456E-5+t6*t54*2.809726196867663E-2-t6*t68*2.809726196867663E-2-t30*t54*9.515128902424641E-4+t30*t68*9.515128902424641E-4+t53*t54*1.646136108951249E-2+t54*t55*1.522231734027378E-5-t53*t68*1.646136108951249E-2-t54*t67*1.644613877217222E-2+t54*t69*1.522231734027378E-5-t55*t68*1.522231734027378E-5+t67*t68*1.644613877217222E-2-t68*t69*1.522231734027378E-5-t6*t7*t8*3.105990081579729E-3-t6*t7*t31*1.918135233212406E-3-t6*t8*t31*3.399783924207052E-1-t7*t8*t30*3.397508858570615E-1-t7*t30*t31*5.615726522659155E-2+t8*t30*t31*3.020283789547073E-3-t6*t30*t54*5.956062305521773E-2-t7*t31*t53*5.952129928176799E-2+t7*t31*t55*7.865874492559456E-5+t6*t30*t68*5.956062305521773E-2+t7*t31*t67*5.959995802669361E-2+t7*t31*t69*7.865874492559456E-5-t6*t54*t55*2.809726196867663E-2-t6*t54*t69*2.809726196867663E-2+t6*t55*t68*2.809726196867663E-2+t30*t54*t55*9.515128902424641E-4+t6*t68*t69*2.809726196867663E-2+t30*t54*t69*9.515128902424641E-4-t30*t55*t68*9.515128902424641E-4-t53*t54*t55*1.646136108951249E-2-t30*t68*t69*9.515128902424641E-4-t53*t54*t69*1.646136108951249E-2+t53*t55*t68*1.646136108951249E-2+t54*t55*t67*1.644613877217222E-2+t53*t68*t69*1.646136108951249E-2+t54*t67*t69*1.644613877217222E-2-t55*t67*t68*1.644613877217222E-2-t67*t68*t69*1.644613877217222E-2-t6*t7*t30*t31*6.581503268977515E-2+t6*t7*t31*t55*1.918135233212406E-3+t6*t7*t31*t69*1.918135233212406E-3+t7*t30*t31*t55*5.615726522659155E-2+t7*t30*t31*t69*5.615726522659155E-2+t6*t30*t54*t55*5.956062305521773E-2+t7*t31*t53*t55*5.952129928176799E-2+t6*t30*t54*t69*5.956062305521773E-2-t6*t30*t55*t68*5.956062305521773E-2+t7*t31*t53*t69*5.952129928176799E-2-t7*t31*t55*t67*5.959995802669361E-2-t6*t30*t68*t69*5.956062305521773E-2-t7*t31*t67*t69*5.959995802669361E-2+t6*t7*t30*t31*t55*6.581503268977515E-2+t6*t7*t30*t31*t69*6.581503268977515E-2; - J(6, 15) = t6*t7*t32*-3.399783924207052E-1-t6*t8*t54*2.246221860400801E-3+t6*t31*t32*3.105990081579729E-3+t7*t30*t32*3.020283789547073E-3-t6*t8*t68*2.246221860400801E-3+t8*t30*t54*1.26575449899492E-2+t30*t31*t32*3.397508858570615E-1+t8*t30*t68*1.26575449899492E-2; - J(6, 19) = t86+t88+t101+t123+t126+t128+t130+t140+t143+t144+t146+t148+t150-t12*t13*2.991020934719675E-3+t12*t37*5.605619802270151E-3+t13*t36*9.070578524442013E-3-t36*t37*1.69996184260823E-2; - J(6, 20) = t12*t13*1.69996184260823E-2+t12*t37*9.070578524442013E-3+t13*t36*5.605619802270151E-3+t36*t37*2.991020934719675E-3; - J(7, 13) = t6*t54*-7.74045457557333E-4-t6*t55*4.455658896021977E-4+t6*t68*1.8502215904887E-4-t6*t69*4.455658896021977E-4+t30*t54*4.104331657223105E-4+t30*t55*2.809111102928818E-2+t30*t68*2.848906577901809E-2+t30*t69*2.809111102928818E-2-t53*t54*2.976469677622939E-2-t53*t55*3.123510177201994E-5+t53*t68*2.979593187800142E-2+t54*t67*2.976469677622939E-2-t53*t69*3.123510177201994E-5+t55*t67*3.123510177201994E-5-t67*t68*2.979593187800142E-2+t67*t69*3.123510177201994E-5-t6*t7*t8*3.399783924207052E-1-t6*t7*t31*2.809726196867663E-2+t6*t8*t31*3.105990081579729E-3+t7*t8*t30*3.020283789547073E-3+t7*t30*t31*9.51512890242464E-4+t8*t30*t31*3.397508858570615E-1-t6*t30*t54*3.287018656587355E-2-t6*t30*t55*7.465955802802721E-5-t7*t31*t53*3.290749986168471E-2+t6*t32*t54*2.246221860400801E-3+t6*t30*t68*3.294484612390159E-2-t6*t30*t69*7.465955802802721E-5+t7*t31*t67*3.290749986168471E-2+t6*t32*t68*2.246221860400801E-3+t6*t54*t55*1.8502215904887E-4-t30*t32*t54*1.26575449899492E-2+t6*t54*t69*1.8502215904887E-4-t6*t55*t68*7.74045457557333E-4-t30*t32*t68*1.26575449899492E-2+t30*t54*t55*2.848906577901809E-2-t6*t68*t69*7.74045457557333E-4+t30*t54*t69*2.848906577901809E-2+t30*t55*t68*4.104331657223105E-4+t53*t54*t55*2.979593187800142E-2+t30*t68*t69*4.104331657223105E-4+t53*t54*t69*2.979593187800142E-2-t53*t55*t68*2.976469677622939E-2-t54*t55*t67*2.979593187800142E-2-t53*t68*t69*2.976469677622939E-2-t54*t67*t69*2.979593187800142E-2+t55*t67*t68*2.976469677622939E-2+t67*t68*t69*2.976469677622939E-2+t6*t7*t30*t31*1.191212461104355E-1+t6*t7*t31*t55*2.809726196867663E-2+t6*t7*t31*t69*2.809726196867663E-2-t7*t30*t31*t55*9.51512890242464E-4-t7*t30*t31*t69*9.51512890242464E-4+t6*t30*t54*t55*3.294484612390159E-2+t7*t31*t53*t55*3.290749986168471E-2+t6*t30*t54*t69*3.294484612390159E-2-t6*t30*t55*t68*3.287018656587355E-2+t7*t31*t53*t69*3.290749986168471E-2-t7*t31*t55*t67*3.290749986168471E-2-t6*t30*t68*t69*3.287018656587355E-2-t7*t31*t67*t69*3.290749986168471E-2-t6*t7*t30*t31*t55*1.191212461104355E-1-t6*t7*t30*t31*t69*1.191212461104355E-1; - J(7, 14) = t54*8.772182874056074E-6-t68*8.772182874056074E-6+t7*t31*4.532876826220704E-5-t6*t54*9.51512890242464E-4+t6*t68*9.51512890242464E-4-t30*t54*2.809726196867663E-2+t30*t68*2.809726196867663E-2-t53*t54*2.978469761904589E-2-t54*t55*8.772182874056074E-6+t53*t68*2.978469761904589E-2+t54*t67*2.977592543617184E-2-t54*t69*8.772182874056074E-6+t55*t68*8.772182874056074E-6-t67*t68*2.977592543617184E-2+t68*t69*8.772182874056074E-6-t6*t7*t8*3.397508858570615E-1-t6*t7*t31*5.615726522659155E-2+t6*t8*t31*3.020283789547073E-3+t7*t8*t30*3.105990081579729E-3+t7*t30*t31*1.918135233212406E-3+t8*t30*t31*3.399783924207052E-1-t6*t30*t54*3.290749986168471E-2-t7*t31*t53*3.293018072901868E-2-t7*t31*t55*4.532876826220704E-5+t6*t30*t68*3.290749986168471E-2+t7*t31*t67*3.288485196075646E-2-t7*t31*t69*4.532876826220704E-5+t6*t54*t55*9.51512890242464E-4+t6*t54*t69*9.51512890242464E-4-t6*t55*t68*9.51512890242464E-4+t30*t54*t55*2.809726196867663E-2-t6*t68*t69*9.51512890242464E-4+t30*t54*t69*2.809726196867663E-2-t30*t55*t68*2.809726196867663E-2+t53*t54*t55*2.978469761904589E-2-t30*t68*t69*2.809726196867663E-2+t53*t54*t69*2.978469761904589E-2-t53*t55*t68*2.978469761904589E-2-t54*t55*t67*2.977592543617184E-2-t53*t68*t69*2.978469761904589E-2-t54*t67*t69*2.977592543617184E-2+t55*t67*t68*2.977592543617184E-2+t67*t68*t69*2.977592543617184E-2+t6*t7*t30*t31*1.191212573084616E-1+t6*t7*t31*t55*5.615726522659155E-2+t6*t7*t31*t69*5.615726522659155E-2-t7*t30*t31*t55*1.918135233212406E-3-t7*t30*t31*t69*1.918135233212406E-3+t6*t30*t54*t55*3.290749986168471E-2+t7*t31*t53*t55*3.293018072901868E-2+t6*t30*t54*t69*3.290749986168471E-2-t6*t30*t55*t68*3.290749986168471E-2+t7*t31*t53*t69*3.293018072901868E-2-t7*t31*t55*t67*3.288485196075646E-2-t6*t30*t68*t69*3.290749986168471E-2-t7*t31*t67*t69*3.288485196075646E-2-t6*t7*t30*t31*t55*1.191212573084616E-1-t6*t7*t30*t31*t69*1.191212573084616E-1; - J(7, 15) = t6*t7*t32*3.020283789547073E-3+t6*t8*t54*1.26575449899492E-2+t6*t31*t32*3.397508858570615E-1+t7*t30*t32*3.399783924207052E-1+t6*t8*t68*1.26575449899492E-2+t8*t30*t54*2.246221860400801E-3-t30*t31*t32*3.105990081579729E-3+t8*t30*t68*2.246221860400801E-3; - J(7, 19) = t85+t104+t118+t120+t122+t132+t134+t136+t137+t138+t141+t152+t153-t12*t13*9.070578524442013E-3+t12*t37*1.69996184260823E-2-t13*t36*2.991020934719675E-3+t36*t37*5.605619802270151E-3; - J(7, 20) = t12*t13*-5.605619802270151E-3-t12*t37*2.991020934719675E-3+t13*t36*1.69996184260823E-2+t36*t37*9.070578524442013E-3; - J(8, 13) = t6*t54*1.101395785003593E-3-t6*t55*9.852121016062406E-4-t6*t68*1.161836833973452E-4-t6*t69*9.852121016062406E-4+t30*t54*6.213602549397351E-4-t30*t55*8.271781369638137E-4+t30*t68*2.058178820240719E-4-t30*t69*8.271781369638137E-4+t6*t7*t31*4.163488242625716E-4-t7*t30*t31*1.218023269812134E-3+t6*t30*t54*1.040834085586084E-17-t6*t30*t68*1.040834085586084E-17-t6*t54*t55*1.161836833973452E-4-t6*t54*t69*1.161836833973452E-4+t6*t55*t68*1.101395785003593E-3+t30*t54*t55*2.058178820240719E-4+t6*t68*t69*1.101395785003593E-3+t30*t54*t69*2.058178820240719E-4+t30*t55*t68*6.213602549397351E-4+t30*t68*t69*6.213602549397351E-4-t6*t7*t30*t31*6.396792817664476E-18-t6*t7*t31*t55*4.163488242625716E-4-t6*t7*t31*t69*4.163488242625716E-4+t7*t30*t31*t55*1.218023269812134E-3+t7*t30*t31*t69*1.218023269812134E-3-t6*t30*t54*t55*1.040834085586084E-17-t6*t30*t54*t69*1.040834085586084E-17+t6*t30*t55*t68*1.040834085586084E-17+t6*t30*t68*t69*1.040834085586084E-17+t6*t7*t30*t31*t55*6.396792817664476E-18+t6*t7*t30*t31*t69*6.396792817664476E-18; - J(8, 14) = t6*t54*1.218023269812134E-3-t6*t68*1.218023269812134E-3+t30*t54*4.163488242625716E-4-t30*t68*4.163488242625716E-4+t53*t54*2.212067055639549E-4-t53*t68*2.212067055639549E-4+t54*t67*2.212067055639517E-4-t67*t68*2.212067055639517E-4+t6*t7*t31*8.310847458313263E-4+t7*t8*t53*1.263678697118524E-2-t7*t30*t31*2.435158936801877E-3+t7*t8*t67*1.263678697118524E-2+t7*t31*t53*2.079441461225871E-3-t8*t31*t53*2.360206106172353E-3+t7*t31*t67*2.07944146122585E-3-t8*t31*t67*2.360206106172353E-3-t6*t54*t55*1.218023269812134E-3-t6*t54*t69*1.218023269812134E-3+t6*t55*t68*1.218023269812134E-3-t30*t54*t55*4.163488242625716E-4+t6*t68*t69*1.218023269812134E-3-t30*t54*t69*4.163488242625716E-4+t30*t55*t68*4.163488242625716E-4-t53*t54*t55*2.212067055639549E-4+t30*t68*t69*4.163488242625716E-4-t53*t54*t69*2.212067055639549E-4+t53*t55*t68*2.212067055639549E-4-t54*t55*t67*2.212067055639517E-4+t53*t68*t69*2.212067055639549E-4-t54*t67*t69*2.212067055639517E-4+t55*t67*t68*2.212067055639517E-4+t67*t68*t69*2.212067055639517E-4-t6*t7*t31*t55*8.310847458313263E-4-t6*t7*t31*t69*8.310847458313263E-4+t7*t30*t31*t55*2.435158936801877E-3+t7*t30*t31*t69*2.435158936801877E-3-t7*t31*t53*t55*2.079441461225871E-3-t7*t31*t53*t69*2.079441461225871E-3-t7*t31*t55*t67*2.07944146122585E-3-t7*t31*t67*t69*2.07944146122585E-3; - J(8, 15) = t7*t32*t53*-2.360206106172353E-3-t7*t32*t67*2.360206106172353E-3+t8*t53*t54*3.39756885202024E-1-t31*t32*t53*1.263678697118524E-2+t8*t53*t68*3.39756885202024E-1+t8*t54*t67*3.39756885202024E-1-t31*t32*t67*1.263678697118524E-2+t8*t67*t68*3.39756885202024E-1; - J(8, 20) = t121+t125-t37*t59*1.79E-2-t37*t73*1.79E-2; - J(9, 16) = t9*t57*-4.353713116283091E-4+t9*t58*2.893932048270239E-2+t9*t71*2.848666080295449E-2+t9*t72*2.893932048270239E-2-t33*t57*8.255702643738717E-4-t33*t58*4.936925916256938E-4+t33*t71*2.846736678889049E-4-t33*t72*4.936925916256938E-4-t56*t57*1.576769239528801E-2-t56*t58*3.197767103277491E-5+t56*t71*1.579967006632079E-2+t57*t70*1.576769239528801E-2-t56*t72*3.197767103277491E-5+t58*t70*3.197767103277491E-5-t70*t71*1.579967006632079E-2+t70*t72*3.197767103277491E-5+t9*t10*t11*3.104080344633556E-3-t9*t10*t34*1.113095897646181E-3-t9*t11*t34*2.87566285868891E-1-t10*t11*t33*2.879825211612492E-1+t10*t33*t34*2.896401859571867E-2-t11*t33*t34*3.064210757541298E-3-t9*t33*t57*1.11375769716061E-1-t9*t33*t58*3.307263176043031E-4-t10*t34*t56*1.115410112085772E-1+t9*t35*t57*1.549180159908108E-2+t9*t33*t71*1.117064960336653E-1-t9*t33*t72*3.307263176043031E-4+t10*t34*t70*1.115410112085772E-1+t9*t35*t71*1.549180159908108E-2+t9*t57*t58*2.848666080295449E-2+t33*t35*t57*8.234792310892687E-4+t9*t57*t72*2.848666080295449E-2-t9*t58*t71*4.353713116283091E-4+t33*t35*t71*8.234792310892687E-4+t33*t57*t58*2.846736678889049E-4-t9*t71*t72*4.353713116283091E-4+t33*t57*t72*2.846736678889049E-4-t33*t58*t71*8.255702643738717E-4+t56*t57*t58*1.579967006632079E-2-t33*t71*t72*8.255702643738717E-4+t56*t57*t72*1.579967006632079E-2-t56*t58*t71*1.576769239528801E-2-t57*t58*t70*1.579967006632079E-2-t56*t71*t72*1.576769239528801E-2-t57*t70*t72*1.579967006632079E-2+t58*t70*t71*1.576769239528801E-2+t70*t71*t72*1.576769239528801E-2+t9*t10*t33*t34*6.313469897321174E-2+t9*t10*t34*t58*1.113095897646181E-3+t9*t10*t34*t72*1.113095897646181E-3-t10*t33*t34*t58*2.896401859571867E-2-t10*t33*t34*t72*2.896401859571867E-2+t9*t33*t57*t58*1.117064960336653E-1+t10*t34*t56*t58*1.115410112085772E-1+t9*t33*t57*t72*1.117064960336653E-1-t9*t33*t58*t71*1.11375769716061E-1+t10*t34*t56*t72*1.115410112085772E-1-t10*t34*t58*t70*1.115410112085772E-1-t9*t33*t71*t72*1.11375769716061E-1-t10*t34*t70*t72*1.115410112085772E-1-t9*t10*t33*t34*t58*6.313469897321174E-2-t9*t10*t33*t34*t72*6.313469897321174E-2; - J(9, 17) = t57*-1.363641158467861E-5+t71*1.363641158467861E-5-t10*t34*3.209258234829029E-4-t9*t57*2.896401859571867E-2+t9*t71*2.896401859571867E-2-t33*t57*1.113095897646181E-3+t33*t71*1.113095897646181E-3-t56*t57*1.577685653751059E-2+t57*t58*1.363641158467861E-5+t56*t71*1.577685653751059E-2+t57*t70*1.579049294909527E-2+t57*t72*1.363641158467861E-5-t58*t71*1.363641158467861E-5-t70*t71*1.579049294909527E-2-t71*t72*1.363641158467861E-5+t9*t10*t11*3.064210757541298E-3-t9*t10*t34*2.220487864525553E-3-t9*t11*t34*2.879825211612492E-1-t10*t11*t33*2.87566285868891E-1+t10*t33*t34*5.784406422916559E-2-t11*t33*t34*3.104080344633556E-3-t9*t33*t57*1.115410112085772E-1-t10*t34*t56*1.113806699631217E-1+t10*t34*t58*3.209258234829029E-4+t9*t33*t71*1.115410112085772E-1+t10*t34*t70*1.117015957866046E-1+t10*t34*t72*3.209258234829029E-4+t9*t57*t58*2.896401859571867E-2+t9*t57*t72*2.896401859571867E-2-t9*t58*t71*2.896401859571867E-2+t33*t57*t58*1.113095897646181E-3-t9*t71*t72*2.896401859571867E-2+t33*t57*t72*1.113095897646181E-3-t33*t58*t71*1.113095897646181E-3+t56*t57*t58*1.577685653751059E-2-t33*t71*t72*1.113095897646181E-3+t56*t57*t72*1.577685653751059E-2-t56*t58*t71*1.577685653751059E-2-t57*t58*t70*1.579049294909527E-2-t56*t71*t72*1.577685653751059E-2-t57*t70*t72*1.579049294909527E-2+t58*t70*t71*1.579049294909527E-2+t70*t71*t72*1.579049294909527E-2+t9*t10*t33*t34*6.31347249232176E-2+t9*t10*t34*t58*2.220487864525553E-3+t9*t10*t34*t72*2.220487864525553E-3-t10*t33*t34*t58*5.784406422916559E-2-t10*t33*t34*t72*5.784406422916559E-2+t9*t33*t57*t58*1.115410112085772E-1+t10*t34*t56*t58*1.113806699631217E-1+t9*t33*t57*t72*1.115410112085772E-1-t9*t33*t58*t71*1.115410112085772E-1+t10*t34*t56*t72*1.113806699631217E-1-t10*t34*t58*t70*1.117015957866046E-1-t9*t33*t71*t72*1.115410112085772E-1-t10*t34*t70*t72*1.117015957866046E-1-t9*t10*t33*t34*t58*6.31347249232176E-2-t9*t10*t33*t34*t72*6.31347249232176E-2; - J(9, 18) = t9*t10*t35*-2.879825211612492E-1-t9*t11*t57*8.234792310892687E-4-t9*t34*t35*3.064210757541298E-3-t10*t33*t35*3.104080344633556E-3-t9*t11*t71*8.234792310892687E-4+t11*t33*t57*1.549180159908108E-2+t33*t34*t35*2.87566285868891E-1+t11*t33*t71*1.549180159908108E-2; - J(9, 19) = t86+t88+t101+t123+t126+t128+t130+t140+t143+t144+t146+t148+t150-t12*t13*2.991020934719677E-3-t12*t37*5.668252425759201E-3+t13*t36*9.070578524442012E-3+t36*t37*1.718955829676478E-2; - J(9, 20) = t12*t13*-1.718955829676478E-2+t12*t37*9.070578524442012E-3-t13*t36*5.668252425759201E-3+t36*t37*2.991020934719677E-3; - J(10, 16) = t9*t57*8.255702643738717E-4+t9*t58*4.936925916256938E-4-t9*t71*2.846736678889049E-4+t9*t72*4.936925916256938E-4-t33*t57*4.353713116283091E-4+t33*t58*2.893932048270239E-2+t33*t71*2.848666080295449E-2+t33*t72*2.893932048270239E-2+t56*t57*5.568788485803049E-2+t56*t58*1.653631588021515E-4-t56*t71*5.585324801683264E-2-t57*t70*5.568788485803049E-2+t56*t72*1.653631588021515E-4-t58*t70*1.653631588021515E-4+t70*t71*5.585324801683264E-2-t70*t72*1.653631588021515E-4+t9*t10*t11*2.879825211612492E-1-t9*t10*t34*2.896401859571867E-2+t9*t11*t34*3.064210757541298E-3+t10*t11*t33*3.104080344633556E-3-t10*t33*t34*1.113095897646181E-3-t11*t33*t34*2.87566285868891E-1-t9*t33*t57*3.153538479057603E-2-t9*t33*t58*6.395534206554983E-5-t10*t34*t56*3.156734948660587E-2-t9*t35*t57*8.234792310892687E-4+t9*t33*t71*3.159934013264157E-2-t9*t33*t72*6.395534206554983E-5+t10*t34*t70*3.156734948660587E-2-t9*t35*t71*8.234792310892687E-4-t9*t57*t58*2.846736678889049E-4+t33*t35*t57*1.549180159908108E-2-t9*t57*t72*2.846736678889049E-4+t9*t58*t71*8.255702643738717E-4+t33*t35*t71*1.549180159908108E-2+t33*t57*t58*2.848666080295449E-2+t9*t71*t72*8.255702643738717E-4+t33*t57*t72*2.848666080295449E-2-t33*t58*t71*4.353713116283091E-4-t56*t57*t58*5.585324801683264E-2-t33*t71*t72*4.353713116283091E-4-t56*t57*t72*5.585324801683264E-2+t56*t58*t71*5.568788485803049E-2+t57*t58*t70*5.585324801683264E-2+t56*t71*t72*5.568788485803049E-2+t57*t70*t72*5.585324801683264E-2-t58*t70*t71*5.568788485803049E-2-t70*t71*t72*5.568788485803049E-2-t9*t10*t33*t34*2.230820224171545E-1+t9*t10*t34*t58*2.896401859571867E-2+t9*t10*t34*t72*2.896401859571867E-2+t10*t33*t34*t58*1.113095897646181E-3+t10*t33*t34*t72*1.113095897646181E-3+t9*t33*t57*t58*3.159934013264157E-2+t10*t34*t56*t58*3.156734948660587E-2+t9*t33*t57*t72*3.159934013264157E-2-t9*t33*t58*t71*3.153538479057603E-2+t10*t34*t56*t72*3.156734948660587E-2-t10*t34*t58*t70*3.156734948660587E-2-t9*t33*t71*t72*3.153538479057603E-2-t10*t34*t70*t72*3.156734948660587E-2+t9*t10*t33*t34*t58*2.230820224171545E-1+t9*t10*t33*t34*t72*2.230820224171545E-1; - J(10, 17) = t57*4.176918863775431E-6-t71*4.176918863775431E-6+t10*t34*9.830160358935766E-5+t9*t57*1.113095897646181E-3-t9*t71*1.113095897646181E-3-t33*t57*2.896401859571867E-2+t33*t71*2.896401859571867E-2+t56*t57*5.576841714485674E-2-t57*t58*4.176918863775431E-6-t56*t71*5.576841714485674E-2-t57*t70*5.577259406372051E-2-t57*t72*4.176918863775431E-6+t58*t71*4.176918863775431E-6+t70*t71*5.577259406372051E-2+t71*t72*4.176918863775431E-6+t9*t10*t11*2.87566285868891E-1-t9*t10*t34*5.784406422916559E-2+t9*t11*t34*3.104080344633556E-3+t10*t11*t33*3.064210757541298E-3-t10*t33*t34*2.220487864525553E-3-t11*t33*t34*2.879825211612492E-1-t9*t33*t57*3.156734948660587E-2-t10*t34*t56*3.161651326340348E-2-t10*t34*t58*9.830160358935766E-5+t9*t33*t71*3.156734948660587E-2+t10*t34*t70*3.151821165981412E-2-t10*t34*t72*9.830160358935766E-5-t9*t57*t58*1.113095897646181E-3-t9*t57*t72*1.113095897646181E-3+t9*t58*t71*1.113095897646181E-3+t33*t57*t58*2.896401859571867E-2+t9*t71*t72*1.113095897646181E-3+t33*t57*t72*2.896401859571867E-2-t33*t58*t71*2.896401859571867E-2-t56*t57*t58*5.576841714485674E-2-t33*t71*t72*2.896401859571867E-2-t56*t57*t72*5.576841714485674E-2+t56*t58*t71*5.576841714485674E-2+t57*t58*t70*5.577259406372051E-2+t56*t71*t72*5.576841714485674E-2+t57*t70*t72*5.577259406372051E-2-t58*t70*t71*5.577259406372051E-2-t70*t71*t72*5.577259406372051E-2-t9*t10*t33*t34*2.230822657497263E-1+t9*t10*t34*t58*5.784406422916559E-2+t9*t10*t34*t72*5.784406422916559E-2+t10*t33*t34*t58*2.220487864525553E-3+t10*t33*t34*t72*2.220487864525553E-3+t9*t33*t57*t58*3.156734948660587E-2+t10*t34*t56*t58*3.161651326340348E-2+t9*t33*t57*t72*3.156734948660587E-2-t9*t33*t58*t71*3.156734948660587E-2+t10*t34*t56*t72*3.161651326340348E-2-t10*t34*t58*t70*3.151821165981412E-2-t9*t33*t71*t72*3.156734948660587E-2-t10*t34*t70*t72*3.151821165981412E-2+t9*t10*t33*t34*t58*2.230822657497263E-1+t9*t10*t33*t34*t72*2.230822657497263E-1; - J(10, 18) = t9*t10*t35*3.104080344633556E-3-t9*t11*t57*1.549180159908108E-2-t9*t34*t35*2.87566285868891E-1-t10*t33*t35*2.879825211612492E-1-t9*t11*t71*1.549180159908108E-2-t11*t33*t57*8.234792310892687E-4-t33*t34*t35*3.064210757541298E-3-t11*t33*t71*8.234792310892687E-4; - J(10, 19) = t85+t104+t118+t120+t122+t132+t134+t136+t137+t138+t141+t152+t153-t12*t13*9.070578524442012E-3-t12*t37*1.718955829676478E-2-t13*t36*2.991020934719677E-3-t36*t37*5.668252425759201E-3; - J(10, 20) = t12*t13*5.668252425759201E-3-t12*t37*2.991020934719677E-3-t13*t36*1.718955829676478E-2+t36*t37*9.070578524442012E-3; - J(11, 16) = t9*t57*-2.99216464372371E-3+t9*t58*3.029602411709927E-3-t9*t71*3.743776798621798E-5+t9*t72*3.029602411709927E-3+t33*t57*8.817574215578565E-4-t33*t58*7.546580486902988E-4-t33*t71*1.270993728675576E-4-t33*t72*7.546580486902988E-4+t9*t10*t34*1.009577116650697E-3+t10*t33*t34*2.959208971395655E-3-t9*t57*t58*3.743776798621798E-5-t9*t57*t72*3.743776798621798E-5-t9*t58*t71*2.99216464372371E-3-t33*t57*t58*1.270993728675576E-4-t9*t71*t72*2.99216464372371E-3-t33*t57*t72*1.270993728675576E-4+t33*t58*t71*8.817574215578565E-4+t33*t71*t72*8.817574215578565E-4-t9*t10*t34*t58*1.009577116650697E-3-t9*t10*t34*t72*1.009577116650697E-3-t10*t33*t34*t58*2.959208971395655E-3-t10*t33*t34*t72*2.959208971395655E-3; - J(11, 17) = t9*t57*-2.959208971395655E-3+t9*t71*2.959208971395655E-3+t33*t57*1.009577116650697E-3-t33*t71*1.009577116650697E-3+t56*t57*2.297574320158763E-5-t56*t71*2.297574320158763E-5+t57*t70*2.297574320158763E-5-t70*t71*2.297574320158763E-5+t9*t10*t34*2.017713588850828E-3-t10*t11*t56*1.549973690114125E-2+t10*t33*t34*5.909453751474983E-3-t10*t11*t70*1.549973690114125E-2+t10*t34*t56*3.122371117714521E-3+t11*t34*t56*6.574122182670539E-4+t10*t34*t70*3.122371117714521E-3+t11*t34*t70*6.574122182670539E-4+t9*t57*t58*2.959208971395655E-3+t9*t57*t72*2.959208971395655E-3-t9*t58*t71*2.959208971395655E-3-t33*t57*t58*1.009577116650697E-3-t9*t71*t72*2.959208971395655E-3-t33*t57*t72*1.009577116650697E-3+t33*t58*t71*1.009577116650697E-3-t56*t57*t58*2.297574320158763E-5+t33*t71*t72*1.009577116650697E-3-t56*t57*t72*2.297574320158763E-5+t56*t58*t71*2.297574320158763E-5-t57*t58*t70*2.297574320158763E-5+t56*t71*t72*2.297574320158763E-5-t57*t70*t72*2.297574320158763E-5+t58*t70*t71*2.297574320158763E-5+t70*t71*t72*2.297574320158763E-5-t9*t10*t34*t58*2.017713588850828E-3-t9*t10*t34*t72*2.017713588850828E-3-t10*t33*t34*t58*5.909453751474983E-3-t10*t33*t34*t72*5.909453751474983E-3-t10*t34*t56*t58*3.122371117714521E-3-t10*t34*t56*t72*3.122371117714521E-3-t10*t34*t58*t70*3.122371117714521E-3-t10*t34*t70*t72*3.122371117714521E-3; - J(11, 18) = t10*t35*t56*6.574122182670539E-4+t10*t35*t70*6.574122182670539E-4-t11*t56*t57*2.875818595898751E-1+t34*t35*t56*1.549973690114125E-2-t11*t56*t71*2.875818595898751E-1-t11*t57*t70*2.875818595898751E-1+t34*t35*t70*1.549973690114125E-2-t11*t70*t71*2.875818595898751E-1; - J(11, 20) = t121+t125+t37*t59*1.81E-2+t37*t73*1.81E-2; - J(12, 24) = t61*1.586414893789752E-3-t75*1.586414893789752E-3-t14*t15*4.954563135453205E-1+t14*t38*8.359461048286212E-4-t15*t38*6.725214316796237E-2-t61*t62*1.586414893789752E-3-t61*t76*1.586414893789752E-3+t62*t75*1.586414893789752E-3+t75*t76*1.586414893789752E-3-t14*t38*t62*8.359461048286212E-4-t14*t38*t76*8.359461048286212E-4; - J(12, 25) = t14*t39*-6.725214316796237E-2-t15*t61*4.365860704565494E-4+t38*t39*4.954563135453205E-1-t15*t75*4.365860704565494E-4; - J(12, 26) = t16*2.47718E-1+t40*3.370300000000001E-2+t90+t100+t110+t112+t16*t63*2.47718E-1+t16*t77*2.47718E-1+t40*t63*3.370300000000001E-2+t40*t77*3.3703E-2; - J(12, 27) = t90+t100+t110+t112+t16*t17*t41*1.387778780781446E-17-t17*t40*t41*5.551115123125783E-17; - J(13, 24) = t61*-4.170075425381788E-4+t75*4.170075425381788E-4+t14*t15*6.711175107535902E-2+t14*t38*3.164698577274911E-3-t15*t38*4.941905177865888E-1+t61*t62*4.170075425381788E-4+t61*t76*4.170075425381788E-4-t62*t75*4.170075425381788E-4-t75*t76*4.170075425381788E-4-t14*t38*t62*3.164698577274911E-3-t14*t38*t76*3.164698577274911E-3; - J(13, 25) = t14*t39*-4.941905177865888E-1-t15*t61*3.566153386244494E-2-t38*t39*6.711175107535902E-2-t15*t75*3.566153386244494E-2; - J(13, 26) = t16*-3.370300000000001E-2+t40*2.47718E-1+t94+t96+t103+t116-t16*t63*3.370300000000001E-2-t16*t77*3.3703E-2+t40*t63*2.47718E-1+t40*t77*2.47718E-1; - J(13, 27) = t94+t96+t103+t116+t16*t17*t41*5.551115123125783E-17+t17*t40*t41*1.387778780781446E-17; - J(14, 24) = t61*2.84294570084937E-5-t75*2.84294570084937E-5-t14*t15*4.365115769377904E-3-t14*t38*2.270241925564839E-4+t15*t38*3.539606431708408E-2-t61*t62*2.84294570084937E-5-t61*t76*2.84294570084937E-5+t62*t75*2.84294570084937E-5+t75*t76*2.84294570084937E-5+t14*t38*t62*2.270241925564839E-4+t14*t38*t76*2.270241925564839E-4; - J(14, 25) = t14*t39*3.539606431708408E-2-t15*t61*4.987264424463382E-1+t38*t39*4.365115769377904E-3-t15*t75*4.987264424463382E-1; - J(15, 9) = t50*-1.586414893789752E-3+t64*1.586414893789752E-3+t2*t3*4.954563135453205E-1+t2*t26*8.359461048286212E-4-t3*t26*6.725214316796237E-2+t50*t51*1.586414893789752E-3+t50*t65*1.586414893789752E-3-t51*t64*1.586414893789752E-3-t64*t65*1.586414893789752E-3-t2*t26*t51*8.359461048286212E-4-t2*t26*t65*8.359461048286212E-4; - J(15, 10) = t2*t27*-6.725214316796237E-2-t3*t50*4.365860704565494E-4-t26*t27*4.954563135453205E-1-t3*t64*4.365860704565494E-4; - J(15, 11) = t4*-2.47718E-1+t28*3.370300000000001E-2+t89+t98+t106+t108-t4*t52*2.47718E-1-t4*t66*2.47718E-1+t28*t52*3.370300000000001E-2+t28*t66*3.3703E-2; - J(15, 12) = t89+t98+t106+t108+t4*t5*t29*1.387778780781446E-17+t5*t28*t29*5.551115123125783E-17; - J(16, 9) = t50*-4.170075425381788E-4+t64*4.170075425381788E-4+t2*t3*6.711175107535902E-2-t2*t26*3.164698577274911E-3+t3*t26*4.941905177865888E-1+t50*t51*4.170075425381788E-4+t50*t65*4.170075425381788E-4-t51*t64*4.170075425381788E-4-t64*t65*4.170075425381788E-4+t2*t26*t51*3.164698577274911E-3+t2*t26*t65*3.164698577274911E-3; - J(16, 10) = t2*t27*4.941905177865888E-1+t3*t50*3.566153386244494E-2-t26*t27*6.711175107535902E-2+t3*t64*3.566153386244494E-2; - J(16, 11) = t4*-3.370300000000001E-2-t28*2.47718E-1+t92+t93+t102+t114-t4*t52*3.370300000000001E-2-t4*t66*3.3703E-2-t28*t52*2.47718E-1-t28*t66*2.47718E-1; - J(16, 12) = t92+t93+t102+t114-t4*t5*t29*5.551115123125783E-17+t5*t28*t29*1.387778780781446E-17; - J(17, 9) = t50*-2.84294570084937E-5+t64*2.84294570084937E-5+t2*t3*4.365115769377904E-3-t2*t26*2.270241925564839E-4+t3*t26*3.539606431708408E-2+t50*t51*2.84294570084937E-5+t50*t65*2.84294570084937E-5-t51*t64*2.84294570084937E-5-t64*t65*2.84294570084937E-5+t2*t26*t51*2.270241925564839E-4+t2*t26*t65*2.270241925564839E-4; - J(17, 10) = t2*t27*3.539606431708408E-2-t3*t50*4.987264424463382E-1-t26*t27*4.365115769377904E-3-t3*t64*4.987264424463382E-1; - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 1); - - J.block(18, 0, 3, modelPtr_->nv) = fkPtr_->getTranslationJacobian(); - J.block(21, 0, 3, modelPtr_->nv) = fkPtr_->getRPYJacobian(); -} - -void DigitDynamicsConstraints::get_Jx_partial_dq(const VecX& q, const VecX& x) { - assert(x.size() == modelPtr_->nv); - assert(Jx_partial_dq.rows() == NUM_DEPENDENT_JOINTS); - assert(Jx_partial_dq.cols() == modelPtr_->nv); - - double x1 = x(0); - double x2 = x(1); - double x3 = x(2); - double x4 = x(3); - double x5 = x(4); - double x6 = x(5); - double x7 = x(6); - double x8 = x(7); - double x9 = x(8); - double x10 = x(9); - double x11 = x(10); - double x12 = x(11); - double x13 = x(12); - double x14 = x(13); - double x15 = x(14); - double x16 = x(15); - double x17 = x(16); - double x18 = x(17); - double x19 = x(18); - double x20 = x(19); - double x21 = x(20); - double x22 = x(21); - double x23 = x(22); - double x24 = x(23); - double x25 = x(24); - double x26 = x(25); - double x27 = x(26); - double x28 = x(27); - double x29 = x(28); - double x30 = x(29); - double x31 = x(30); - double x32 = x(31); - double x33 = x(32); - double x34 = x(33); - double x35 = x(34); - double x36 = x(35); - - double t2 = cos(q(9)); - double t3 = cos(q(10)); - double t4 = cos(q(11)); - double t5 = cos(q(12)); - double t6 = cos(q(13)); - double t7 = cos(q(14)); - double t8 = cos(q(15)); - double t9 = cos(q(16)); - double t10 = cos(q(17)); - double t11 = cos(q(18)); - double t12 = cos(q(19)); - double t13 = cos(q(20)); - double t14 = cos(q(24)); - double t15 = cos(q(25)); - double t16 = cos(q(26)); - double t17 = cos(q(27)); - double t18 = cos(q(28)); - double t19 = cos(q(29)); - double t20 = cos(q(30)); - double t21 = cos(q(31)); - double t22 = cos(q(32)); - double t23 = cos(q(33)); - double t24 = cos(q(34)); - double t25 = cos(q(35)); - double t26 = sin(q(9)); - double t27 = sin(q(10)); - double t28 = sin(q(11)); - double t29 = sin(q(12)); - double t30 = sin(q(13)); - double t31 = sin(q(14)); - double t32 = sin(q(15)); - double t33 = sin(q(16)); - double t34 = sin(q(17)); - double t35 = sin(q(18)); - double t36 = sin(q(19)); - double t37 = sin(q(20)); - double t38 = sin(q(24)); - double t39 = sin(q(25)); - double t40 = sin(q(26)); - double t41 = sin(q(27)); - double t42 = sin(q(28)); - double t43 = sin(q(29)); - double t44 = sin(q(30)); - double t45 = sin(q(31)); - double t46 = sin(q(32)); - double t47 = sin(q(33)); - double t48 = sin(q(34)); - double t49 = sin(q(35)); - double t50 = t12*1.696216709330505E-2; - double t51 = t24*1.696216709330505E-2; - double t52 = t36*1.696216709330505E-2; - double t53 = t48*-1.696216709330505E-2; - double t54 = t48*1.696216709330505E-2; - double t55 = t37*9.551E-3; - double t56 = t49*9.551E-3; - double t57 = t12*5.143951577823025E-2; - double t58 = t24*5.143951577823025E-2; - double t59 = t36*5.143951577823025E-2; - double t60 = t48*5.143951577823025E-2; - double t61 = t4*t5*-6.370345907739961E-5; - double t62 = t4*t5*6.370345907739961E-5; - double t63 = t16*t17*6.370345907739961E-5; - double t64 = t2*t3*6.725214316796237E-2; - double t65 = t14*t15*6.725214316796237E-2; - double t66 = t12*t13*1.69996184260823E-2; - double t67 = t24*t25*1.69996184260823E-2; - double t68 = t12*t13*1.718955829676478E-2; - double t69 = t24*t25*1.718955829676478E-2; - double t70 = t2*t3*3.539606431708408E-2; - double t71 = t14*t15*3.539606431708408E-2; - double t72 = t6*t8*-2.246221860400801E-3; - double t73 = t6*t8*2.246221860400801E-3; - double t74 = t18*t20*2.246221860400801E-3; - double t75 = t12*t13*9.070578524442012E-3; - double t76 = t12*t13*9.070578524442013E-3; - double t77 = t24*t25*9.070578524442012E-3; - double t78 = t24*t25*9.070578524442013E-3; - double t79 = t7*t8*2.360206106172353E-3; - double t80 = t19*t20*2.360206106172353E-3; - double t81 = t4*t29*6.370345907739961E-5; - double t82 = t5*t28*6.370345907739961E-5; - double t83 = t16*t41*6.370345907739961E-5; - double t84 = t17*t40*6.370345907739961E-5; - double t85 = t2*t27*6.711175107535902E-2; - double t86 = t3*t26*6.711175107535902E-2; - double t87 = t14*t39*6.711175107535902E-2; - double t88 = t15*t38*6.711175107535902E-2; - double t89 = t12*t37*1.69996184260823E-2; - double t90 = t13*t36*-1.69996184260823E-2; - double t91 = t13*t36*1.69996184260823E-2; - double t92 = t24*t49*1.69996184260823E-2; - double t93 = t25*t48*1.69996184260823E-2; - double t94 = t10*t11*6.574122182670539E-4; - double t95 = t22*t23*6.574122182670539E-4; - double t96 = t12*t37*1.718955829676478E-2; - double t97 = t13*t36*-1.718955829676478E-2; - double t98 = t13*t36*1.718955829676478E-2; - double t99 = t24*t49*1.718955829676478E-2; - double t100 = t25*t48*1.718955829676478E-2; - double t101 = t2*t27*4.365115769377904E-3; - double t102 = t3*t26*-4.365115769377904E-3; - double t103 = t3*t26*4.365115769377904E-3; - double t104 = t14*t39*4.365115769377904E-3; - double t105 = t15*t38*4.365115769377904E-3; - double t106 = t6*t32*2.246221860400801E-3; - double t107 = t8*t30*2.246221860400801E-3; - double t108 = t18*t44*2.246221860400801E-3; - double t109 = t20*t42*2.246221860400801E-3; - double t110 = t12*t37*-9.070578524442012E-3; - double t111 = t12*t37*9.070578524442012E-3; - double t112 = t12*t37*9.070578524442013E-3; - double t113 = t13*t36*-9.070578524442013E-3; - double t114 = t13*t36*-9.070578524442012E-3; - double t115 = t13*t36*9.070578524442012E-3; - double t116 = t13*t36*9.070578524442013E-3; - double t117 = t24*t49*-9.070578524442013E-3; - double t118 = t24*t49*9.070578524442012E-3; - double t119 = t24*t49*9.070578524442013E-3; - double t120 = t25*t48*9.070578524442012E-3; - double t121 = t25*t48*9.070578524442013E-3; - double t122 = t12*t13*5.605619802270151E-3; - double t123 = t24*t25*5.605619802270151E-3; - double t124 = t12*t13*5.668252425759201E-3; - double t125 = t24*t25*5.668252425759201E-3; - double t126 = t12*t13*2.991020934719675E-3; - double t127 = t12*t13*2.991020934719677E-3; - double t128 = t24*t25*2.991020934719675E-3; - double t129 = t24*t25*2.991020934719677E-3; - double t130 = t28*t29*6.370345907739961E-5; - double t131 = t40*t41*-6.370345907739961E-5; - double t132 = t40*t41*6.370345907739961E-5; - double t133 = t26*t27*-6.725214316796237E-2; - double t134 = t26*t27*6.725214316796237E-2; - double t135 = t38*t39*6.725214316796237E-2; - double t136 = t36*t37*1.69996184260823E-2; - double t137 = t48*t49*1.69996184260823E-2; - double t138 = t36*t37*-1.718955829676478E-2; - double t139 = t36*t37*1.718955829676478E-2; - double t140 = t48*t49*-1.718955829676478E-2; - double t141 = t48*t49*1.718955829676478E-2; - double t142 = t6*t8*1.26575449899492E-2; - double t143 = t18*t20*-1.26575449899492E-2; - double t144 = t18*t20*1.26575449899492E-2; - double t145 = t26*t27*3.539606431708408E-2; - double t146 = t38*t39*-3.539606431708408E-2; - double t147 = t38*t39*3.539606431708408E-2; - double t148 = t30*t32*2.246221860400801E-3; - double t149 = t42*t44*-2.246221860400801E-3; - double t150 = t42*t44*2.246221860400801E-3; - double t151 = t36*t37*-9.070578524442013E-3; - double t152 = t36*t37*9.070578524442012E-3; - double t153 = t36*t37*9.070578524442013E-3; - double t154 = t48*t49*-9.070578524442013E-3; - double t155 = t48*t49*9.070578524442012E-3; - double t156 = t48*t49*9.070578524442013E-3; - double t157 = t12*t37*-5.605619802270151E-3; - double t158 = t12*t37*5.605619802270151E-3; - double t159 = t13*t36*5.605619802270151E-3; - double t160 = t24*t49*5.605619802270151E-3; - double t161 = t25*t48*-5.605619802270151E-3; - double t162 = t25*t48*5.605619802270151E-3; - double t163 = t9*t11*8.234792310892687E-4; - double t164 = t21*t23*8.234792310892687E-4; - double t165 = t12*t37*5.668252425759201E-3; - double t166 = t13*t36*5.668252425759201E-3; - double t167 = t24*t49*-5.668252425759201E-3; - double t168 = t24*t49*5.668252425759201E-3; - double t169 = t25*t48*-5.668252425759201E-3; - double t170 = t25*t48*5.668252425759201E-3; - double t171 = t31*t32*-2.360206106172353E-3; - double t172 = t31*t32*2.360206106172353E-3; - double t173 = t43*t44*2.360206106172353E-3; - double t174 = t12*t37*-2.991020934719677E-3; - double t175 = t12*t37*2.991020934719675E-3; - double t176 = t12*t37*2.991020934719677E-3; - double t177 = t13*t36*2.991020934719675E-3; - double t178 = t13*t36*2.991020934719677E-3; - double t179 = t24*t49*-2.991020934719675E-3; - double t180 = t24*t49*2.991020934719675E-3; - double t181 = t24*t49*2.991020934719677E-3; - double t182 = t25*t48*2.991020934719675E-3; - double t183 = t25*t48*2.991020934719677E-3; - double t184 = t34*t35*-6.574122182670539E-4; - double t185 = t34*t35*6.574122182670539E-4; - double t186 = t46*t47*6.574122182670539E-4; - double t187 = t7*t32*1.263678697118524E-2; - double t188 = t8*t31*1.263678697118524E-2; - double t189 = t19*t44*1.263678697118524E-2; - double t190 = t20*t43*-1.263678697118524E-2; - double t191 = t20*t43*1.263678697118524E-2; - double t192 = t6*t32*1.26575449899492E-2; - double t193 = t8*t30*1.26575449899492E-2; - double t194 = t18*t44*1.26575449899492E-2; - double t195 = t20*t42*1.26575449899492E-2; - double t196 = t4*t5*1.20005624647208E-1; - double t197 = t16*t17*-1.20005624647208E-1; - double t198 = t16*t17*1.20005624647208E-1; - double t199 = t36*t37*5.605619802270151E-3; - double t200 = t48*t49*5.605619802270151E-3; - double t201 = t9*t35*-8.234792310892687E-4; - double t202 = t9*t35*8.234792310892687E-4; - double t203 = t11*t33*8.234792310892687E-4; - double t204 = t21*t47*8.234792310892687E-4; - double t205 = t23*t45*8.234792310892687E-4; - double t206 = t36*t37*5.668252425759201E-3; - double t207 = t48*t49*5.668252425759201E-3; - double t208 = t2*t3*4.941905177865888E-1; - double t209 = t14*t15*4.941905177865888E-1; - double t210 = t9*t11*1.549180159908108E-2; - double t211 = t21*t23*-1.549180159908108E-2; - double t212 = t21*t23*1.549180159908108E-2; - double t213 = t36*t37*-2.991020934719677E-3; - double t214 = t36*t37*2.991020934719675E-3; - double t215 = t36*t37*2.991020934719677E-3; - double t216 = t48*t49*-2.991020934719677E-3; - double t217 = t48*t49*2.991020934719675E-3; - double t218 = t48*t49*2.991020934719677E-3; - double t219 = t30*t32*-1.26575449899492E-2; - double t220 = t30*t32*1.26575449899492E-2; - double t221 = t42*t44*1.26575449899492E-2; - double t222 = t4*t29*1.20005624647208E-1; - double t223 = t5*t28*1.20005624647208E-1; - double t224 = t16*t41*1.20005624647208E-1; - double t225 = t17*t40*1.20005624647208E-1; - double t226 = t33*t35*8.234792310892687E-4; - double t227 = t45*t47*-8.234792310892687E-4; - double t228 = t45*t47*8.234792310892687E-4; - double t229 = t2*t27*4.954563135453205E-1; - double t230 = t3*t26*4.954563135453205E-1; - double t231 = t14*t39*4.954563135453205E-1; - double t232 = t15*t38*4.954563135453205E-1; - double t233 = t9*t35*1.549180159908108E-2; - double t234 = t11*t33*-1.549180159908108E-2; - double t235 = t11*t33*1.549180159908108E-2; - double t236 = t21*t47*1.549180159908108E-2; - double t237 = t23*t45*1.549180159908108E-2; - double t238 = t10*t35*1.549973690114125E-2; - double t239 = t11*t34*1.549973690114125E-2; - double t240 = t22*t47*1.549973690114125E-2; - double t241 = t23*t46*-1.549973690114125E-2; - double t242 = t23*t46*1.549973690114125E-2; - double t243 = t28*t29*-1.20005624647208E-1; - double t244 = t28*t29*1.20005624647208E-1; - double t245 = t40*t41*1.20005624647208E-1; - double t246 = t26*t27*4.941905177865888E-1; - double t247 = t38*t39*-4.941905177865888E-1; - double t248 = t38*t39*4.941905177865888E-1; - double t249 = t33*t35*1.549180159908108E-2; - double t250 = t45*t47*1.549180159908108E-2; - double t251 = t9*t10*t11*2.87566285868891E-1; - double t252 = t21*t22*t23*-2.87566285868891E-1; - double t253 = t21*t22*t23*2.87566285868891E-1; - double t254 = t9*t10*t11*2.879825211612492E-1; - double t255 = t21*t22*t23*-2.879825211612492E-1; - double t256 = t21*t22*t23*2.879825211612492E-1; - double t257 = t6*t7*t8*-3.397508858570615E-1; - double t258 = t6*t7*t8*3.397508858570615E-1; - double t259 = t18*t19*t20*3.397508858570615E-1; - double t260 = t6*t7*t8*-3.399783924207052E-1; - double t261 = t6*t7*t8*3.399783924207052E-1; - double t262 = t18*t19*t20*3.399783924207052E-1; - double t263 = t9*t10*t35*2.87566285868891E-1; - double t264 = t9*t11*t34*-2.87566285868891E-1; - double t265 = t9*t11*t34*2.87566285868891E-1; - double t266 = t10*t11*t33*2.87566285868891E-1; - double t267 = t21*t22*t47*-2.87566285868891E-1; - double t268 = t21*t22*t47*2.87566285868891E-1; - double t269 = t21*t23*t46*2.87566285868891E-1; - double t270 = t22*t23*t45*2.87566285868891E-1; - double t271 = t9*t10*t35*2.879825211612492E-1; - double t272 = t9*t11*t34*2.879825211612492E-1; - double t273 = t10*t11*t33*-2.879825211612492E-1; - double t274 = t10*t11*t33*2.879825211612492E-1; - double t275 = t21*t22*t47*2.879825211612492E-1; - double t276 = t21*t23*t46*2.879825211612492E-1; - double t277 = t22*t23*t45*2.879825211612492E-1; - double t278 = t6*t7*t8*3.020283789547073E-3; - double t279 = t18*t19*t20*-3.020283789547073E-3; - double t280 = t18*t19*t20*3.020283789547073E-3; - double t281 = t9*t10*t11*-3.064210757541298E-3; - double t282 = t9*t10*t11*3.064210757541298E-3; - double t283 = t21*t22*t23*3.064210757541298E-3; - double t284 = t9*t10*t11*3.104080344633556E-3; - double t285 = t21*t22*t23*3.104080344633556E-3; - double t286 = t6*t7*t8*3.105990081579729E-3; - double t287 = t18*t19*t20*-3.105990081579729E-3; - double t288 = t18*t19*t20*3.105990081579729E-3; - double t289 = t6*t7*t32*-3.397508858570615E-1; - double t290 = t6*t7*t32*3.397508858570615E-1; - double t291 = t6*t8*t31*3.397508858570615E-1; - double t292 = t7*t8*t30*3.397508858570615E-1; - double t293 = t18*t19*t44*3.397508858570615E-1; - double t294 = t18*t20*t43*3.397508858570615E-1; - double t295 = t19*t20*t42*3.397508858570615E-1; - double t296 = t6*t7*t32*-3.399783924207052E-1; - double t297 = t6*t7*t32*3.399783924207052E-1; - double t298 = t6*t8*t31*3.399783924207052E-1; - double t299 = t7*t8*t30*3.399783924207052E-1; - double t300 = t18*t19*t44*3.399783924207052E-1; - double t301 = t18*t20*t43*3.399783924207052E-1; - double t302 = t19*t20*t42*3.399783924207052E-1; - double t303 = t9*t34*t35*2.87566285868891E-1; - double t304 = t10*t33*t35*2.87566285868891E-1; - double t305 = t11*t33*t34*-2.87566285868891E-1; - double t306 = t11*t33*t34*2.87566285868891E-1; - double t307 = t21*t46*t47*2.87566285868891E-1; - double t308 = t22*t45*t47*2.87566285868891E-1; - double t309 = t23*t45*t46*2.87566285868891E-1; - double t310 = t9*t34*t35*2.879825211612492E-1; - double t311 = t10*t33*t35*2.879825211612492E-1; - double t312 = t11*t33*t34*-2.879825211612492E-1; - double t313 = t11*t33*t34*2.879825211612492E-1; - double t314 = t21*t46*t47*2.879825211612492E-1; - double t315 = t22*t45*t47*2.879825211612492E-1; - double t316 = t23*t45*t46*2.879825211612492E-1; - double t317 = t6*t7*t32*3.020283789547073E-3; - double t318 = t6*t8*t31*3.020283789547073E-3; - double t319 = t7*t8*t30*3.020283789547073E-3; - double t320 = t18*t19*t44*-3.020283789547073E-3; - double t321 = t18*t19*t44*3.020283789547073E-3; - double t322 = t18*t20*t43*3.020283789547073E-3; - double t323 = t19*t20*t42*3.020283789547073E-3; - double t324 = t9*t10*t35*-3.064210757541298E-3; - double t325 = t9*t10*t35*3.064210757541298E-3; - double t326 = t9*t11*t34*3.064210757541298E-3; - double t327 = t10*t11*t33*3.064210757541298E-3; - double t328 = t21*t22*t47*3.064210757541298E-3; - double t329 = t21*t23*t46*3.064210757541298E-3; - double t330 = t22*t23*t45*3.064210757541298E-3; - double t331 = t9*t10*t35*-3.104080344633556E-3; - double t332 = t9*t10*t35*3.104080344633556E-3; - double t333 = t9*t11*t34*3.104080344633556E-3; - double t334 = t10*t11*t33*3.104080344633556E-3; - double t335 = t21*t22*t47*3.104080344633556E-3; - double t336 = t21*t23*t46*3.104080344633556E-3; - double t337 = t22*t23*t45*3.104080344633556E-3; - double t338 = t6*t7*t32*3.105990081579729E-3; - double t339 = t6*t8*t31*3.105990081579729E-3; - double t340 = t7*t8*t30*3.105990081579729E-3; - double t341 = t18*t19*t44*-3.105990081579729E-3; - double t342 = t18*t19*t44*3.105990081579729E-3; - double t343 = t18*t20*t43*3.105990081579729E-3; - double t344 = t19*t20*t42*3.105990081579729E-3; - double t345 = t6*t31*t32*3.397508858570615E-1; - double t346 = t7*t30*t32*3.397508858570615E-1; - double t347 = t8*t30*t31*3.397508858570615E-1; - double t348 = t18*t43*t44*3.397508858570615E-1; - double t349 = t19*t42*t44*3.397508858570615E-1; - double t350 = t20*t42*t43*-3.397508858570615E-1; - double t351 = t20*t42*t43*3.397508858570615E-1; - double t352 = t6*t31*t32*3.399783924207052E-1; - double t353 = t7*t30*t32*3.399783924207052E-1; - double t354 = t8*t30*t31*3.399783924207052E-1; - double t355 = t18*t43*t44*3.399783924207052E-1; - double t356 = t19*t42*t44*3.399783924207052E-1; - double t357 = t20*t42*t43*-3.399783924207052E-1; - double t358 = t20*t42*t43*3.399783924207052E-1; - double t359 = t33*t34*t35*-2.87566285868891E-1; - double t360 = t33*t34*t35*2.87566285868891E-1; - double t361 = t45*t46*t47*-2.87566285868891E-1; - double t362 = t45*t46*t47*2.87566285868891E-1; - double t363 = t33*t34*t35*-2.879825211612492E-1; - double t364 = t33*t34*t35*2.879825211612492E-1; - double t365 = t45*t46*t47*2.879825211612492E-1; - double t366 = t6*t31*t32*3.020283789547073E-3; - double t367 = t7*t30*t32*3.020283789547073E-3; - double t368 = t8*t30*t31*-3.020283789547073E-3; - double t369 = t8*t30*t31*3.020283789547073E-3; - double t370 = t18*t43*t44*3.020283789547073E-3; - double t371 = t19*t42*t44*3.020283789547073E-3; - double t372 = t20*t42*t43*3.020283789547073E-3; - double t373 = t9*t34*t35*3.064210757541298E-3; - double t374 = t10*t33*t35*3.064210757541298E-3; - double t375 = t11*t33*t34*-3.064210757541298E-3; - double t376 = t11*t33*t34*3.064210757541298E-3; - double t377 = t21*t46*t47*-3.064210757541298E-3; - double t378 = t21*t46*t47*3.064210757541298E-3; - double t379 = t22*t45*t47*3.064210757541298E-3; - double t380 = t23*t45*t46*-3.064210757541298E-3; - double t381 = t23*t45*t46*3.064210757541298E-3; - double t382 = t9*t34*t35*3.104080344633556E-3; - double t383 = t10*t33*t35*3.104080344633556E-3; - double t384 = t11*t33*t34*3.104080344633556E-3; - double t385 = t21*t46*t47*3.104080344633556E-3; - double t386 = t22*t45*t47*-3.104080344633556E-3; - double t387 = t22*t45*t47*3.104080344633556E-3; - double t388 = t23*t45*t46*-3.104080344633556E-3; - double t389 = t23*t45*t46*3.104080344633556E-3; - double t390 = t6*t31*t32*3.105990081579729E-3; - double t391 = t7*t30*t32*3.105990081579729E-3; - double t392 = t8*t30*t31*-3.105990081579729E-3; - double t393 = t8*t30*t31*3.105990081579729E-3; - double t394 = t18*t43*t44*3.105990081579729E-3; - double t395 = t19*t42*t44*3.105990081579729E-3; - double t396 = t20*t42*t43*3.105990081579729E-3; - double t397 = t30*t31*t32*3.397508858570615E-1; - double t398 = t42*t43*t44*-3.397508858570615E-1; - double t399 = t42*t43*t44*3.397508858570615E-1; - double t400 = t30*t31*t32*3.399783924207052E-1; - double t401 = t42*t43*t44*-3.399783924207052E-1; - double t402 = t42*t43*t44*3.399783924207052E-1; - double t403 = t30*t31*t32*-3.020283789547073E-3; - double t404 = t30*t31*t32*3.020283789547073E-3; - double t405 = t42*t43*t44*3.020283789547073E-3; - double t406 = t33*t34*t35*3.064210757541298E-3; - double t407 = t45*t46*t47*-3.064210757541298E-3; - double t408 = t45*t46*t47*3.064210757541298E-3; - double t409 = t33*t34*t35*3.104080344633556E-3; - double t410 = t45*t46*t47*-3.104080344633556E-3; - double t411 = t45*t46*t47*3.104080344633556E-3; - double t412 = t30*t31*t32*-3.105990081579729E-3; - double t413 = t30*t31*t32*3.105990081579729E-3; - double t414 = t42*t43*t44*3.105990081579729E-3; - double t415 = t101+t145; - double t416 = t104+t146; - double t417 = t171+t187; - double t418 = t173+t189; - double t419 = t85+t246; - double t420 = t87+t247; - double t421 = t133+t229; - double t422 = t135+t231; - double t423 = t184+t238; - double t424 = t186+t240; - double t425 = t66+t112+t159+t214; - double t426 = t90+t122+t151+t175; - double t427 = t67+t117+t161+t217; - double t428 = t93+t123+t154+t179; - double t429 = t68+t110+t166+t213; - double t430 = t97+t124+t152+t174; - double t431 = t69+t118+t169+t216; - double t432 = t100+t125+t155+t181; - double t433 = t61+t130+t222+t223; - double t434 = t81+t82+t196+t243; - double t435 = t63+t131+t224+t225; - double t436 = t83+t84+t197+t245; - double t444 = t263+t363+t374+t382; - double t445 = t304+t310+t324+t409; - double t446 = t267+t365+t379+t385; - double t447 = t308+t314+t328+t410; - double t448 = t289+t366+t391+t400; - double t449 = t338+t346+t352+t403; - double t450 = t293+t370+t395+t401; - double t451 = t341+t349+t355+t405; - double t452 = t266+t272+t281+t384; - double t453 = t251+t312+t327+t333; - double t454 = t270+t276+t283+t388; - double t455 = t252+t316+t330+t336; - double t456 = t286+t292+t298+t368; - double t457 = t257+t318+t340+t354; - double t458 = t287+t295+t301+t372; - double t459 = t259+t322+t344+t357; - double t460 = t72+t193+t296+t367+t390+t397; - double t461 = t107+t142+t317+t345+t353+t412; - double t462 = t74+t195+t300+t371+t394+t398; - double t463 = t109+t143+t320+t348+t356+t414; - double t464 = t163+t234+t271+t359+t373+t383; - double t465 = t203+t210+t303+t311+t331+t406; - double t466 = t164+t237+t275+t361+t377+t386; - double t467 = t205+t211+t307+t315+t335+t407; - double t437 = t433*x13; - double t438 = t434*x13; - double t439 = t435*x28; - double t440 = t436*x28; - double t441 = -t437; - double t442 = -t438; - double t443 = -t439; - - Jx_partial_dq.setZero(); - Jx_partial_dq(0, 28) = -x29*(t18*1.034589188110661E-3+t42*5.699060997402858E-2-t108-t221+t262+t323+t343+t350)-t459*x30+t463*x31; - Jx_partial_dq(0, 29) = -x30*(t262+t323+t343+t350)+t451*x31-t459*x29; - Jx_partial_dq(0, 30) = x31*(t108+t221-t262-t323-t343+t351)+t451*x30+t463*x29; - Jx_partial_dq(0, 34) = t428*x36-x35*(t51+t60-t78-t92+t182+t200); - Jx_partial_dq(0, 35) = t428*x35+x36*(t78+t92-t182-t200); - Jx_partial_dq(1, 28) = t458*x30+t462*x31+x29*(t18*-5.699060997402858E-2+t42*1.034589188110661E-3+t149+t194+t279+t294+t302+t396); - Jx_partial_dq(1, 29) = x30*(t279+t294+t302+t396)+t450*x31+t458*x29; - Jx_partial_dq(1, 30) = t450*x30+t462*x29+x31*(t149+t194+t279+t294+t302+t396); - Jx_partial_dq(1, 34) = -t427*x36+x35*(t53+t58+t121+t128+t137+t160); - Jx_partial_dq(1, 35) = x36*(t121+t128+t137+t160)-t427*x35; - Jx_partial_dq(2, 29) = -x30*(t80+t190)+t418*x31; - Jx_partial_dq(2, 30) = -x31*(t44*3.39756885202024E-1+t80+t190)+t418*x30; - Jx_partial_dq(2, 35) = -x36*(t25*1.79E-2-t56); - Jx_partial_dq(3, 31) = t455*x33+t467*x34+x32*(t21*-1.034589188110661E-3+t45*5.699060997402856E-2+t204+t250+t255+t309+t329+t337); - Jx_partial_dq(3, 32) = x33*(t255+t309+t329+t337)+t447*x34+t455*x32; - Jx_partial_dq(3, 33) = t447*x33+t467*x32+x34*(t204+t250+t255+t309+t329+t337); - Jx_partial_dq(3, 34) = -t432*x36-x35*(t51+t60-t77+t99+t183-t207); - Jx_partial_dq(3, 35) = -t432*x35+x36*(t77-t99-t183+t207); - Jx_partial_dq(4, 31) = -t454*x33-t466*x34-x32*(t21*5.699060997402856E-2+t45*1.034589188110661E-3+t227+t236+t269+t277+t285+t380); - Jx_partial_dq(4, 32) = -x33*(t269+t277+t285+t380)+t446*x34-t454*x32; - Jx_partial_dq(4, 33) = t446*x33-t466*x32-x34*(t227+t236+t269+t277+t285+t380); - Jx_partial_dq(4, 34) = t431*x36+x35*(t53+t58+t120+t129+t140+t167); - Jx_partial_dq(4, 35) = x36*(t120+t129+t140+t167)+t431*x35; - Jx_partial_dq(5, 32) = x33*(t95+t241)-t424*x34; - Jx_partial_dq(5, 33) = x34*(t47*2.875818595898751E-1+t95+t241)-t424*x33; - Jx_partial_dq(5, 35) = x36*(t25*1.81E-2+t56); - Jx_partial_dq(6, 13) = t457*x15+t461*x16+x14*(t6*-1.034589188110661E-3+t30*5.699060997402858E-2+t106+t219+t260+t319+t339+t347); - Jx_partial_dq(6, 14) = x15*(t260+t319+t339+t347)+t449*x16+t457*x14; - Jx_partial_dq(6, 15) = t449*x15+t461*x14+x16*(t106+t219+t260+t319+t339+t347); - Jx_partial_dq(6, 19) = -x20*(t50-t59-t76+t89-t177+t199)+t426*x21; - Jx_partial_dq(6, 20) = t426*x20+x21*(t76-t89+t177-t199); - Jx_partial_dq(7, 13) = x14*(t6*5.699060997402858E-2+t30*1.034589188110661E-3-t148-t192+t278+t291+t299+t392)+t456*x15-t460*x16; - Jx_partial_dq(7, 14) = x15*(t278+t291+t299+t392)-t448*x16+t456*x14; - Jx_partial_dq(7, 15) = -x16*(t148+t192-t278-t291-t299+t393)-t448*x15-t460*x14; - Jx_partial_dq(7, 19) = t425*x21-x20*(t52+t57+t113+t126+t136+t157); - Jx_partial_dq(7, 20) = -x21*(t113+t126+t136+t157)+t425*x20; - Jx_partial_dq(8, 14) = -x15*(t79+t188)-t417*x16; - Jx_partial_dq(8, 15) = -x16*(t32*3.39756885202024E-1+t79+t188)-t417*x15; - Jx_partial_dq(8, 20) = -x21*(t13*1.79E-2+t55); - Jx_partial_dq(9, 16) = -t453*x18+t465*x19-x17*(t9*1.034589188110661E-3+t33*5.699060997402856E-2+t201+t249+t254+t305+t326+t334); - Jx_partial_dq(9, 17) = -x18*(t254+t305+t326+t334)+t445*x19-t453*x17; - Jx_partial_dq(9, 18) = t445*x18+t465*x17-x19*(t201+t249+t254+t305+t326+t334); - Jx_partial_dq(9, 19) = -t430*x21+x20*(-t50+t59+t75+t96+t178+t206); - Jx_partial_dq(9, 20) = x21*(t75+t96+t178+t206)-t430*x20; - Jx_partial_dq(10, 16) = -t452*x18-t464*x19+x17*(t9*5.699060997402856E-2-t33*1.034589188110661E-3+t226+t233+t264+t273+t284+t375); - Jx_partial_dq(10, 17) = -t444*x19-t452*x17-x18*(t265+t274-t284+t376); - Jx_partial_dq(10, 18) = -t444*x18-t464*x17+x19*(t226+t233+t264+t273+t284+t375); - Jx_partial_dq(10, 19) = -t429*x21-x20*(t52+t57+t114+t127+t138+t165); - Jx_partial_dq(10, 20) = -x21*(t114+t127+t138+t165)-t429*x20; - Jx_partial_dq(11, 17) = x18*(t94+t239)+t423*x19; - Jx_partial_dq(11, 18) = x19*(t35*2.875818595898751E-1+t94+t239)+t423*x18; - Jx_partial_dq(11, 20) = x21*(t13*1.81E-2-t55); - Jx_partial_dq(12, 24) = t422*x26-x25*(t65-t232); - Jx_partial_dq(12, 25) = t422*x25+x26*(t39*4.365860704565494E-4-t65+t232); - Jx_partial_dq(12, 26) = t440+x27*(t16*6.740600000000002E-2-t40*4.95436E-1+t436); - Jx_partial_dq(12, 27) = t440+t436*x27; - Jx_partial_dq(13, 24) = -x25*(t88+t209)-t420*x26; - Jx_partial_dq(13, 25) = -x26*(t39*-3.566153386244494E-2+t88+t209)-t420*x25; - Jx_partial_dq(13, 26) = t443+x27*(t16*4.95436E-1+t40*6.740600000000002E-2-t63+t132-t224-t225); - Jx_partial_dq(13, 27) = t443-t435*x27; - Jx_partial_dq(14, 24) = x25*(t71+t105)+t416*x26; - Jx_partial_dq(14, 25) = t416*x25+x26*(t39*4.987264424463382E-1+t71+t105); - Jx_partial_dq(15, 9) = -x10*(t64+t230)-t421*x11; - Jx_partial_dq(15, 10) = -t421*x10-x11*(t27*-4.365860704565494E-4+t64+t230); - Jx_partial_dq(15, 11) = t442+x12*(t4*6.740600000000002E-2+t28*4.95436E-1-t81-t82-t196+t244); - Jx_partial_dq(15, 12) = t442-t434*x12; - Jx_partial_dq(16, 9) = -t419*x11-x10*(t86-t208); - Jx_partial_dq(16, 10) = -t419*x10-x11*(t27*3.566153386244494E-2+t86-t208); - Jx_partial_dq(16, 11) = t441-x12*(t4*4.95436E-1-t28*6.740600000000002E-2+t433); - Jx_partial_dq(16, 12) = t441-t433*x12; - Jx_partial_dq(17, 9) = x10*(t70+t102)-t415*x11; - Jx_partial_dq(17, 10) = -t415*x10+x11*(t27*4.987264424463382E-1+t70+t102); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 2); - - fkPtr_->getTranslationHessian(H_translation); - fkPtr_->getRPYHessian(H_rotation); - - Jx_partial_dq.row(18) = H_translation(0) * x; - Jx_partial_dq.row(19) = H_translation(1) * x; - Jx_partial_dq.row(20) = H_translation(2) * x; - Jx_partial_dq.row(21) = H_rotation(0) * x; - Jx_partial_dq.row(22) = H_rotation(1) * x; - Jx_partial_dq.row(23) = H_rotation(2) * x; -} - -void DigitDynamicsConstraints::get_JTx_partial_dq(const VecX& q, const VecX& x) { - assert(x.size() == NUM_DEPENDENT_JOINTS); - assert(JTx_partial_dq.rows() == modelPtr_->nv); - assert(JTx_partial_dq.cols() == modelPtr_->nv); - - double x1 = x(0); - double x2 = x(1); - double x3 = x(2); - double x4 = x(3); - double x5 = x(4); - double x6 = x(5); - double x7 = x(6); - double x8 = x(7); - double x9 = x(8); - double x10 = x(9); - double x11 = x(10); - double x12 = x(11); - double x13 = x(12); - double x14 = x(13); - double x15 = x(14); - double x16 = x(15); - double x17 = x(16); - double x18 = x(17); - double x19 = x(18); - - double t2 = cos(q(9)); - double t3 = cos(q(10)); - double t4 = cos(q(11)); - double t5 = cos(q(12)); - double t6 = cos(q(13)); - double t7 = cos(q(14)); - double t8 = cos(q(15)); - double t9 = cos(q(16)); - double t10 = cos(q(17)); - double t11 = cos(q(18)); - double t12 = cos(q(19)); - double t13 = cos(q(20)); - double t14 = cos(q(24)); - double t15 = cos(q(25)); - double t16 = cos(q(26)); - double t17 = cos(q(27)); - double t18 = cos(q(28)); - double t19 = cos(q(29)); - double t20 = cos(q(30)); - double t21 = cos(q(31)); - double t22 = cos(q(32)); - double t23 = cos(q(33)); - double t24 = cos(q(34)); - double t25 = cos(q(35)); - double t26 = sin(q(9)); - double t27 = sin(q(10)); - double t28 = sin(q(11)); - double t29 = sin(q(12)); - double t30 = sin(q(13)); - double t31 = sin(q(14)); - double t32 = sin(q(15)); - double t33 = sin(q(16)); - double t34 = sin(q(17)); - double t35 = sin(q(18)); - double t36 = sin(q(19)); - double t37 = sin(q(20)); - double t38 = sin(q(24)); - double t39 = sin(q(25)); - double t40 = sin(q(26)); - double t41 = sin(q(27)); - double t42 = sin(q(28)); - double t43 = sin(q(29)); - double t44 = sin(q(30)); - double t45 = sin(q(31)); - double t46 = sin(q(32)); - double t47 = sin(q(33)); - double t48 = sin(q(34)); - double t49 = sin(q(35)); - double t50 = t12*1.696216709330505E-2; - double t51 = t24*1.696216709330505E-2; - double t52 = t36*1.696216709330505E-2; - double t53 = t48*-1.696216709330505E-2; - double t54 = t48*1.696216709330505E-2; - double t55 = t37*9.551E-3; - double t56 = t49*9.551E-3; - double t57 = t12*5.143951577823025E-2; - double t58 = t24*5.143951577823025E-2; - double t59 = t36*5.143951577823025E-2; - double t60 = t48*5.143951577823025E-2; - double t61 = t4*t5*-6.370345907739961E-5; - double t62 = t4*t5*6.370345907739961E-5; - double t63 = t16*t17*6.370345907739961E-5; - double t64 = t2*t3*6.725214316796237E-2; - double t65 = t14*t15*6.725214316796237E-2; - double t66 = t12*t13*1.69996184260823E-2; - double t67 = t24*t25*1.69996184260823E-2; - double t68 = t12*t13*1.718955829676478E-2; - double t69 = t24*t25*1.718955829676478E-2; - double t70 = t2*t3*3.539606431708408E-2; - double t71 = t14*t15*3.539606431708408E-2; - double t72 = t6*t8*-2.246221860400801E-3; - double t73 = t6*t8*2.246221860400801E-3; - double t74 = t18*t20*2.246221860400801E-3; - double t75 = t12*t13*9.070578524442012E-3; - double t76 = t12*t13*9.070578524442013E-3; - double t77 = t24*t25*9.070578524442012E-3; - double t78 = t24*t25*9.070578524442013E-3; - double t79 = t7*t8*2.360206106172353E-3; - double t80 = t19*t20*2.360206106172353E-3; - double t81 = t4*t29*6.370345907739961E-5; - double t82 = t5*t28*6.370345907739961E-5; - double t83 = t16*t41*6.370345907739961E-5; - double t84 = t17*t40*6.370345907739961E-5; - double t85 = t2*t27*6.711175107535902E-2; - double t86 = t3*t26*6.711175107535902E-2; - double t87 = t14*t39*6.711175107535902E-2; - double t88 = t15*t38*6.711175107535902E-2; - double t89 = t12*t37*1.69996184260823E-2; - double t90 = t13*t36*-1.69996184260823E-2; - double t91 = t13*t36*1.69996184260823E-2; - double t92 = t24*t49*1.69996184260823E-2; - double t93 = t25*t48*1.69996184260823E-2; - double t94 = t10*t11*6.574122182670539E-4; - double t95 = t22*t23*6.574122182670539E-4; - double t96 = t12*t37*1.718955829676478E-2; - double t97 = t13*t36*-1.718955829676478E-2; - double t98 = t13*t36*1.718955829676478E-2; - double t99 = t24*t49*1.718955829676478E-2; - double t100 = t25*t48*1.718955829676478E-2; - double t101 = t2*t27*4.365115769377904E-3; - double t102 = t3*t26*-4.365115769377904E-3; - double t103 = t3*t26*4.365115769377904E-3; - double t104 = t14*t39*4.365115769377904E-3; - double t105 = t15*t38*4.365115769377904E-3; - double t106 = t6*t32*2.246221860400801E-3; - double t107 = t8*t30*2.246221860400801E-3; - double t108 = t18*t44*2.246221860400801E-3; - double t109 = t20*t42*2.246221860400801E-3; - double t110 = t12*t37*-9.070578524442012E-3; - double t111 = t12*t37*9.070578524442012E-3; - double t112 = t12*t37*9.070578524442013E-3; - double t113 = t13*t36*-9.070578524442013E-3; - double t114 = t13*t36*-9.070578524442012E-3; - double t115 = t13*t36*9.070578524442012E-3; - double t116 = t13*t36*9.070578524442013E-3; - double t117 = t24*t49*-9.070578524442013E-3; - double t118 = t24*t49*9.070578524442012E-3; - double t119 = t24*t49*9.070578524442013E-3; - double t120 = t25*t48*9.070578524442012E-3; - double t121 = t25*t48*9.070578524442013E-3; - double t122 = t12*t13*5.605619802270151E-3; - double t123 = t24*t25*5.605619802270151E-3; - double t124 = t12*t13*5.668252425759201E-3; - double t125 = t24*t25*5.668252425759201E-3; - double t126 = t12*t13*2.991020934719675E-3; - double t127 = t12*t13*2.991020934719677E-3; - double t128 = t24*t25*2.991020934719675E-3; - double t129 = t24*t25*2.991020934719677E-3; - double t130 = t28*t29*6.370345907739961E-5; - double t131 = t40*t41*-6.370345907739961E-5; - double t132 = t40*t41*6.370345907739961E-5; - double t133 = t26*t27*-6.725214316796237E-2; - double t134 = t26*t27*6.725214316796237E-2; - double t135 = t38*t39*6.725214316796237E-2; - double t136 = t36*t37*1.69996184260823E-2; - double t137 = t48*t49*1.69996184260823E-2; - double t138 = t36*t37*-1.718955829676478E-2; - double t139 = t36*t37*1.718955829676478E-2; - double t140 = t48*t49*-1.718955829676478E-2; - double t141 = t48*t49*1.718955829676478E-2; - double t142 = t6*t8*1.26575449899492E-2; - double t143 = t18*t20*-1.26575449899492E-2; - double t144 = t18*t20*1.26575449899492E-2; - double t145 = t26*t27*3.539606431708408E-2; - double t146 = t38*t39*-3.539606431708408E-2; - double t147 = t38*t39*3.539606431708408E-2; - double t148 = t30*t32*2.246221860400801E-3; - double t149 = t42*t44*-2.246221860400801E-3; - double t150 = t42*t44*2.246221860400801E-3; - double t151 = t36*t37*-9.070578524442013E-3; - double t152 = t36*t37*9.070578524442012E-3; - double t153 = t36*t37*9.070578524442013E-3; - double t154 = t48*t49*-9.070578524442013E-3; - double t155 = t48*t49*9.070578524442012E-3; - double t156 = t48*t49*9.070578524442013E-3; - double t157 = t12*t37*-5.605619802270151E-3; - double t158 = t12*t37*5.605619802270151E-3; - double t159 = t13*t36*5.605619802270151E-3; - double t160 = t24*t49*5.605619802270151E-3; - double t161 = t25*t48*-5.605619802270151E-3; - double t162 = t25*t48*5.605619802270151E-3; - double t163 = t9*t11*8.234792310892687E-4; - double t164 = t21*t23*8.234792310892687E-4; - double t165 = t12*t37*5.668252425759201E-3; - double t166 = t13*t36*5.668252425759201E-3; - double t167 = t24*t49*-5.668252425759201E-3; - double t168 = t24*t49*5.668252425759201E-3; - double t169 = t25*t48*-5.668252425759201E-3; - double t170 = t25*t48*5.668252425759201E-3; - double t171 = t31*t32*-2.360206106172353E-3; - double t172 = t31*t32*2.360206106172353E-3; - double t173 = t43*t44*2.360206106172353E-3; - double t174 = t12*t37*-2.991020934719677E-3; - double t175 = t12*t37*2.991020934719675E-3; - double t176 = t12*t37*2.991020934719677E-3; - double t177 = t13*t36*2.991020934719675E-3; - double t178 = t13*t36*2.991020934719677E-3; - double t179 = t24*t49*-2.991020934719675E-3; - double t180 = t24*t49*2.991020934719675E-3; - double t181 = t24*t49*2.991020934719677E-3; - double t182 = t25*t48*2.991020934719675E-3; - double t183 = t25*t48*2.991020934719677E-3; - double t184 = t34*t35*-6.574122182670539E-4; - double t185 = t34*t35*6.574122182670539E-4; - double t186 = t46*t47*6.574122182670539E-4; - double t187 = t7*t32*1.263678697118524E-2; - double t188 = t8*t31*1.263678697118524E-2; - double t189 = t19*t44*1.263678697118524E-2; - double t190 = t20*t43*-1.263678697118524E-2; - double t191 = t20*t43*1.263678697118524E-2; - double t192 = t6*t32*1.26575449899492E-2; - double t193 = t8*t30*1.26575449899492E-2; - double t194 = t18*t44*1.26575449899492E-2; - double t195 = t20*t42*1.26575449899492E-2; - double t196 = t4*t5*1.20005624647208E-1; - double t197 = t16*t17*-1.20005624647208E-1; - double t198 = t16*t17*1.20005624647208E-1; - double t199 = t36*t37*5.605619802270151E-3; - double t200 = t48*t49*5.605619802270151E-3; - double t201 = t9*t35*-8.234792310892687E-4; - double t202 = t9*t35*8.234792310892687E-4; - double t203 = t11*t33*8.234792310892687E-4; - double t204 = t21*t47*8.234792310892687E-4; - double t205 = t23*t45*8.234792310892687E-4; - double t206 = t36*t37*5.668252425759201E-3; - double t207 = t48*t49*5.668252425759201E-3; - double t208 = t2*t3*4.941905177865888E-1; - double t209 = t14*t15*4.941905177865888E-1; - double t210 = t9*t11*1.549180159908108E-2; - double t211 = t21*t23*-1.549180159908108E-2; - double t212 = t21*t23*1.549180159908108E-2; - double t213 = t36*t37*-2.991020934719677E-3; - double t214 = t36*t37*2.991020934719675E-3; - double t215 = t36*t37*2.991020934719677E-3; - double t216 = t48*t49*-2.991020934719677E-3; - double t217 = t48*t49*2.991020934719675E-3; - double t218 = t48*t49*2.991020934719677E-3; - double t219 = t30*t32*-1.26575449899492E-2; - double t220 = t30*t32*1.26575449899492E-2; - double t221 = t42*t44*1.26575449899492E-2; - double t222 = t4*t29*1.20005624647208E-1; - double t223 = t5*t28*1.20005624647208E-1; - double t224 = t16*t41*1.20005624647208E-1; - double t225 = t17*t40*1.20005624647208E-1; - double t226 = t33*t35*8.234792310892687E-4; - double t227 = t45*t47*-8.234792310892687E-4; - double t228 = t45*t47*8.234792310892687E-4; - double t229 = t2*t27*4.954563135453205E-1; - double t230 = t3*t26*4.954563135453205E-1; - double t231 = t14*t39*4.954563135453205E-1; - double t232 = t15*t38*4.954563135453205E-1; - double t233 = t9*t35*1.549180159908108E-2; - double t234 = t11*t33*-1.549180159908108E-2; - double t235 = t11*t33*1.549180159908108E-2; - double t236 = t21*t47*1.549180159908108E-2; - double t237 = t23*t45*1.549180159908108E-2; - double t238 = t10*t35*1.549973690114125E-2; - double t239 = t11*t34*1.549973690114125E-2; - double t240 = t22*t47*1.549973690114125E-2; - double t241 = t23*t46*-1.549973690114125E-2; - double t242 = t23*t46*1.549973690114125E-2; - double t243 = t28*t29*-1.20005624647208E-1; - double t244 = t28*t29*1.20005624647208E-1; - double t245 = t40*t41*1.20005624647208E-1; - double t246 = t26*t27*4.941905177865888E-1; - double t247 = t38*t39*-4.941905177865888E-1; - double t248 = t38*t39*4.941905177865888E-1; - double t249 = t33*t35*1.549180159908108E-2; - double t250 = t45*t47*1.549180159908108E-2; - double t251 = t9*t10*t11*2.87566285868891E-1; - double t252 = t21*t22*t23*-2.87566285868891E-1; - double t253 = t21*t22*t23*2.87566285868891E-1; - double t254 = t9*t10*t11*2.879825211612492E-1; - double t255 = t21*t22*t23*-2.879825211612492E-1; - double t256 = t21*t22*t23*2.879825211612492E-1; - double t257 = t6*t7*t8*-3.397508858570615E-1; - double t258 = t6*t7*t8*3.397508858570615E-1; - double t259 = t18*t19*t20*3.397508858570615E-1; - double t260 = t6*t7*t8*-3.399783924207052E-1; - double t261 = t6*t7*t8*3.399783924207052E-1; - double t262 = t18*t19*t20*3.399783924207052E-1; - double t263 = t9*t10*t35*2.87566285868891E-1; - double t264 = t9*t11*t34*-2.87566285868891E-1; - double t265 = t9*t11*t34*2.87566285868891E-1; - double t266 = t10*t11*t33*2.87566285868891E-1; - double t267 = t21*t22*t47*-2.87566285868891E-1; - double t268 = t21*t22*t47*2.87566285868891E-1; - double t269 = t21*t23*t46*2.87566285868891E-1; - double t270 = t22*t23*t45*2.87566285868891E-1; - double t271 = t9*t10*t35*2.879825211612492E-1; - double t272 = t9*t11*t34*2.879825211612492E-1; - double t273 = t10*t11*t33*-2.879825211612492E-1; - double t274 = t10*t11*t33*2.879825211612492E-1; - double t275 = t21*t22*t47*2.879825211612492E-1; - double t276 = t21*t23*t46*2.879825211612492E-1; - double t277 = t22*t23*t45*2.879825211612492E-1; - double t278 = t6*t7*t8*3.020283789547073E-3; - double t279 = t18*t19*t20*-3.020283789547073E-3; - double t280 = t18*t19*t20*3.020283789547073E-3; - double t281 = t9*t10*t11*-3.064210757541298E-3; - double t282 = t9*t10*t11*3.064210757541298E-3; - double t283 = t21*t22*t23*3.064210757541298E-3; - double t284 = t9*t10*t11*3.104080344633556E-3; - double t285 = t21*t22*t23*3.104080344633556E-3; - double t286 = t6*t7*t8*3.105990081579729E-3; - double t287 = t18*t19*t20*-3.105990081579729E-3; - double t288 = t18*t19*t20*3.105990081579729E-3; - double t289 = t6*t7*t32*-3.397508858570615E-1; - double t290 = t6*t7*t32*3.397508858570615E-1; - double t291 = t6*t8*t31*3.397508858570615E-1; - double t292 = t7*t8*t30*3.397508858570615E-1; - double t293 = t18*t19*t44*3.397508858570615E-1; - double t294 = t18*t20*t43*3.397508858570615E-1; - double t295 = t19*t20*t42*3.397508858570615E-1; - double t296 = t6*t7*t32*-3.399783924207052E-1; - double t297 = t6*t7*t32*3.399783924207052E-1; - double t298 = t6*t8*t31*3.399783924207052E-1; - double t299 = t7*t8*t30*3.399783924207052E-1; - double t300 = t18*t19*t44*3.399783924207052E-1; - double t301 = t18*t20*t43*3.399783924207052E-1; - double t302 = t19*t20*t42*3.399783924207052E-1; - double t303 = t9*t34*t35*2.87566285868891E-1; - double t304 = t10*t33*t35*2.87566285868891E-1; - double t305 = t11*t33*t34*-2.87566285868891E-1; - double t306 = t11*t33*t34*2.87566285868891E-1; - double t307 = t21*t46*t47*2.87566285868891E-1; - double t308 = t22*t45*t47*2.87566285868891E-1; - double t309 = t23*t45*t46*2.87566285868891E-1; - double t310 = t9*t34*t35*2.879825211612492E-1; - double t311 = t10*t33*t35*2.879825211612492E-1; - double t312 = t11*t33*t34*-2.879825211612492E-1; - double t313 = t11*t33*t34*2.879825211612492E-1; - double t314 = t21*t46*t47*2.879825211612492E-1; - double t315 = t22*t45*t47*2.879825211612492E-1; - double t316 = t23*t45*t46*2.879825211612492E-1; - double t317 = t6*t7*t32*3.020283789547073E-3; - double t318 = t6*t8*t31*3.020283789547073E-3; - double t319 = t7*t8*t30*3.020283789547073E-3; - double t320 = t18*t19*t44*-3.020283789547073E-3; - double t321 = t18*t19*t44*3.020283789547073E-3; - double t322 = t18*t20*t43*3.020283789547073E-3; - double t323 = t19*t20*t42*3.020283789547073E-3; - double t324 = t9*t10*t35*-3.064210757541298E-3; - double t325 = t9*t10*t35*3.064210757541298E-3; - double t326 = t9*t11*t34*3.064210757541298E-3; - double t327 = t10*t11*t33*3.064210757541298E-3; - double t328 = t21*t22*t47*3.064210757541298E-3; - double t329 = t21*t23*t46*3.064210757541298E-3; - double t330 = t22*t23*t45*3.064210757541298E-3; - double t331 = t9*t10*t35*-3.104080344633556E-3; - double t332 = t9*t10*t35*3.104080344633556E-3; - double t333 = t9*t11*t34*3.104080344633556E-3; - double t334 = t10*t11*t33*3.104080344633556E-3; - double t335 = t21*t22*t47*3.104080344633556E-3; - double t336 = t21*t23*t46*3.104080344633556E-3; - double t337 = t22*t23*t45*3.104080344633556E-3; - double t338 = t6*t7*t32*3.105990081579729E-3; - double t339 = t6*t8*t31*3.105990081579729E-3; - double t340 = t7*t8*t30*3.105990081579729E-3; - double t341 = t18*t19*t44*-3.105990081579729E-3; - double t342 = t18*t19*t44*3.105990081579729E-3; - double t343 = t18*t20*t43*3.105990081579729E-3; - double t344 = t19*t20*t42*3.105990081579729E-3; - double t345 = t6*t31*t32*3.397508858570615E-1; - double t346 = t7*t30*t32*3.397508858570615E-1; - double t347 = t8*t30*t31*3.397508858570615E-1; - double t348 = t18*t43*t44*3.397508858570615E-1; - double t349 = t19*t42*t44*3.397508858570615E-1; - double t350 = t20*t42*t43*-3.397508858570615E-1; - double t351 = t20*t42*t43*3.397508858570615E-1; - double t352 = t6*t31*t32*3.399783924207052E-1; - double t353 = t7*t30*t32*3.399783924207052E-1; - double t354 = t8*t30*t31*3.399783924207052E-1; - double t355 = t18*t43*t44*3.399783924207052E-1; - double t356 = t19*t42*t44*3.399783924207052E-1; - double t357 = t20*t42*t43*-3.399783924207052E-1; - double t358 = t20*t42*t43*3.399783924207052E-1; - double t359 = t33*t34*t35*-2.87566285868891E-1; - double t360 = t33*t34*t35*2.87566285868891E-1; - double t361 = t45*t46*t47*-2.87566285868891E-1; - double t362 = t45*t46*t47*2.87566285868891E-1; - double t363 = t33*t34*t35*-2.879825211612492E-1; - double t364 = t33*t34*t35*2.879825211612492E-1; - double t365 = t45*t46*t47*2.879825211612492E-1; - double t366 = t6*t31*t32*3.020283789547073E-3; - double t367 = t7*t30*t32*3.020283789547073E-3; - double t368 = t8*t30*t31*-3.020283789547073E-3; - double t369 = t8*t30*t31*3.020283789547073E-3; - double t370 = t18*t43*t44*3.020283789547073E-3; - double t371 = t19*t42*t44*3.020283789547073E-3; - double t372 = t20*t42*t43*3.020283789547073E-3; - double t373 = t9*t34*t35*3.064210757541298E-3; - double t374 = t10*t33*t35*3.064210757541298E-3; - double t375 = t11*t33*t34*-3.064210757541298E-3; - double t376 = t11*t33*t34*3.064210757541298E-3; - double t377 = t21*t46*t47*-3.064210757541298E-3; - double t378 = t21*t46*t47*3.064210757541298E-3; - double t379 = t22*t45*t47*3.064210757541298E-3; - double t380 = t23*t45*t46*-3.064210757541298E-3; - double t381 = t23*t45*t46*3.064210757541298E-3; - double t382 = t9*t34*t35*3.104080344633556E-3; - double t383 = t10*t33*t35*3.104080344633556E-3; - double t384 = t11*t33*t34*3.104080344633556E-3; - double t385 = t21*t46*t47*3.104080344633556E-3; - double t386 = t22*t45*t47*-3.104080344633556E-3; - double t387 = t22*t45*t47*3.104080344633556E-3; - double t388 = t23*t45*t46*-3.104080344633556E-3; - double t389 = t23*t45*t46*3.104080344633556E-3; - double t390 = t6*t31*t32*3.105990081579729E-3; - double t391 = t7*t30*t32*3.105990081579729E-3; - double t392 = t8*t30*t31*-3.105990081579729E-3; - double t393 = t8*t30*t31*3.105990081579729E-3; - double t394 = t18*t43*t44*3.105990081579729E-3; - double t395 = t19*t42*t44*3.105990081579729E-3; - double t396 = t20*t42*t43*3.105990081579729E-3; - double t397 = t30*t31*t32*3.397508858570615E-1; - double t398 = t42*t43*t44*-3.397508858570615E-1; - double t399 = t42*t43*t44*3.397508858570615E-1; - double t400 = t30*t31*t32*3.399783924207052E-1; - double t401 = t42*t43*t44*-3.399783924207052E-1; - double t402 = t42*t43*t44*3.399783924207052E-1; - double t403 = t30*t31*t32*-3.020283789547073E-3; - double t404 = t30*t31*t32*3.020283789547073E-3; - double t405 = t42*t43*t44*3.020283789547073E-3; - double t406 = t33*t34*t35*3.064210757541298E-3; - double t407 = t45*t46*t47*-3.064210757541298E-3; - double t408 = t45*t46*t47*3.064210757541298E-3; - double t409 = t33*t34*t35*3.104080344633556E-3; - double t410 = t45*t46*t47*-3.104080344633556E-3; - double t411 = t45*t46*t47*3.104080344633556E-3; - double t412 = t30*t31*t32*-3.105990081579729E-3; - double t413 = t30*t31*t32*3.105990081579729E-3; - double t414 = t42*t43*t44*3.105990081579729E-3; - double t415 = t101+t145; - double t416 = t104+t146; - double t417 = t171+t187; - double t418 = t173+t189; - double t419 = t85+t246; - double t420 = t87+t247; - double t421 = t133+t229; - double t422 = t135+t231; - double t423 = t184+t238; - double t424 = t186+t240; - double t441 = t66+t112+t159+t214; - double t442 = t90+t122+t151+t175; - double t443 = t67+t117+t161+t217; - double t444 = t93+t123+t154+t179; - double t445 = t68+t110+t166+t213; - double t446 = t97+t124+t152+t174; - double t447 = t69+t118+t169+t216; - double t448 = t100+t125+t155+t181; - double t449 = t61+t130+t222+t223; - double t450 = t81+t82+t196+t243; - double t451 = t63+t131+t224+t225; - double t452 = t83+t84+t197+t245; - double t472 = t263+t363+t374+t382; - double t473 = t304+t310+t324+t409; - double t474 = t267+t365+t379+t385; - double t475 = t308+t314+t328+t410; - double t476 = t289+t366+t391+t400; - double t477 = t338+t346+t352+t403; - double t478 = t293+t370+t395+t401; - double t479 = t341+t349+t355+t405; - double t480 = t266+t272+t281+t384; - double t481 = t251+t312+t327+t333; - double t482 = t270+t276+t283+t388; - double t483 = t252+t316+t330+t336; - double t484 = t286+t292+t298+t368; - double t485 = t257+t318+t340+t354; - double t486 = t287+t295+t301+t372; - double t487 = t259+t322+t344+t357; - double t510 = t72+t193+t296+t367+t390+t397; - double t511 = t107+t142+t317+t345+t353+t412; - double t512 = t74+t195+t300+t371+t394+t398; - double t513 = t109+t143+t320+t348+t356+t414; - double t514 = t163+t234+t271+t359+t373+t383; - double t515 = t203+t210+t303+t311+t331+t406; - double t516 = t164+t237+t275+t361+t377+t386; - double t517 = t205+t211+t307+t315+t335+t407; - double t425 = t415*x18; - double t426 = t416*x15; - double t427 = t417*x9; - double t428 = t418*x3; - double t429 = t419*x17; - double t430 = t420*x14; - double t431 = t421*x16; - double t432 = t422*x13; - double t433 = t423*x12; - double t434 = t424*x6; - double t453 = t442*x7; - double t454 = t441*x8; - double t455 = t444*x1; - double t456 = t443*x2; - double t457 = t446*x10; - double t458 = t445*x11; - double t459 = t448*x4; - double t460 = t447*x5; - double t461 = t450*x16; - double t462 = t449*x17; - double t463 = t452*x13; - double t464 = t451*x14; - double t488 = t481*x10; - double t489 = t480*x11; - double t490 = t483*x4; - double t491 = t482*x5; - double t492 = t485*x7; - double t493 = t484*x8; - double t494 = t487*x1; - double t495 = t486*x2; - double t496 = t473*x10; - double t497 = t472*x11; - double t498 = t475*x4; - double t499 = t474*x5; - double t500 = t477*x7; - double t501 = t476*x8; - double t502 = t479*x1; - double t503 = t478*x2; - double t518 = t515*x10; - double t519 = t514*x11; - double t520 = t517*x4; - double t521 = t516*x5; - double t522 = t511*x7; - double t523 = t510*x8; - double t524 = t513*x1; - double t525 = t512*x2; - double t435 = -t425; - double t436 = -t427; - double t437 = -t429; - double t438 = -t430; - double t439 = -t431; - double t440 = -t434; - double t465 = -t456; - double t466 = -t457; - double t467 = -t458; - double t468 = -t459; - double t469 = -t461; - double t470 = -t462; - double t471 = -t464; - double t504 = -t488; - double t505 = -t489; - double t506 = -t491; - double t507 = -t494; - double t508 = -t497; - double t509 = -t501; - double t527 = -t519; - double t528 = -t521; - double t529 = -t523; - double t533 = t492+t493; - double t537 = t428+t502+t503; - double t541 = t524+t525; - double t526 = t426+t432+t438; - double t530 = t435+t437+t439; - double t531 = t463+t471; - double t532 = t469+t470; - double t534 = t490+t506; - double t535 = t495+t507; - double t536 = t504+t505; - double t538 = t433+t496+t508; - double t539 = t440+t498+t499; - double t540 = t436+t500+t509; - double t542 = t522+t529; - double t543 = t518+t527; - double t544 = t520+t528; - double t545 = t453+t454+t466+t467; - double t546 = t455+t460+t465+t468; - - JTx_partial_dq.setZero(); - JTx_partial_dq(9, 9) = x18*(t70+t102)-x16*(t64+t230)-x17*(t86-t208); - JTx_partial_dq(9, 10) = t530; - JTx_partial_dq(10, 9) = t530; - JTx_partial_dq(10, 10) = -x16*(t27*-4.365860704565494E-4+t64+t230)+x18*(t27*4.987264424463382E-1+t70+t102)-x17*(t27*3.566153386244494E-2+t86-t208); - JTx_partial_dq(11, 11) = -x17*(t4*4.95436E-1-t28*6.740600000000002E-2+t449)+x16*(t4*6.740600000000002E-2+t28*4.95436E-1-t81-t82-t196+t244); - JTx_partial_dq(11, 12) = t532; - JTx_partial_dq(12, 11) = t532; - JTx_partial_dq(12, 12) = t532; - JTx_partial_dq(13, 13) = x8*(t6*5.699060997402858E-2+t30*1.034589188110661E-3-t148-t192+t278+t291+t299+t392)+x7*(t6*-1.034589188110661E-3+t30*5.699060997402858E-2+t106+t219+t260+t319+t339+t347); - JTx_partial_dq(13, 14) = t533; - JTx_partial_dq(13, 15) = t542; - JTx_partial_dq(14, 13) = t533; - JTx_partial_dq(14, 14) = x8*(t278+t291+t299+t392)+x7*(t260+t319+t339+t347)-x9*(t79+t188); - JTx_partial_dq(14, 15) = t540; - JTx_partial_dq(15, 13) = t542; - JTx_partial_dq(15, 14) = t540; - JTx_partial_dq(15, 15) = -x8*(t148+t192-t278-t291-t299+t393)-x9*(t32*3.39756885202024E-1+t79+t188)+x7*(t106+t219+t260+t319+t339+t347); - JTx_partial_dq(16, 16) = x11*(t9*5.699060997402856E-2-t33*1.034589188110661E-3+t226+t233+t264+t273+t284+t375)-x10*(t9*1.034589188110661E-3+t33*5.699060997402856E-2+t201+t249+t254+t305+t326+t334); - JTx_partial_dq(16, 17) = t536; - JTx_partial_dq(16, 18) = t543; - JTx_partial_dq(17, 16) = t536; - JTx_partial_dq(17, 17) = -x10*(t254+t305+t326+t334)+x12*(t94+t239)-x11*(t265+t274-t284+t376); - JTx_partial_dq(17, 18) = t538; - JTx_partial_dq(18, 16) = t543; - JTx_partial_dq(18, 17) = t538; - JTx_partial_dq(18, 18) = x12*(t35*2.875818595898751E-1+t94+t239)+x11*(t226+t233+t264+t273+t284+t375)-x10*(t201+t249+t254+t305+t326+t334); - JTx_partial_dq(19, 19) = -x7*(t50-t59-t76+t89-t177+t199)+x10*(-t50+t59+t75+t96+t178+t206)-x8*(t52+t57+t113+t126+t136+t157)-x11*(t52+t57+t114+t127+t138+t165); - JTx_partial_dq(19, 20) = t545; - JTx_partial_dq(20, 19) = t545; - JTx_partial_dq(20, 20) = -x8*(t113+t126+t136+t157)-x11*(t114+t127+t138+t165)+x10*(t75+t96+t178+t206)+x12*(t13*1.81E-2-t55)+x7*(t76-t89+t177-t199)-x9*(t13*1.79E-2+t55); - JTx_partial_dq(24, 24) = x15*(t71+t105)-x14*(t88+t209)-x13*(t65-t232); - JTx_partial_dq(24, 25) = t526; - JTx_partial_dq(25, 24) = t526; - JTx_partial_dq(25, 25) = -x14*(t39*-3.566153386244494E-2+t88+t209)+x15*(t39*4.987264424463382E-1+t71+t105)+x13*(t39*4.365860704565494E-4-t65+t232); - JTx_partial_dq(26, 26) = x13*(t16*6.740600000000002E-2-t40*4.95436E-1+t452)+x14*(t16*4.95436E-1+t40*6.740600000000002E-2-t63+t132-t224-t225); - JTx_partial_dq(26, 27) = t531; - JTx_partial_dq(27, 26) = t531; - JTx_partial_dq(27, 27) = t531; - JTx_partial_dq(28, 28) = -x1*(t18*1.034589188110661E-3+t42*5.699060997402858E-2-t108-t221+t262+t323+t343+t350)+x2*(t18*-5.699060997402858E-2+t42*1.034589188110661E-3+t149+t194+t279+t294+t302+t396); - JTx_partial_dq(28, 29) = t535; - JTx_partial_dq(28, 30) = t541; - JTx_partial_dq(29, 28) = t535; - JTx_partial_dq(29, 29) = x2*(t279+t294+t302+t396)-x1*(t262+t323+t343+t350)-x3*(t80+t190); - JTx_partial_dq(29, 30) = t537; - JTx_partial_dq(30, 28) = t541; - JTx_partial_dq(30, 29) = t537; - JTx_partial_dq(30, 30) = x1*(t108+t221-t262-t323-t343+t351)-x3*(t44*3.39756885202024E-1+t80+t190)+x2*(t149+t194+t279+t294+t302+t396); - JTx_partial_dq(31, 31) = -x5*(t21*5.699060997402856E-2+t45*1.034589188110661E-3+t227+t236+t269+t277+t285+t380)+x4*(t21*-1.034589188110661E-3+t45*5.699060997402856E-2+t204+t250+t255+t309+t329+t337); - JTx_partial_dq(31, 32) = t534; - JTx_partial_dq(31, 33) = t544; - JTx_partial_dq(32, 31) = t534; - JTx_partial_dq(32, 32) = -x5*(t269+t277+t285+t380)+x4*(t255+t309+t329+t337)+x6*(t95+t241); - JTx_partial_dq(32, 33) = t539; - JTx_partial_dq(33, 31) = t544; - JTx_partial_dq(33, 32) = t539; - JTx_partial_dq(33, 33) = x6*(t47*2.875818595898751E-1+t95+t241)-x5*(t227+t236+t269+t277+t285+t380)+x4*(t204+t250+t255+t309+t329+t337); - JTx_partial_dq(34, 34) = -x1*(t51+t60-t78-t92+t182+t200)-x4*(t51+t60-t77+t99+t183-t207)+x2*(t53+t58+t121+t128+t137+t160)+x5*(t53+t58+t120+t129+t140+t167); - JTx_partial_dq(34, 35) = t546; - JTx_partial_dq(35, 34) = t546; - JTx_partial_dq(35, 35) = x2*(t121+t128+t137+t160)+x5*(t120+t129+t140+t167)-x3*(t25*1.79E-2-t56)+x1*(t78+t92-t182-t200)+x4*(t77-t99-t183+t207)+x6*(t25*1.81E-2+t56); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 2); - - fkPtr_->getTranslationHessian(H_translation); - fkPtr_->getRPYHessian(H_rotation); - - JTx_partial_dq += H_translation(0) * x(18); - JTx_partial_dq += H_translation(1) * x(19); - JTx_partial_dq += H_translation(2) * x(20); - JTx_partial_dq += H_rotation(0) * x(21); - JTx_partial_dq += H_rotation(1) * x(22); - JTx_partial_dq += H_rotation(2) * x(23); -} - -void DigitDynamicsConstraints::get_Jxy_partial_dq(const VecX& q, const VecX& x, const VecX& y) { - assert(x.size() == modelPtr_->nv); - assert(y.size() == modelPtr_->nv); - assert(Jxy_partial_dq.rows() == NUM_DEPENDENT_JOINTS); - assert(Jxy_partial_dq.cols() == modelPtr_->nv); - - double x1 = x(0); - double x2 = x(1); - double x3 = x(2); - double x4 = x(3); - double x5 = x(4); - double x6 = x(5); - double x7 = x(6); - double x8 = x(7); - double x9 = x(8); - double x10 = x(9); - double x11 = x(10); - double x12 = x(11); - double x13 = x(12); - double x14 = x(13); - double x15 = x(14); - double x16 = x(15); - double x17 = x(16); - double x18 = x(17); - double x19 = x(18); - double x20 = x(19); - double x21 = x(20); - double x22 = x(21); - double x23 = x(22); - double x24 = x(23); - double x25 = x(24); - double x26 = x(25); - double x27 = x(26); - double x28 = x(27); - double x29 = x(28); - double x30 = x(29); - double x31 = x(30); - double x32 = x(31); - double x33 = x(32); - double x34 = x(33); - double x35 = x(34); - double x36 = x(35); - - double y1 = y(0); - double y2 = y(1); - double y3 = y(2); - double y4 = y(3); - double y5 = y(4); - double y6 = y(5); - double y7 = y(6); - double y8 = y(7); - double y9 = y(8); - double y10 = y(9); - double y11 = y(10); - double y12 = y(11); - double y13 = y(12); - double y14 = y(13); - double y15 = y(14); - double y16 = y(15); - double y17 = y(16); - double y18 = y(17); - double y19 = y(18); - double y20 = y(19); - double y21 = y(20); - double y22 = y(21); - double y23 = y(22); - double y24 = y(23); - double y25 = y(24); - double y26 = y(25); - double y27 = y(26); - double y28 = y(27); - double y29 = y(28); - double y30 = y(29); - double y31 = y(30); - double y32 = y(31); - double y33 = y(32); - double y34 = y(33); - double y35 = y(34); - double y36 = y(35); - - double t2 = cos(q(9)); - double t3 = cos(q(10)); - double t4 = cos(q(11)); - double t5 = cos(q(12)); - double t6 = cos(q(13)); - double t7 = cos(q(14)); - double t8 = cos(q(15)); - double t9 = cos(q(16)); - double t10 = cos(q(17)); - double t11 = cos(q(18)); - double t12 = cos(q(19)); - double t13 = cos(q(20)); - double t14 = cos(q(24)); - double t15 = cos(q(25)); - double t16 = cos(q(26)); - double t17 = cos(q(27)); - double t18 = cos(q(28)); - double t19 = cos(q(29)); - double t20 = cos(q(30)); - double t21 = cos(q(31)); - double t22 = cos(q(32)); - double t23 = cos(q(33)); - double t24 = cos(q(34)); - double t25 = cos(q(35)); - double t26 = sin(q(9)); - double t27 = sin(q(10)); - double t28 = sin(q(11)); - double t29 = sin(q(12)); - double t30 = sin(q(13)); - double t31 = sin(q(14)); - double t32 = sin(q(15)); - double t33 = sin(q(16)); - double t34 = sin(q(17)); - double t35 = sin(q(18)); - double t36 = sin(q(19)); - double t37 = sin(q(20)); - double t38 = sin(q(24)); - double t39 = sin(q(25)); - double t40 = sin(q(26)); - double t41 = sin(q(27)); - double t42 = sin(q(28)); - double t43 = sin(q(29)); - double t44 = sin(q(30)); - double t45 = sin(q(31)); - double t46 = sin(q(32)); - double t47 = sin(q(33)); - double t48 = sin(q(34)); - double t49 = sin(q(35)); - double t50 = t12*1.696216709330505E-2; - double t51 = t24*1.696216709330505E-2; - double t52 = t13*9.551E-3; - double t53 = t25*9.551E-3; - double t54 = t36*1.696216709330505E-2; - double t55 = t48*-1.696216709330505E-2; - double t56 = t48*1.696216709330505E-2; - double t57 = t12*5.143951577823025E-2; - double t58 = t24*5.143951577823025E-2; - double t59 = t36*5.143951577823025E-2; - double t60 = t48*5.143951577823025E-2; - double t61 = t4*t5*-6.370345907739961E-5; - double t62 = t4*t5*6.370345907739961E-5; - double t63 = t16*t17*6.370345907739961E-5; - double t64 = t2*t3*6.711175107535902E-2; - double t65 = t14*t15*6.711175107535902E-2; - double t66 = t12*t13*1.69996184260823E-2; - double t67 = t24*t25*1.69996184260823E-2; - double t68 = t12*t13*1.718955829676478E-2; - double t69 = t24*t25*1.718955829676478E-2; - double t70 = t2*t3*4.365115769377904E-3; - double t71 = t14*t15*4.365115769377904E-3; - double t72 = t6*t8*-2.246221860400801E-3; - double t73 = t6*t8*2.246221860400801E-3; - double t74 = t18*t20*2.246221860400801E-3; - double t75 = t12*t13*9.070578524442012E-3; - double t76 = t12*t13*9.070578524442013E-3; - double t77 = t24*t25*9.070578524442012E-3; - double t78 = t24*t25*9.070578524442013E-3; - double t79 = t4*t29*6.370345907739961E-5; - double t80 = t5*t28*6.370345907739961E-5; - double t81 = t16*t41*6.370345907739961E-5; - double t82 = t17*t40*6.370345907739961E-5; - double t83 = t2*t27*6.725214316796237E-2; - double t84 = t3*t26*-6.725214316796237E-2; - double t85 = t3*t26*6.725214316796237E-2; - double t86 = t14*t39*6.725214316796237E-2; - double t87 = t15*t38*6.725214316796237E-2; - double t88 = t12*t37*-1.69996184260823E-2; - double t89 = t12*t37*1.69996184260823E-2; - double t90 = t13*t36*-1.69996184260823E-2; - double t91 = t13*t36*1.69996184260823E-2; - double t92 = t24*t49*1.69996184260823E-2; - double t93 = t25*t48*1.69996184260823E-2; - double t94 = t12*t37*1.718955829676478E-2; - double t95 = t13*t36*-1.718955829676478E-2; - double t96 = t13*t36*1.718955829676478E-2; - double t97 = t24*t49*-1.718955829676478E-2; - double t98 = t24*t49*1.718955829676478E-2; - double t99 = t25*t48*1.718955829676478E-2; - double t100 = t2*t27*3.539606431708408E-2; - double t101 = t3*t26*3.539606431708408E-2; - double t102 = t14*t39*3.539606431708408E-2; - double t103 = t15*t38*-3.539606431708408E-2; - double t104 = t15*t38*3.539606431708408E-2; - double t105 = t6*t32*2.246221860400801E-3; - double t106 = t8*t30*2.246221860400801E-3; - double t107 = t18*t44*2.246221860400801E-3; - double t108 = t20*t42*2.246221860400801E-3; - double t109 = t12*t37*-9.070578524442012E-3; - double t110 = t12*t37*9.070578524442012E-3; - double t111 = t12*t37*9.070578524442013E-3; - double t112 = t13*t36*-9.070578524442013E-3; - double t113 = t13*t36*-9.070578524442012E-3; - double t114 = t13*t36*9.070578524442012E-3; - double t115 = t13*t36*9.070578524442013E-3; - double t116 = t24*t49*-9.070578524442013E-3; - double t117 = t24*t49*9.070578524442012E-3; - double t118 = t24*t49*9.070578524442013E-3; - double t119 = t25*t48*9.070578524442012E-3; - double t120 = t25*t48*9.070578524442013E-3; - double t121 = t12*t13*5.605619802270151E-3; - double t122 = t24*t25*5.605619802270151E-3; - double t123 = t12*t13*5.668252425759201E-3; - double t124 = t24*t25*5.668252425759201E-3; - double t125 = t7*t32*2.360206106172353E-3; - double t126 = t8*t31*-2.360206106172353E-3; - double t127 = t8*t31*2.360206106172353E-3; - double t128 = t19*t44*2.360206106172353E-3; - double t129 = t20*t43*2.360206106172353E-3; - double t130 = t12*t13*2.991020934719675E-3; - double t131 = t12*t13*2.991020934719677E-3; - double t132 = t24*t25*2.991020934719675E-3; - double t133 = t24*t25*2.991020934719677E-3; - double t134 = t28*t29*6.370345907739961E-5; - double t135 = t40*t41*-6.370345907739961E-5; - double t136 = t40*t41*6.370345907739961E-5; - double t137 = t26*t27*-6.711175107535902E-2; - double t138 = t26*t27*6.711175107535902E-2; - double t139 = t38*t39*6.711175107535902E-2; - double t140 = t36*t37*1.69996184260823E-2; - double t141 = t48*t49*1.69996184260823E-2; - double t142 = t10*t35*6.574122182670539E-4; - double t143 = t11*t34*-6.574122182670539E-4; - double t144 = t11*t34*6.574122182670539E-4; - double t145 = t22*t47*6.574122182670539E-4; - double t146 = t23*t46*6.574122182670539E-4; - double t147 = t36*t37*-1.718955829676478E-2; - double t148 = t36*t37*1.718955829676478E-2; - double t149 = t48*t49*-1.718955829676478E-2; - double t150 = t48*t49*1.718955829676478E-2; - double t151 = t26*t27*-4.365115769377904E-3; - double t152 = t26*t27*4.365115769377904E-3; - double t153 = t38*t39*4.365115769377904E-3; - double t154 = t7*t8*1.263678697118524E-2; - double t155 = t19*t20*1.263678697118524E-2; - double t156 = t6*t8*1.26575449899492E-2; - double t157 = t18*t20*-1.26575449899492E-2; - double t158 = t18*t20*1.26575449899492E-2; - double t159 = t30*t32*2.246221860400801E-3; - double t160 = t42*t44*-2.246221860400801E-3; - double t161 = t42*t44*2.246221860400801E-3; - double t162 = t36*t37*-9.070578524442013E-3; - double t163 = t36*t37*9.070578524442012E-3; - double t164 = t36*t37*9.070578524442013E-3; - double t165 = t48*t49*-9.070578524442013E-3; - double t166 = t48*t49*9.070578524442012E-3; - double t167 = t48*t49*9.070578524442013E-3; - double t168 = t12*t37*-5.605619802270151E-3; - double t169 = t12*t37*5.605619802270151E-3; - double t170 = t13*t36*5.605619802270151E-3; - double t171 = t24*t49*5.605619802270151E-3; - double t172 = t25*t48*-5.605619802270151E-3; - double t173 = t25*t48*5.605619802270151E-3; - double t174 = t9*t11*8.234792310892687E-4; - double t175 = t21*t23*8.234792310892687E-4; - double t176 = t12*t37*5.668252425759201E-3; - double t177 = t13*t36*5.668252425759201E-3; - double t178 = t24*t49*-5.668252425759201E-3; - double t179 = t24*t49*5.668252425759201E-3; - double t180 = t25*t48*-5.668252425759201E-3; - double t181 = t25*t48*5.668252425759201E-3; - double t182 = t12*t37*-2.991020934719677E-3; - double t183 = t12*t37*2.991020934719675E-3; - double t184 = t12*t37*2.991020934719677E-3; - double t185 = t13*t36*2.991020934719675E-3; - double t186 = t13*t36*2.991020934719677E-3; - double t187 = t24*t49*-2.991020934719675E-3; - double t188 = t24*t49*2.991020934719675E-3; - double t189 = t24*t49*2.991020934719677E-3; - double t190 = t25*t48*-2.991020934719677E-3; - double t191 = t25*t48*-2.991020934719675E-3; - double t192 = t25*t48*2.991020934719675E-3; - double t193 = t25*t48*2.991020934719677E-3; - double t194 = t6*t32*1.26575449899492E-2; - double t195 = t8*t30*1.26575449899492E-2; - double t196 = t18*t44*1.26575449899492E-2; - double t197 = t20*t42*1.26575449899492E-2; - double t198 = t4*t5*1.20005624647208E-1; - double t199 = t16*t17*-1.20005624647208E-1; - double t200 = t16*t17*1.20005624647208E-1; - double t201 = t36*t37*-5.605619802270151E-3; - double t202 = t36*t37*5.605619802270151E-3; - double t203 = t48*t49*-5.605619802270151E-3; - double t204 = t48*t49*5.605619802270151E-3; - double t205 = t9*t35*-8.234792310892687E-4; - double t206 = t9*t35*8.234792310892687E-4; - double t207 = t11*t33*8.234792310892687E-4; - double t208 = t21*t47*8.234792310892687E-4; - double t209 = t23*t45*8.234792310892687E-4; - double t210 = t36*t37*5.668252425759201E-3; - double t211 = t48*t49*5.668252425759201E-3; - double t212 = t2*t3*4.954563135453205E-1; - double t213 = t14*t15*4.954563135453205E-1; - double t214 = t9*t11*1.549180159908108E-2; - double t215 = t21*t23*-1.549180159908108E-2; - double t216 = t21*t23*1.549180159908108E-2; - double t217 = t10*t11*1.549973690114125E-2; - double t218 = t22*t23*1.549973690114125E-2; - double t219 = t36*t37*-2.991020934719677E-3; - double t220 = t36*t37*2.991020934719675E-3; - double t221 = t36*t37*2.991020934719677E-3; - double t222 = t48*t49*-2.991020934719677E-3; - double t223 = t48*t49*2.991020934719675E-3; - double t224 = t48*t49*2.991020934719677E-3; - double t225 = t31*t32*1.263678697118524E-2; - double t226 = t43*t44*-1.263678697118524E-2; - double t227 = t43*t44*1.263678697118524E-2; - double t228 = t30*t32*-1.26575449899492E-2; - double t229 = t30*t32*1.26575449899492E-2; - double t230 = t42*t44*1.26575449899492E-2; - double t231 = t4*t29*1.20005624647208E-1; - double t232 = t5*t28*1.20005624647208E-1; - double t233 = t16*t41*1.20005624647208E-1; - double t234 = t17*t40*1.20005624647208E-1; - double t235 = t33*t35*8.234792310892687E-4; - double t236 = t45*t47*-8.234792310892687E-4; - double t237 = t45*t47*8.234792310892687E-4; - double t238 = t2*t27*4.941905177865888E-1; - double t239 = t3*t26*4.941905177865888E-1; - double t240 = t14*t39*4.941905177865888E-1; - double t241 = t15*t38*-4.941905177865888E-1; - double t242 = t15*t38*4.941905177865888E-1; - double t243 = t9*t35*1.549180159908108E-2; - double t244 = t11*t33*-1.549180159908108E-2; - double t245 = t11*t33*1.549180159908108E-2; - double t246 = t21*t47*1.549180159908108E-2; - double t247 = t23*t45*1.549180159908108E-2; - double t248 = t28*t29*-1.20005624647208E-1; - double t249 = t28*t29*1.20005624647208E-1; - double t250 = t40*t41*1.20005624647208E-1; - double t251 = t26*t27*4.954563135453205E-1; - double t252 = t38*t39*-4.954563135453205E-1; - double t253 = t38*t39*4.954563135453205E-1; - double t254 = t33*t35*1.549180159908108E-2; - double t255 = t45*t47*1.549180159908108E-2; - double t256 = t34*t35*1.549973690114125E-2; - double t257 = t46*t47*-1.549973690114125E-2; - double t258 = t46*t47*1.549973690114125E-2; - double t259 = t9*t10*t11*2.87566285868891E-1; - double t260 = t21*t22*t23*-2.87566285868891E-1; - double t261 = t21*t22*t23*2.87566285868891E-1; - double t262 = t9*t10*t11*2.879825211612492E-1; - double t263 = t21*t22*t23*-2.879825211612492E-1; - double t264 = t21*t22*t23*2.879825211612492E-1; - double t265 = t6*t7*t8*-3.397508858570615E-1; - double t266 = t6*t7*t8*3.397508858570615E-1; - double t267 = t18*t19*t20*3.397508858570615E-1; - double t268 = t6*t7*t8*-3.399783924207052E-1; - double t269 = t6*t7*t8*3.399783924207052E-1; - double t270 = t18*t19*t20*-3.399783924207052E-1; - double t271 = t18*t19*t20*3.399783924207052E-1; - double t272 = t9*t10*t35*2.87566285868891E-1; - double t273 = t9*t11*t34*-2.87566285868891E-1; - double t274 = t9*t11*t34*2.87566285868891E-1; - double t275 = t10*t11*t33*2.87566285868891E-1; - double t276 = t21*t22*t47*-2.87566285868891E-1; - double t277 = t21*t22*t47*2.87566285868891E-1; - double t278 = t21*t23*t46*2.87566285868891E-1; - double t279 = t22*t23*t45*2.87566285868891E-1; - double t280 = t9*t10*t35*2.879825211612492E-1; - double t281 = t9*t11*t34*2.879825211612492E-1; - double t282 = t10*t11*t33*-2.879825211612492E-1; - double t283 = t10*t11*t33*2.879825211612492E-1; - double t284 = t21*t22*t47*-2.879825211612492E-1; - double t285 = t21*t22*t47*2.879825211612492E-1; - double t286 = t21*t23*t46*2.879825211612492E-1; - double t287 = t22*t23*t45*2.879825211612492E-1; - double t288 = t6*t7*t8*-3.020283789547073E-3; - double t289 = t6*t7*t8*3.020283789547073E-3; - double t290 = t18*t19*t20*-3.020283789547073E-3; - double t291 = t18*t19*t20*3.020283789547073E-3; - double t292 = t9*t10*t11*-3.064210757541298E-3; - double t293 = t9*t10*t11*3.064210757541298E-3; - double t294 = t21*t22*t23*3.064210757541298E-3; - double t295 = t9*t10*t11*-3.104080344633556E-3; - double t296 = t9*t10*t11*3.104080344633556E-3; - double t297 = t21*t22*t23*3.104080344633556E-3; - double t298 = t6*t7*t8*3.105990081579729E-3; - double t299 = t18*t19*t20*-3.105990081579729E-3; - double t300 = t18*t19*t20*3.105990081579729E-3; - double t301 = t6*t7*t32*-3.397508858570615E-1; - double t302 = t6*t7*t32*3.397508858570615E-1; - double t303 = t6*t8*t31*-3.397508858570615E-1; - double t304 = t6*t8*t31*3.397508858570615E-1; - double t305 = t7*t8*t30*3.397508858570615E-1; - double t306 = t18*t19*t44*3.397508858570615E-1; - double t307 = t18*t20*t43*3.397508858570615E-1; - double t308 = t19*t20*t42*3.397508858570615E-1; - double t309 = t6*t7*t32*-3.399783924207052E-1; - double t310 = t6*t7*t32*3.399783924207052E-1; - double t311 = t6*t8*t31*3.399783924207052E-1; - double t312 = t7*t8*t30*-3.399783924207052E-1; - double t313 = t7*t8*t30*3.399783924207052E-1; - double t314 = t18*t19*t44*3.399783924207052E-1; - double t315 = t18*t20*t43*3.399783924207052E-1; - double t316 = t19*t20*t42*3.399783924207052E-1; - double t317 = t9*t34*t35*2.87566285868891E-1; - double t318 = t10*t33*t35*2.87566285868891E-1; - double t319 = t11*t33*t34*-2.87566285868891E-1; - double t320 = t11*t33*t34*2.87566285868891E-1; - double t321 = t21*t46*t47*2.87566285868891E-1; - double t322 = t22*t45*t47*2.87566285868891E-1; - double t323 = t23*t45*t46*2.87566285868891E-1; - double t324 = t9*t34*t35*2.879825211612492E-1; - double t325 = t10*t33*t35*2.879825211612492E-1; - double t326 = t11*t33*t34*-2.879825211612492E-1; - double t327 = t11*t33*t34*2.879825211612492E-1; - double t328 = t21*t46*t47*2.879825211612492E-1; - double t329 = t22*t45*t47*2.879825211612492E-1; - double t330 = t23*t45*t46*2.879825211612492E-1; - double t331 = t6*t7*t32*3.020283789547073E-3; - double t332 = t6*t8*t31*3.020283789547073E-3; - double t333 = t7*t8*t30*3.020283789547073E-3; - double t334 = t18*t19*t44*-3.020283789547073E-3; - double t335 = t18*t19*t44*3.020283789547073E-3; - double t336 = t18*t20*t43*3.020283789547073E-3; - double t337 = t19*t20*t42*-3.020283789547073E-3; - double t338 = t19*t20*t42*3.020283789547073E-3; - double t339 = t9*t10*t35*-3.064210757541298E-3; - double t340 = t9*t10*t35*3.064210757541298E-3; - double t341 = t9*t11*t34*3.064210757541298E-3; - double t342 = t10*t11*t33*3.064210757541298E-3; - double t343 = t21*t22*t47*3.064210757541298E-3; - double t344 = t21*t23*t46*3.064210757541298E-3; - double t345 = t22*t23*t45*3.064210757541298E-3; - double t346 = t9*t10*t35*-3.104080344633556E-3; - double t347 = t9*t10*t35*3.104080344633556E-3; - double t348 = t9*t11*t34*3.104080344633556E-3; - double t349 = t10*t11*t33*3.104080344633556E-3; - double t350 = t21*t22*t47*3.104080344633556E-3; - double t351 = t21*t23*t46*3.104080344633556E-3; - double t352 = t22*t23*t45*3.104080344633556E-3; - double t353 = t6*t7*t32*3.105990081579729E-3; - double t354 = t6*t8*t31*3.105990081579729E-3; - double t355 = t7*t8*t30*3.105990081579729E-3; - double t356 = t18*t19*t44*-3.105990081579729E-3; - double t357 = t18*t19*t44*3.105990081579729E-3; - double t358 = t18*t20*t43*-3.105990081579729E-3; - double t359 = t18*t20*t43*3.105990081579729E-3; - double t360 = t19*t20*t42*3.105990081579729E-3; - double t361 = t6*t31*t32*3.397508858570615E-1; - double t362 = t7*t30*t32*3.397508858570615E-1; - double t363 = t8*t30*t31*3.397508858570615E-1; - double t364 = t18*t43*t44*3.397508858570615E-1; - double t365 = t19*t42*t44*3.397508858570615E-1; - double t366 = t20*t42*t43*-3.397508858570615E-1; - double t367 = t20*t42*t43*3.397508858570615E-1; - double t368 = t6*t31*t32*3.399783924207052E-1; - double t369 = t7*t30*t32*3.399783924207052E-1; - double t370 = t8*t30*t31*3.399783924207052E-1; - double t371 = t18*t43*t44*3.399783924207052E-1; - double t372 = t19*t42*t44*3.399783924207052E-1; - double t373 = t20*t42*t43*-3.399783924207052E-1; - double t374 = t20*t42*t43*3.399783924207052E-1; - double t375 = t33*t34*t35*-2.87566285868891E-1; - double t376 = t33*t34*t35*2.87566285868891E-1; - double t377 = t45*t46*t47*-2.87566285868891E-1; - double t378 = t45*t46*t47*2.87566285868891E-1; - double t379 = t33*t34*t35*-2.879825211612492E-1; - double t380 = t33*t34*t35*2.879825211612492E-1; - double t381 = t45*t46*t47*2.879825211612492E-1; - double t382 = t6*t31*t32*3.020283789547073E-3; - double t383 = t7*t30*t32*3.020283789547073E-3; - double t384 = t8*t30*t31*-3.020283789547073E-3; - double t385 = t8*t30*t31*3.020283789547073E-3; - double t386 = t18*t43*t44*3.020283789547073E-3; - double t387 = t19*t42*t44*3.020283789547073E-3; - double t388 = t20*t42*t43*3.020283789547073E-3; - double t389 = t9*t34*t35*3.064210757541298E-3; - double t390 = t10*t33*t35*3.064210757541298E-3; - double t391 = t11*t33*t34*-3.064210757541298E-3; - double t392 = t11*t33*t34*3.064210757541298E-3; - double t393 = t21*t46*t47*-3.064210757541298E-3; - double t394 = t21*t46*t47*3.064210757541298E-3; - double t395 = t22*t45*t47*3.064210757541298E-3; - double t396 = t23*t45*t46*-3.064210757541298E-3; - double t397 = t23*t45*t46*3.064210757541298E-3; - double t398 = t9*t34*t35*3.104080344633556E-3; - double t399 = t10*t33*t35*3.104080344633556E-3; - double t400 = t11*t33*t34*3.104080344633556E-3; - double t401 = t21*t46*t47*3.104080344633556E-3; - double t402 = t22*t45*t47*-3.104080344633556E-3; - double t403 = t22*t45*t47*3.104080344633556E-3; - double t404 = t23*t45*t46*-3.104080344633556E-3; - double t405 = t23*t45*t46*3.104080344633556E-3; - double t406 = t6*t31*t32*3.105990081579729E-3; - double t407 = t7*t30*t32*3.105990081579729E-3; - double t408 = t8*t30*t31*-3.105990081579729E-3; - double t409 = t8*t30*t31*3.105990081579729E-3; - double t410 = t18*t43*t44*3.105990081579729E-3; - double t411 = t19*t42*t44*3.105990081579729E-3; - double t412 = t20*t42*t43*3.105990081579729E-3; - double t413 = t30*t31*t32*3.397508858570615E-1; - double t414 = t42*t43*t44*-3.397508858570615E-1; - double t415 = t42*t43*t44*3.397508858570615E-1; - double t416 = t30*t31*t32*3.399783924207052E-1; - double t417 = t42*t43*t44*-3.399783924207052E-1; - double t418 = t42*t43*t44*3.399783924207052E-1; - double t419 = t30*t31*t32*-3.020283789547073E-3; - double t420 = t30*t31*t32*3.020283789547073E-3; - double t421 = t42*t43*t44*3.020283789547073E-3; - double t422 = t33*t34*t35*3.064210757541298E-3; - double t423 = t45*t46*t47*-3.064210757541298E-3; - double t424 = t45*t46*t47*3.064210757541298E-3; - double t425 = t33*t34*t35*3.104080344633556E-3; - double t426 = t45*t46*t47*-3.104080344633556E-3; - double t427 = t45*t46*t47*3.104080344633556E-3; - double t428 = t30*t31*t32*-3.105990081579729E-3; - double t429 = t30*t31*t32*3.105990081579729E-3; - double t430 = t42*t43*t44*3.105990081579729E-3; - double t431 = t70+t101; - double t432 = t71+t103; - double t433 = t100+t151; - double t434 = t102+t153; - double t435 = t126+t154; - double t436 = t129+t155; - double t437 = t64+t239; - double t438 = t65+t241; - double t439 = t84+t212; - double t440 = t87+t213; - double t441 = t125+t225; - double t442 = t128+t226; - double t443 = t143+t217; - double t444 = t146+t218; - double t445 = t137+t238; - double t446 = t139+t240; - double t447 = t83+t251; - double t448 = t86+t252; - double t449 = t142+t256; - double t450 = t145+t257; - double t486 = t66+t111+t170+t220; - double t487 = t76+t88+t185+t201; - double t488 = t90+t121+t162+t183; - double t489 = t112+t130+t140+t168; - double t490 = t67+t116+t172+t223; - double t491 = t78+t92+t191+t203; - double t492 = t93+t122+t165+t187; - double t493 = t120+t132+t141+t171; - double t494 = t68+t109+t177+t219; - double t495 = t75+t94+t186+t210; - double t496 = t95+t123+t163+t182; - double t497 = t113+t131+t147+t176; - double t498 = t69+t117+t180+t222; - double t499 = t77+t97+t190+t211; - double t500 = t99+t124+t166+t189; - double t501 = t119+t133+t149+t178; - double t502 = t61+t134+t231+t232; - double t503 = t79+t80+t198+t248; - double t504 = t63+t135+t233+t234; - double t505 = t81+t82+t199+t250; - double t547 = t272+t379+t390+t398; - double t548 = t280+t375+t389+t399; - double t549 = t318+t324+t339+t425; - double t550 = t317+t325+t346+t422; - double t551 = t276+t381+t395+t401; - double t552 = t284+t378+t394+t403; - double t553 = t322+t328+t343+t426; - double t554 = t321+t329+t350+t423; - double t556 = t301+t382+t407+t416; - double t557 = t309+t383+t406+t413; - double t558 = t331+t361+t369+t428; - double t559 = t353+t362+t368+t419; - double t560 = t306+t386+t411+t417; - double t561 = t314+t387+t410+t414; - double t562 = t334+t364+t372+t430; - double t563 = t356+t365+t371+t421; - double t565 = t274+t283+t295+t392; - double t566 = t275+t281+t292+t400; - double t567 = t262+t319+t341+t349; - double t568 = t259+t326+t342+t348; - double t569 = t278+t287+t297+t396; - double t570 = t279+t286+t294+t404; - double t571 = t263+t323+t344+t352; - double t572 = t260+t330+t345+t351; - double t574 = t289+t304+t313+t408; - double t575 = t298+t305+t311+t384; - double t576 = t265+t332+t355+t370; - double t577 = t268+t333+t354+t363; - double t578 = t290+t307+t316+t412; - double t579 = t299+t308+t315+t388; - double t580 = t267+t336+t360+t373; - double t581 = t271+t338+t359+t366; - double t662 = t235+t243+t273+t282+t296+t391; - double t667 = t175+t247+t285+t377+t393+t402; - double t670 = t159+t194+t288+t303+t312+t409; - double t671 = t107+t230+t270+t337+t358+t367; - double t451 = t431*x10; - double t452 = t431*x11; - double t453 = t432*x25; - double t454 = t432*x26; - double t455 = t433*x10; - double t456 = t434*x25; - double t457 = t435*x15; - double t458 = t435*x16; - double t459 = t436*x30; - double t460 = t436*x31; - double t461 = t437*x10; - double t462 = t437*x11; - double t463 = t438*x25; - double t464 = t438*x26; - double t465 = t439*x10; - double t466 = t439*x11; - double t467 = t440*x25; - double t468 = t440*x26; - double t469 = t441*x15; - double t470 = t442*x30; - double t471 = t443*x18; - double t472 = t443*x19; - double t473 = t444*x33; - double t474 = t444*x34; - double t475 = t445*x10; - double t476 = t446*x25; - double t477 = t447*x10; - double t478 = t448*x25; - double t479 = t449*x18; - double t480 = t450*x33; - double t506 = t486*x20; - double t507 = t488*x20; - double t508 = t486*x21; - double t509 = t487*x21; - double t510 = t488*x21; - double t511 = t489*x21; - double t512 = t490*x35; - double t513 = t492*x35; - double t514 = t490*x36; - double t515 = t491*x36; - double t516 = t492*x36; - double t517 = t493*x36; - double t518 = t494*x20; - double t519 = t496*x20; - double t520 = t494*x21; - double t521 = t495*x21; - double t522 = t496*x21; - double t523 = t497*x21; - double t524 = t498*x35; - double t525 = t500*x35; - double t526 = t498*x36; - double t527 = t499*x36; - double t528 = t500*x36; - double t529 = t501*x36; - double t530 = t502*x12; - double t531 = t503*x12; - double t532 = t502*x13; - double t533 = t503*x13; - double t534 = t504*x27; - double t535 = t505*x27; - double t536 = t504*x28; - double t537 = t505*x28; - double t584 = t566*x17; - double t585 = t568*x17; - double t586 = t565*x18; - double t587 = t566*x18; - double t588 = t567*x18; - double t589 = t568*x18; - double t590 = t566*x19; - double t591 = t568*x19; - double t592 = t570*x32; - double t593 = t572*x32; - double t594 = t569*x33; - double t595 = t570*x33; - double t596 = t571*x33; - double t597 = t572*x33; - double t598 = t570*x34; - double t599 = t572*x34; - double t600 = t575*x14; - double t601 = t576*x14; - double t602 = t574*x15; - double t603 = t575*x15; - double t604 = t576*x15; - double t605 = t577*x15; - double t606 = t575*x16; - double t607 = t576*x16; - double t608 = t579*x29; - double t609 = t580*x29; - double t610 = t578*x30; - double t611 = t579*x30; - double t612 = t580*x30; - double t613 = t581*x30; - double t614 = t579*x31; - double t615 = t580*x31; - double t616 = t547*x17; - double t617 = t549*x17; - double t618 = t547*x18; - double t619 = t548*x18; - double t620 = t549*x18; - double t621 = t550*x18; - double t622 = t547*x19; - double t623 = t549*x19; - double t624 = t551*x32; - double t625 = t553*x32; - double t626 = t551*x33; - double t627 = t552*x33; - double t628 = t553*x33; - double t629 = t554*x33; - double t630 = t551*x34; - double t631 = t553*x34; - double t632 = t556*x14; - double t633 = t559*x14; - double t634 = t556*x15; - double t635 = t557*x15; - double t636 = t558*x15; - double t637 = t559*x15; - double t638 = t556*x16; - double t639 = t559*x16; - double t640 = t560*x29; - double t641 = t563*x29; - double t642 = t560*x30; - double t643 = t561*x30; - double t644 = t562*x30; - double t645 = t563*x30; - double t646 = t560*x31; - double t647 = t563*x31; - double t657 = t72+t195+t557; - double t658 = t106+t156+t558; - double t659 = t74+t197+t561; - double t660 = t108+t157+t562; - double t661 = t205+t254+t567; - double t663 = t208+t255+t571; - double t664 = t236+t246+t569; - double t665 = t174+t244+t548; - double t666 = t207+t214+t550; - double t668 = t209+t215+t554; - double t669 = t105+t228+t577; - double t672 = t160+t196+t578; - double t677 = t667*x32; - double t679 = t667*x34; - double t682 = t670*x16; - double t683 = t671*x31; - double t694 = t662*x19; - double t481 = -t456; - double t482 = -t469; - double t483 = -t476; - double t484 = -t477; - double t485 = -t479; - double t538 = -t511; - double t539 = -t517; - double t540 = -t521; - double t541 = -t527; - double t542 = t474+t480; - double t543 = t452+t455; - double t544 = t460+t470; - double t545 = t462+t475; - double t546 = t468+t478; - double t648 = -t591; - double t649 = -t598; - double t650 = -t606; - double t651 = -t615; - double t652 = -t623; - double t653 = -t626; - double t654 = -t630; - double t655 = -t638; - double t656 = -t647; - double t673 = t665*x17; - double t674 = t666*x17; - double t675 = t665*x19; - double t676 = t666*x19; - double t678 = t668*x32; - double t680 = t668*x34; - double t681 = t669*x16; - double t684 = t672*x31; - double t685 = t657*x14; - double t686 = t658*x14; - double t687 = t657*x16; - double t688 = t658*x16; - double t689 = t659*x29; - double t690 = t660*x29; - double t691 = t659*x31; - double t692 = t660*x31; - double t693 = t661*x19; - double t695 = t663*x34; - double t696 = t664*x34; - double t699 = -t694; - double t700 = t507+t509; - double t701 = t513+t515; - double t702 = t518+t523; - double t703 = t524+t529; - double t704 = t530+t532; - double t705 = t531+t533; - double t706 = t534+t536; - double t707 = t535+t537; - double t717 = t584+t586+t622; - double t718 = t593+t596+t631; - double t719 = t590+t616+t619; - double t720 = t599+t625+t629; - double t721 = t601+t605+t639; - double t722 = t608+t610+t646; - double t723 = t607+t633+t636; - double t724 = t614+t640+t643; - double t555 = t472+t485; - double t564 = t454+t481; - double t573 = t458+t482; - double t582 = t464+t483; - double t583 = t466+t484; - double t697 = -t676; - double t698 = -t693; - double t708 = t506+t538; - double t709 = t512+t539; - double t710 = t519+t540; - double t711 = t525+t541; - double t712 = t704*y13; - double t713 = t705*y13; - double t714 = t706*y28; - double t715 = t707*y28; - double t725 = t585+t588+t652; - double t726 = t592+t594+t654; - double t727 = t617+t621+t648; - double t728 = t624+t627+t649; - double t729 = t600+t602+t655; - double t730 = t609+t613+t656; - double t731 = t632+t635+t650; - double t732 = t641+t644+t651; - double t733 = t628+t678+t695; - double t734 = t637+t681+t686; - double t735 = t634+t682+t685; - double t736 = t645+t683+t690; - double t737 = t642+t684+t689; - double t738 = t618+t673+t699; - double t740 = t653+t677+t696; - double t716 = -t713; - double t739 = t620+t674+t698; - - Jxy_partial_dq.setZero(); - Jxy_partial_dq(0, 28) = y29*(t611+t691+x29*(t18*-5.699060997402858E-2+t42*1.034589188110661E-3+t672))+t722*y30+t737*y31; - Jxy_partial_dq(0, 29) = y30*(t611+t561*x31+t578*x29)+t722*y29+t724*y31; - Jxy_partial_dq(0, 30) = t724*y30+t737*y29+y31*(t611+t691+t672*x29); - Jxy_partial_dq(0, 34) = y35*(t514-x35*(t55+t58+t493))+t709*y36; - Jxy_partial_dq(0, 35) = t709*y35+y36*(t514-t493*x35); - Jxy_partial_dq(1, 28) = y29*(t612-t692+x29*(t18*1.034589188110661E-3+t42*5.699060997402858E-2-t107-t230+t581))+t730*y30-t736*y31; - Jxy_partial_dq(1, 29) = y30*(t612-t562*x31+t581*x29)+t730*y29-t732*y31; - Jxy_partial_dq(1, 30) = -t732*y30-t736*y29-y31*(-t612+t692+t671*x29); - Jxy_partial_dq(1, 34) = t701*y36+y35*(t516-x35*(t51+t60-t78-t92+t192+t204)); - Jxy_partial_dq(1, 35) = t701*y35+y36*(t516+t491*x35); - Jxy_partial_dq(2, 29) = t544*y31+y30*(t459+t442*x31); - Jxy_partial_dq(2, 30) = t544*y30+y31*(t459-x31*(t20*3.39756885202024E-1-t128+t227)); - Jxy_partial_dq(2, 35) = x36*y36*(t49*1.79E-2+t53); - Jxy_partial_dq(3, 31) = y32*(t595+t679+x32*(t21*5.699060997402856E-2+t45*1.034589188110661E-3+t664))+t726*y33+t740*y34; - Jxy_partial_dq(3, 32) = y33*(t595-t552*x34+t569*x32)+t726*y32-t728*y34; - Jxy_partial_dq(3, 33) = -t728*y33+t740*y32+y34*(t595+t679+t664*x32); - Jxy_partial_dq(3, 34) = -y35*(t526+x35*(t55+t58+t501))-t703*y36; - Jxy_partial_dq(3, 35) = -t703*y35-y36*(t526+t501*x35); - Jxy_partial_dq(4, 31) = y32*(t597+t680+x32*(t21*-1.034589188110661E-3+t45*5.699060997402856E-2+t663))+t718*y33+t733*y34; - Jxy_partial_dq(4, 32) = y33*(t597+t554*x34+t571*x32)+t718*y32+t720*y34; - Jxy_partial_dq(4, 33) = t720*y33+t733*y32+y34*(t597+t680+t663*x32); - Jxy_partial_dq(4, 34) = -t711*y36-y35*(t528+x35*(t51+t60-t77+t98+t193-t211)); - Jxy_partial_dq(4, 35) = -t711*y35-y36*(t528-t499*x35); - Jxy_partial_dq(5, 32) = -t542*y34-y33*(t473+t450*x34); - Jxy_partial_dq(5, 33) = -y34*(t473-x34*(t23*2.875818595898751E-1-t145+t258))-t542*y33; - Jxy_partial_dq(5, 35) = -x36*y36*(t49*1.81E-2-t53); - Jxy_partial_dq(6, 13) = y14*(t603-t687+x14*(t6*5.699060997402858E-2+t30*1.034589188110661E-3-t159-t194+t574))+t729*y15-t735*y16; - Jxy_partial_dq(6, 14) = y15*(t603-t557*x16+t574*x14)+t729*y14-t731*y16; - Jxy_partial_dq(6, 15) = -t731*y15-t735*y14-y16*(-t603+t687+t670*x14); - Jxy_partial_dq(6, 19) = -y20*(t508-x20*(t54+t57+t489))-t708*y21; - Jxy_partial_dq(6, 20) = -t708*y20-y21*(t508-t489*x20); - Jxy_partial_dq(7, 13) = -y14*(t604+t688+x14*(t6*-1.034589188110661E-3+t30*5.699060997402858E-2+t669))-t721*y15-t734*y16; - Jxy_partial_dq(7, 14) = -y15*(t604+t558*x16+t577*x14)-t721*y14-t723*y16; - Jxy_partial_dq(7, 15) = -t723*y15-t734*y14-y16*(t604+t688+t669*x14); - Jxy_partial_dq(7, 19) = t700*y21+y20*(t510-x20*(t50-t59-t76+t89-t185+t202)); - Jxy_partial_dq(7, 20) = t700*y20+y21*(t510+t487*x20); - Jxy_partial_dq(8, 14) = -t573*y16-y15*(t457-t441*x16); - Jxy_partial_dq(8, 15) = -t573*y15-y16*(t457+x16*(t8*3.39756885202024E-1-t441)); - Jxy_partial_dq(8, 20) = x21*y21*(t37*1.79E-2-t52); - Jxy_partial_dq(9, 16) = y17*(t587+t675-x17*(t9*5.699060997402856E-2-t33*1.034589188110661E-3+t662))+t717*y18+t738*y19; - Jxy_partial_dq(9, 17) = y18*(t587+t548*x19+t565*x17)+t717*y17+t719*y19; - Jxy_partial_dq(9, 18) = t719*y18+t738*y17+y19*(t587+t675-t662*x17); - Jxy_partial_dq(9, 19) = y20*(t520+x20*(t54+t57+t497))+t702*y21; - Jxy_partial_dq(9, 20) = t702*y20+y21*(t520+t497*x20); - Jxy_partial_dq(10, 16) = -y17*(t589+t697+x17*(t9*1.034589188110661E-3+t33*5.699060997402856E-2+t661))-t725*y18+t739*y19; - Jxy_partial_dq(10, 17) = -y18*(t589-t550*x19+t567*x17)-t725*y17+t727*y19; - Jxy_partial_dq(10, 18) = t727*y18+t739*y17-y19*(t589+t697+t661*x17); - Jxy_partial_dq(10, 19) = -t710*y21-y20*(t522-x20*(-t50+t59+t495)); - Jxy_partial_dq(10, 20) = -t710*y20-y21*(t522-t495*x20); - Jxy_partial_dq(11, 17) = t555*y19+y18*(t471-t449*x19); - Jxy_partial_dq(11, 18) = t555*y18+y19*(t471+x19*(t11*2.875818595898751E-1-t449)); - Jxy_partial_dq(11, 20) = -x21*y21*(t37*1.81E-2+t52); - Jxy_partial_dq(12, 24) = t546*y26+y25*(t467+t448*x26); - Jxy_partial_dq(12, 25) = t546*y25+y26*(t467+x26*(t15*4.365860704565494E-4+t448)); - Jxy_partial_dq(12, 26) = t714+y27*(t536-x27*(t16*4.95436E-1+t40*6.740600000000002E-2-t63+t136-t233-t234)); - Jxy_partial_dq(12, 27) = t714+t706*y27; - Jxy_partial_dq(13, 24) = -t582*y26-y25*(t463-t446*x26); - Jxy_partial_dq(13, 25) = -t582*y25-y26*(t463-x26*(t15*3.566153386244494E-2+t446)); - Jxy_partial_dq(13, 26) = t715+y27*(t537+x27*(t16*6.740600000000002E-2-t40*4.95436E-1+t505)); - Jxy_partial_dq(13, 27) = t715+t707*y27; - Jxy_partial_dq(14, 24) = t564*y26+y25*(t453-t434*x26); - Jxy_partial_dq(14, 25) = y26*(t453+x26*(t15*4.987264424463382E-1-t434))+t564*y25; - Jxy_partial_dq(15, 9) = -t583*y11-y10*(t465-t447*x11); - Jxy_partial_dq(15, 10) = -t583*y10-y11*(t465-x11*(t3*4.365860704565494E-4+t447)); - Jxy_partial_dq(15, 11) = t712+y12*(t532+x12*(t4*4.95436E-1-t28*6.740600000000002E-2+t502)); - Jxy_partial_dq(15, 12) = t712+t704*y12; - Jxy_partial_dq(16, 9) = -t545*y11-y10*(t461+t445*x11); - Jxy_partial_dq(16, 10) = -t545*y10-y11*(t461+x11*(t3*3.566153386244494E-2+t445)); - Jxy_partial_dq(16, 11) = t716-y12*(t533-x12*(t4*6.740600000000002E-2+t28*4.95436E-1-t79-t80-t198+t249)); - Jxy_partial_dq(16, 12) = t716-t705*y12; - Jxy_partial_dq(17, 9) = -t543*y11-y10*(t451+t433*x11); - Jxy_partial_dq(17, 10) = -t543*y10-y11*(t451-x11*(t3*4.987264424463382E-1-t100+t152)); - - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 3); - - fkPtr_->getTranslationThirdOrderTensor(x, TOx_translation); - fkPtr_->getRPYThirdOrderTensor(x, TOx_rotation); - - Jxy_partial_dq.row(18) = TOx_translation(0) * y; - Jxy_partial_dq.row(19) = TOx_translation(1) * y; - Jxy_partial_dq.row(20) = TOx_translation(2) * y; - Jxy_partial_dq.row(21) = TOx_rotation(0) * y; - Jxy_partial_dq.row(22) = TOx_rotation(1) * y; - Jxy_partial_dq.row(23) = TOx_rotation(2) * y; -} - -}; // namespace Digit -}; // namespace RAPTOR diff --git a/Examples/Digit/src/DigitDynamicsConstraints_clean.cpp b/Examples/Digit/src/DigitDynamicsConstraints_clean.cpp deleted file mode 100644 index 3d202548..00000000 --- a/Examples/Digit/src/DigitDynamicsConstraints_clean.cpp +++ /dev/null @@ -1,1045 +0,0 @@ -#include "DigitDynamicsConstraints.h" - -namespace RAPTOR { -namespace Digit { - -DigitDynamicsConstraints::DigitDynamicsConstraints(const std::shared_ptr& modelPtr_input, - const Eigen::VectorXi& jtype_input, - char stanceLeg_input, - const Transform& stance_foot_T_des_input) : - modelPtr_(modelPtr_input), - jtype(jtype_input), - stanceLeg(stanceLeg_input), - DynamicsConstraints(modelPtr_input->nv, NUM_DEPENDENT_JOINTS) { - fkPtr_ = std::make_unique(modelPtr_.get(), jtype); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - if (modelPtr_->existJointName(dependentJointNames[i])) { - dependentJointIds[i] = modelPtr_->getJointId(dependentJointNames[i]) - 1; - } - else { - throw std::runtime_error("Can not find joint: " + dependentJointNames[i]); - } - } - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (modelPtr_->existJointName(independentJointNames[i])) { - independentJointIds[i] = modelPtr_->getJointId(independentJointNames[i]) - 1; - } - else { - throw std::runtime_error("Can not find joint: " + dependentJointNames[i]); - } - } - - if (stanceLeg == 'L' || stanceLeg == 'l') { - if (modelPtr_->existJointName("left_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("left_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: left_toe_roll"); - } - - stance_foot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, -0.05456, -0.0315; - } - else { - if (modelPtr_->existJointName("right_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: right_toe_roll"); - } - - stance_foot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0.05456, -0.0315; - } - - stance_foot_T_des = stance_foot_T_des_input; -} - -void DigitDynamicsConstraints::reinitialize(const char stanceLeg_input) { - if (stanceLeg_input == 0) { // swap the stance leg if there's no input - if (stanceLeg == 'L' || stanceLeg == 'l') { - stanceLeg = 'R'; - } - else { - stanceLeg = 'L'; - } - } - else { - if (stanceLeg_input != 'L' && - stanceLeg_input != 'R' && - stanceLeg_input != 'l' && - stanceLeg_input != 'r') { - throw std::runtime_error("Invalid stance leg input"); - } - stanceLeg = stanceLeg_input; - } - - // reinitialize the stance leg end effector transformation matrix - if (stanceLeg == 'L' || stanceLeg == 'l') { - if (modelPtr_->existJointName("left_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("left_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: left_toe_roll"); - } - - stance_foot_endT.R << 0, 1, 0, - -0.5, 0, sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, -0.05456, -0.0315; - } - else { - if (modelPtr_->existJointName("right_toe_roll")) { - contact_joint_id = modelPtr_->getJointId("right_toe_roll"); - } - else { - throw std::runtime_error("Can not find joint: right_toe_roll"); - } - - stance_foot_endT.R << 0, -1, 0, - 0.5, 0, -sin(M_PI / 3), - sin(M_PI / 3), 0, 0.5; - stance_foot_endT.p << 0, 0.05456, -0.0315; - } -} - -int DigitDynamicsConstraints::return_dependent_joint_index(const int id) { - assert(0 <= id && id < NUM_DEPENDENT_JOINTS); - return dependentJointIds[id]; -} - -int DigitDynamicsConstraints::return_independent_joint_index(const int id) { - assert(0 <= id && id < NUM_INDEPENDENT_JOINTS); - return independentJointIds[id]; -} - -void DigitDynamicsConstraints::fill_dependent_vector(VecX& r, const VecX& v, const bool setZero) { - assert(r.size() == modelPtr_->nv); - assert(v.size() == NUM_DEPENDENT_JOINTS); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r(dependentJointIds[i]) = v(i); - } -} - -void DigitDynamicsConstraints::fill_independent_vector(VecX& r, const VecX& v, const bool setZero) { - assert(r.size() == modelPtr_->nv); - assert(v.size() == NUM_INDEPENDENT_JOINTS); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r(independentJointIds[i]) = v(i); - } -} - -void DigitDynamicsConstraints::fill_dependent_columns(MatX& r, const MatX& m, const bool setZero) { - assert(m.cols() == NUM_DEPENDENT_JOINTS); - assert(r.cols() == modelPtr_->nv); - assert(m.rows() == r.rows()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.col(dependentJointIds[i]) = m.col(i); - } -} - -void DigitDynamicsConstraints::fill_independent_columns(MatX& r, const MatX& m, const bool setZero) { - assert(m.cols() == NUM_INDEPENDENT_JOINTS); - assert(r.cols() == modelPtr_->nv); - assert(m.rows() == r.rows()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.col(independentJointIds[i]) = m.col(i); - } -} - -void DigitDynamicsConstraints::fill_dependent_rows(MatX& r, const MatX& m, const bool setZero) { - assert(m.rows() == NUM_DEPENDENT_JOINTS); - assert(r.rows() == modelPtr_->nv); - assert(m.cols() == r.cols()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.row(dependentJointIds[i]) = m.row(i); - } -} - -void DigitDynamicsConstraints::fill_independent_rows(MatX& r, const MatX& m, const bool setZero) { - assert(m.rows() == NUM_INDEPENDENT_JOINTS); - assert(r.rows() == modelPtr_->nv); - assert(m.cols() == r.cols()); - - if (setZero) r.setZero(); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.row(independentJointIds[i]) = m.row(i); - } -} - -Eigen::VectorXd DigitDynamicsConstraints::get_dependent_vector(const VecX& v) { - assert(v.size() == modelPtr_->nv); - - VecX r(NUM_DEPENDENT_JOINTS); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r(i) = v(dependentJointIds[i]); - } - - return r; -} - -Eigen::VectorXd DigitDynamicsConstraints::get_independent_vector(const VecX& v) { - assert(v.size() == modelPtr_->nv); - - VecX r(NUM_INDEPENDENT_JOINTS); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r(i) = v(independentJointIds[i]); - } - - return r; -} - -void DigitDynamicsConstraints::get_dependent_columns(MatX& r, const MatX& m) { - assert(m.cols() == modelPtr_->nv); - assert(r.cols() == NUM_DEPENDENT_JOINTS); - assert(m.rows() == r.rows()); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.col(i) = m.col(dependentJointIds[i]); - } -} - -void DigitDynamicsConstraints::get_independent_columns(MatX& r, const MatX& m) { - assert(m.cols() == modelPtr_->nv); - assert(r.cols() == NUM_INDEPENDENT_JOINTS); - assert(m.rows() == r.rows()); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.col(i) = m.col(independentJointIds[i]); - } -} - -void DigitDynamicsConstraints::get_dependent_rows(MatX& r, const MatX& m) { - assert(m.rows() == modelPtr_->nv); - assert(r.rows() == NUM_DEPENDENT_JOINTS); - assert(m.cols() == r.cols()); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - r.row(i) = m.row(dependentJointIds[i]); - } -} - -void DigitDynamicsConstraints::get_independent_rows(MatX& r, const MatX& m) { - assert(m.rows() == modelPtr_->nv); - assert(r.rows() == NUM_INDEPENDENT_JOINTS); - assert(m.cols() == r.cols()); - - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - r.row(i) = m.row(independentJointIds[i]); - } -} - -void DigitDynamicsConstraints::setupJointPosition(VecX& q, bool compute_derivatives) { - if (recoverSavedData(q, compute_derivatives)) { - return; - } - - qcopy = q; - - // fill in dependent joint positions - // we first use approximation to give an initial guess for dependent joints - // then we use gsl multidimensional root-finding to provide a more accurate solution - - // a Fourier based approximation, that provides a good initial guess - // to the later GSL iterative root-findings - // the code is modified from code generation by Matlab symbolic toolbox - // right toe close loop - double q1 = q(modelPtr_->getJointId("right_toe_A") - 1); - double q2 = q(modelPtr_->getJointId("right_toe_B") - 1); - double t2 = cos(q1); - double t3 = cos(q2); - double t4 = sin(q1); - double t5 = sin(q2); - double t6 = q1*2.0; - double t7 = q2*2.0; - double t8 = cos(t6); - double t9 = cos(t7); - double t10 = sin(t6); - double t11 = sin(t7); - qcopy(modelPtr_->getJointId("right_toe_A_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*5.114967905744713E-1+t3*4.072902857734452E-1+t4*5.15019991000208E-1-t5*7.189052210514619E-1-t8*1.914308797309167E-1+t9*1.020482430826409E-1+t10*2.762037569148706E-1+t11*4.461078267300372E-1-t2*t3*5.669643819573642E-1+t2*t5*9.826313004441752E-1+t3*t4*7.066611122892024E-1-t4*t5*8.751268427354268E-2-t2*t9*1.526013089230614E-1+t3*t8*2.146475284950957E-1-t2*t11*6.373722738059316E-1-t3*t10*4.061233553924697E-1-t4*t9*2.591685269591633E-1-t5*t8*3.141536266927853E-1+t4*t11*1.154141052196728E-1+t5*t10*5.111686735370716E-2+t8*t9*2.629326893467769E-2+t8*t11*2.261999172185061E-1+t9*t10*1.714455893661597E-1-t10*t11*5.322298539488757E-2-3.496118766033006E-1); - qcopy(modelPtr_->getJointId("right_A2") - 1) = HigherOrderDerivatives::safeasin(t2*8.08508988653757+t3*8.033204820099098+t4*1.820663506981432+t5*9.760946687487468E-1-t8*2.143120822774579-t9*1.998293355526279-t10*1.095961671709748-t11*4.953901287885355E-1-t2*t3*1.273036938982479E+1-t2*t5*1.117825053271672-t3*t4*2.420405429552186+t4*t5*1.170037217867019+t2*t9*2.835751824751641+t3*t8*3.021210259875125+t2*t11*5.805706025081291E-1+t3*t10*1.468854423683356+t4*t9*5.420356015832145E-1+t5*t8*2.326217949887465E-1+t4*t11*1.600391744429722E-1+t5*t10*1.170908991628156E-1-t8*t9*4.40337591097512E-1-t8*t11*1.065731420572547E-1-t9*t10*3.400548068856727E-1-t10*t11*4.121086576936894E-1-4.663948776295725); - qcopy(modelPtr_->getJointId("right_toe_B_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*(-6.277469604269263E-1)-t3*6.676022839778379E-1-t4*8.399423316628958E-1+t5*1.529719504893023E-1+t8*1.44381731007234E-1+t9*4.870768316848186E-1+t10*5.196004288358185E-1+t11*4.899787657102767E-1+t2*t3*1.038356653790214+t2*t5*1.224128324227442+t3*t4*1.136249345040226-t4*t5*2.962244825741616E-1-t2*t9*7.061936994304631E-1-t3*t8*2.508516890107657E-1-t2*t11*7.147313228957868E-1-t3*t10*7.361945978788478E-1-t4*t9*3.638713473685915E-1-t5*t8*4.137119943797418E-1+t4*t11*2.198247731883075E-1+t5*t10*1.190720324216782E-1+t8*t9*1.91374695742122E-1+t8*t11*2.674233531253601E-1+t9*t10*2.592073581826719E-1-t10*t11*1.033333326963321E-1+3.90361533934657E-1); - qcopy(modelPtr_->getJointId("right_B2") - 1) = HigherOrderDerivatives::safeasin(t2*9.834460864042423+t3*1.012210878062529E+1+t4*4.114199768067971+t5*2.918615505121678-t8*2.33635398066763-t9*2.824506662141518-t10*2.436915399445716-t11*1.657161229950764-t2*t3*1.56824564483544E+1-t2*t5*4.071020737587724-t3*t4*5.741194151827515+t4*t5*1.412990674523749+t2*t9*3.933187185458805+t3*t8*3.362495208957303+t2*t11*2.232327688060054+t3*t10*3.377470435585635+t4*t9*1.500870022094839+t5*t8*1.104866050029796+t4*t11*1.111537011807542E-1+t5*t10*1.885808892070127E-1-t8*t9*5.75161230009884E-1-t8*t11*6.064451339600673E-1-t9*t10*9.059551372991537E-1-t10*t11*4.795029892364048E-1-5.829415122169579); - qcopy(modelPtr_->getJointId("right_toe_pitch") - 1) = -HigherOrderDerivatives::safeasin(t2*1.893989559445235E+1+t3*1.920869550741533E+1+t4*6.868951098405778+t5*3.51561215609706-t8*4.080342460091422-t9*4.456343821978674-t10*3.407846365996183-t11*2.503419572692307-t2*t3*2.929974638198693E+1-t2*t5*5.157359318839895-t3*t4*8.730366732237334+t4*t5*1.967066798625298+t2*t9*5.895002722892592+t3*t8*5.481616764469075+t2*t11*3.209642327668312+t3*t10*4.724342470672166+t4*t9*2.235861962074205+t5*t8*1.242934577497499+t4*t11*1.119458771782081+t5*t10*1.139096075125015-t8*t9*3.306304563913454E-1-t8*t11*7.650487856200098E-1-t9*t10*1.240832663118515-t10*t11*1.549291982370939-1.135688467845833E+1); - qcopy(modelPtr_->getJointId("right_toe_roll") - 1) = -HigherOrderDerivatives::safeasin(t2*1.925184467728597+t3*7.345366777287639+t4*2.843699508105022E+1+t5*2.720900626308319E+1+t8*3.57433462778918-t9*6.332457856504018-t10*1.691966583575934E+1-t11*1.60681725794845E+1-t2*t3*6.299462363896048-t2*t5*4.117199083152367E+1-t3*t4*4.282360516873334E+1+t4*t5*7.910198316638063E-2+t2*t9*7.854243405249483-t3*t8*4.011841131466795+t2*t11*2.320584908063003E+1+t3*t10*2.435545111239733E+1+t4*t9*1.284334673751474E+1+t5*t8*1.242519321726289E+1-t4*t11*1.106503053903957+t5*t10*8.20592115289103E-1-t8*t9*6.089534825702931E-1-t8*t11*7.206363286390523-t9*t10*7.502224690460121+t10*t11*1.255468934813467E-1-3.446675672448973); - - // left toe close loop - q1 = q(modelPtr_->getJointId("left_toe_A") - 1); - q2 = q(modelPtr_->getJointId("left_toe_B") - 1); - t2 = cos(q1); - t3 = cos(q2); - t4 = sin(q1); - t5 = sin(q2); - t6 = q1*2.0; - t7 = q2*2.0; - t8 = cos(t6); - t9 = cos(t7); - t10 = sin(t6); - t11 = sin(t7); - qcopy(modelPtr_->getJointId("left_toe_A_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*(-5.114926838701811E-1)-t3*4.072861778028993E-1+t4*5.150198314974532E-1-t5*7.189053388109297E-1+t8*1.914297694169048E-1-t9*1.020493568564005E-1+t10*2.762038362432678E-1+t11*4.461078824742524E-1+t2*t3*5.669587551620541E-1+t2*t5*9.826314482751646E-1+t3*t4*7.066613185154341E-1+t4*t5*8.751254943270917E-2+t2*t9*1.526028367712922E-1-t3*t8*2.146460048968412E-1-t2*t11*6.3737234338624E-1-t3*t10*4.061234579946822E-1-t4*t9*2.591685735898548E-1-t5*t8*3.141536565807085E-1-t4*t11*1.15414032157577E-1-t5*t10*5.111679397817798E-2-t8*t9*2.629368451925844E-2+t8*t11*2.261999309581469E-1+t9*t10*1.714456126031132E-1+t10*t11*5.322294548977349E-2+3.49608876912165E-1); - qcopy(modelPtr_->getJointId("left_A2") - 1) = HigherOrderDerivatives::safeasin(t2*8.085018160207163+t3*8.033133064086231-t4*1.820662389025083-t5*9.760941431166779E-1-t8*2.143101587807895-t9*1.998274051537184+t10*1.095961134647075+t11*4.953899201857669E-1-t2*t3*1.273027136495291E+1+t2*t5*1.11782444127773+t3*t4*2.420403987960523+t4*t5*1.17003991016308+t2*t9*2.835725404415498+t3*t8*3.021183922843985-t2*t11*5.805703711334091E-1-t3*t10*1.468853731639082-t4*t9*5.420352775054091E-1-t5*t8*2.326217084069311E-1+t4*t11*1.600377315075955E-1+t5*t10*1.170894522681423E-1-t8*t9*4.403304524513951E-1+t8*t11*1.065731191784157E-1+t9*t10*3.400546515442943E-1-t10*t11*4.121078791939159E-1-4.663896238435751); - qcopy(modelPtr_->getJointId("left_toe_B_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*6.277394162562625E-1+t3*6.6759473381467E-1-t4*8.399421448959555E-1+t5*1.52972128181329E-1-t8*1.443797201746081E-1-t9*4.870748185116889E-1+t10*5.196003431996498E-1+t11*4.899786813403554E-1-t2*t3*1.038346357873659+t2*t5*1.224128102824049+t3*t4*1.13624910964581+t4*t5*2.962247949009167E-1+t2*t9*7.06190949266457E-1+t3*t8*2.508489399062072E-1-t2*t11*7.147312184882999E-1-t3*t10*7.361944907642444E-1-t4*t9*3.638712999144287E-1-t5*t8*4.137119506580644E-1-t4*t11*2.198249429188003E-1-t5*t10*1.190722007592886E-1-t8*t9*1.913739574570494E-1+t8*t11*2.67423333074767E-1+t9*t10*2.592073373292207E-1+t10*t11*1.033334244581574E-1-3.903559984881447E-1); - qcopy(modelPtr_->getJointId("left_B2") - 1) = HigherOrderDerivatives::safeasin(t2*9.83436298937519+t3*1.012201086685809E+1-t4*4.114198671144258-t5*2.918615149671792-t8*2.3363277301986-t9*2.824480323210008+t10*2.436914889769644+t11*1.657161129062592-t2*t3*1.568232268107518E+1+t2*t5*4.071020370775729+t3*t4*5.741192746046224+t4*t5*1.412994342972937+t2*t9*3.933151135492005+t3*t8*3.362459265404445-t2*t11*2.232327610328257-t3*t10*3.377469784128673-t4*t9*1.500869712732554-t5*t8*1.104866037480153+t4*t11*1.111517321363341E-1+t5*t10*1.885789156342841E-1-t8*t9*5.751514893646594E-1+t8*t11*6.064451565041576E-1+t9*t10*9.05954995114355E-1-t10*t11*4.795019258370027E-1-5.829343436697216); - qcopy(modelPtr_->getJointId("left_toe_pitch") - 1) = HigherOrderDerivatives::safeasin(t2*1.893971200186481E+1+t3*1.920851184673561E+1-t4*6.868948549897667-t5*3.515611050123151-t8*4.080293186785779-t9*4.456294384490135+t10*3.407845158508307+t11*2.503419165594914-t2*t3*2.92994954151725E+1+t2*t5*5.157358065307949+t3*t4*8.730363458834317+t4*t5*1.967073609987012+t2*t9*5.894935044834759+t3*t8*5.481549285471837-t2*t11*3.209641901519396-t3*t10*4.72434092223045-t4*t9*2.235861236273534-t5*t8*1.242934428625482+t4*t11*1.119455114838831+t5*t10*1.139092408157223-t8*t9*3.306121591996909E-1+t8*t11*7.650487658943779E-1+t9*t10*1.240832321393588-t10*t11*1.549290006003741-1.135675024134487E+1); - qcopy(modelPtr_->getJointId("left_toe_roll") - 1) = HigherOrderDerivatives::safeasin(t2*1.92500223858939+t3*7.345184505784512-t4*2.843699897601034E+1-t5*2.720901076343499E+1+t8*3.574383636815427-t9*6.332408732694388+t10*1.691966794969343E+1+t11*1.606817500398276E+1-t2*t3*6.299213096460508+t2*t5*4.117199681273369E+1+t3*t4*4.282361029947926E+1+t4*t5*7.910853681196074E-2+t2*t9*7.854176116345911-t3*t8*4.011908279005063-t2*t11*2.320585230931768E+1-t3*t10*2.435545390288711E+1-t4*t9*1.284334796900915E+1-t5*t8*1.242519468406818E+1-t4*t11*1.106506602910529+t5*t10*8.205885613072367E-1-t8*t9*6.089352667535674E-1+t8*t11*7.206364083227697+t9*t10*7.502225364895965+t10*t11*1.255488247882333E-1-3.446542349892159); - - // right knee close loop - q1 = q(modelPtr_->getJointId("right_knee") - 1); - t2 = cos(q1); - t3 = sin(q1); - t4 = q1*2.0; - t5 = cos(t4); - t6 = sin(t4); - qcopy(modelPtr_->getJointId("right_tarsus") - 1) = -HigherOrderDerivatives::safeasin(t2*1.155848969647063E-3+t3*1.004686948291003+t5*1.274417498011625E-4-t6*1.785981355062532E-3-1.132590494159057E-2); - qcopy(modelPtr_->getJointId("right_achilles_rod") - 1) = -HigherOrderDerivatives::safeasin(t2*(-1.587289102030986E-3)-t3*1.001736520672665+t5*3.407131509821247E-4+t6*9.646678263881318E-4+1.539911054998293E-3); - qcopy(modelPtr_->getJointId("right_ach2") - 1) = HigherOrderDerivatives::safeasin(t2*(-7.197863326636346E-2)-t3*8.929579539511397E-3+t5*2.669904889172627E-4+t6*8.46571305589265E-5+7.18964949007849E-2); - - // left knee close loop - q1 = q(modelPtr_->getJointId("left_knee") - 1); - t2 = cos(q1); - t3 = sin(q1); - t4 = q1*2.0; - t5 = cos(t4); - t6 = sin(t4); - qcopy(modelPtr_->getJointId("left_tarsus") - 1) = HigherOrderDerivatives::safeasin(t2*1.155848972188414E-3-t3*1.004686948291033+t5*1.274417489907877E-4+t6*1.785981355072367E-3-1.132590494335349E-2); - qcopy(modelPtr_->getJointId("left_achilles_rod") - 1) = HigherOrderDerivatives::safeasin(t2*(-1.587289102219775E-3)+t3*1.001736520672708+t5*3.407131510426615E-4-t6*9.646678264174077E-4+1.539911055129276E-3); - qcopy(modelPtr_->getJointId("left_ach2") - 1) = HigherOrderDerivatives::safeasin(t2*(-7.197863326651638E-2)+t3*8.929579539517018E-3+t5*2.669904889661342E-4-t6*8.465713056222183E-5+7.189649490089099E-2); - - fkPtr_->compute(modelPtr_->getJointId("Rz"), - contact_joint_id, - qcopy, - nullptr, - &stance_foot_endT, - 0); - Transform torso_T = stance_foot_T_des * fkPtr_->getTransform().inverse(); - qcopy.block(0, 0, 6, 1) = torso_T.getXYZRPY(); - - // gsl multidimensional root-finding - const gsl_multiroot_fdfsolver_type *T; - gsl_multiroot_fdfsolver *s; - - int status; - size_t i, iter = 0; - - const size_t n = NUM_DEPENDENT_JOINTS; - gsl_multiroot_function_fdf f = {&fillDependent_f, - &fillDependent_df, - &fillDependent_fdf, - n, this}; - - gsl_vector *x = gsl_vector_alloc(n); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - gsl_vector_set(x, i, qcopy(dependentJointIds[i])); - } - - T = gsl_multiroot_fdfsolver_hybridsj; - s = gsl_multiroot_fdfsolver_alloc(T, n); - gsl_multiroot_fdfsolver_set(s, &f, x); - - // printf("\nstart gsl iterations:\n"); - - do { - iter++; - status = gsl_multiroot_fdfsolver_iterate(s); - - if (status) break; - - status = gsl_multiroot_test_residual(s->f, 1e-14); - - // printf ("iter = %ld, status = %s\n", iter, gsl_strerror(status)); - // for (int i = 0; i < 6; i++) { - // printf("%f, ", gsl_vector_get(s->x, i)) ; - // } - // for (int i = 0; i < 6; i++) { - // printf("%f, ", gsl_vector_get(s->f, 18 + i)) ; - // } - // printf("\n"); - } - while (status == GSL_CONTINUE && iter < 50); - - // printf ("total iter = %ld, status = %s\n\n", iter, gsl_strerror(status)); - - // the optimal solution found by gsl! - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - qcopy(dependentJointIds[i]) = gsl_vector_get(s->x, i); - } - - gsl_multiroot_fdfsolver_free(s); - gsl_vector_free(x); - - q = qcopy; - - if (compute_derivatives) { - get_c(q); - get_J(q); - - get_dependent_columns(J_dep, J); - get_independent_columns(J_indep, J); - - J_dep_qr = QRSolver(J_dep); - J_dep_T_qr = QRSolver(J_dep.transpose()); - - // sanity check on uniqueness (these two arguments are actually equivalent) - assert(J_dep_qr.rank() == J_dep.rows() && J_dep_qr.rank() == J_dep.cols()); - assert(J_dep_T_qr.rank() == J_dep.rows() && J_dep_T_qr.rank() == J_dep.cols()); - - P_dep = -J_dep_qr.solve(J_indep); - - pq_dep_pq_indep = P_dep; - } - - updateQueue(q, compute_derivatives); -} - -int fillDependent_f(const gsl_vector* x, void *params, gsl_vector* f) { - DigitDynamicsConstraints* constraintsData = (DigitDynamicsConstraints*)params; - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - constraintsData->qcopy(constraintsData->dependentJointIds[i]) = gsl_vector_get(x, i); - } - - constraintsData->get_c(constraintsData->qcopy); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - gsl_vector_set(f, i, constraintsData->c(i)); - } - - return GSL_SUCCESS; -} - -int fillDependent_df(const gsl_vector* x, void *params, gsl_matrix* J) { - DigitDynamicsConstraints* constraintsData = (DigitDynamicsConstraints*)params; - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - constraintsData->qcopy(constraintsData->dependentJointIds[i]) = gsl_vector_get(x, i); - } - - constraintsData->get_J(constraintsData->qcopy); - - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - for (int j = 0; j < NUM_DEPENDENT_JOINTS; j++) { - gsl_matrix_set(J, i, j, constraintsData->J(i, constraintsData->dependentJointIds[j])); - } - } - - return GSL_SUCCESS; -} - -int fillDependent_fdf(const gsl_vector* x, void *params, gsl_vector* f, gsl_matrix* J) { - fillDependent_f(x, params, f); - fillDependent_df(x, params, J); - - return GSL_SUCCESS; -} - -void DigitDynamicsConstraints::get_c(const VecX& q) { - c.resize(NUM_DEPENDENT_JOINTS); - - // left toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_A2"), - q, nullptr, - &left_toeA_rod_endT); - const Vec3 left_rodA = fkPtr_->getTranslation(); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeA_anchor_endT); - const Vec3 left_anchorA = fkPtr_->getTranslation(); - c.block(0, 0, 3, 1) = left_rodA - left_anchorA; - - // left toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_B2"), - q, nullptr, - &left_toeB_rod_endT); - const Vec3 left_rodB = fkPtr_->getTranslation(); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeB_anchor_endT); - const Vec3 left_anchorB = fkPtr_->getTranslation(); - c.block(3, 0, 3, 1) = left_rodB - left_anchorB; - - // left knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_ach2"), - q, nullptr, - &left_knee_rod_endT); - const Vec3 left_rodKnee = fkPtr_->getTranslation(); - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_tarsus"), - q, nullptr, - &left_knee_anchor_endT); - const Vec3 left_anchorKnee = fkPtr_->getTranslation(); - c.block(6, 0, 3, 1) = left_rodKnee - left_anchorKnee; - - // right toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_A2"), - q, nullptr, - &right_toeA_rod_endT); - const Vec3 right_rodA = fkPtr_->getTranslation(); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeA_anchor_endT); - const Vec3 right_anchorA = fkPtr_->getTranslation(); - c.block(9, 0, 3, 1) = right_rodA - right_anchorA; - - // right toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_B2"), - q, nullptr, - &right_toeB_rod_endT); - const Vec3 right_rodB = fkPtr_->getTranslation(); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeB_anchor_endT); - const Vec3 right_anchorB = fkPtr_->getTranslation(); - c.block(12, 0, 3, 1) = right_rodB - right_anchorB; - - // right knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_ach2"), - q, nullptr, - &right_knee_rod_endT); - const Vec3 right_rodKnee = fkPtr_->getTranslation(); - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_tarsus"), - q, nullptr, - &right_knee_anchor_endT); - const Vec3 right_anchorKnee = fkPtr_->getTranslation(); - c.block(15, 0, 3, 1) = right_rodKnee - right_anchorKnee; - - // stance foot contact constraint - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 0); - - c.block(18, 0, 3, 1) = fkPtr_->getTranslation() - stance_foot_T_des.p; - c.block(21, 0, 3, 1) = fkPtr_->getRPY() - stance_foot_T_des.getRPY(); -} - -void DigitDynamicsConstraints::get_J(const VecX& q) { - J.resize(NUM_DEPENDENT_JOINTS, NUM_JOINTS); - - // left toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_A2"), - q, nullptr, - &left_toeA_rod_endT, - 1); - const MatX J_left_rodA = fkPtr_->getTranslationJacobian(); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeA_anchor_endT, - 1); - const MatX J_left_anchorA = fkPtr_->getTranslationJacobian(); - J.middleRows(0, 3) = J_left_rodA - J_left_anchorA; - - // left toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_B2"), - q, nullptr, - &left_toeB_rod_endT, - 1); - const MatX J_left_rodB = fkPtr_->getTranslationJacobian(); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeB_anchor_endT, - 1); - const MatX J_left_anchorB = fkPtr_->getTranslationJacobian(); - J.middleRows(3, 3) = J_left_rodB - J_left_anchorB; - - // left knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_ach2"), - q, nullptr, - &left_knee_rod_endT, - 1); - const MatX J_left_rodKnee = fkPtr_->getTranslationJacobian(); - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_tarsus"), - q, nullptr, - &left_knee_anchor_endT, - 1); - const MatX J_left_anchorKnee = fkPtr_->getTranslationJacobian(); - J.middleRows(6, 3) = J_left_rodKnee - J_left_anchorKnee; - - // right toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_A2"), - q, nullptr, - &right_toeA_rod_endT, - 1); - const MatX J_right_rodA = fkPtr_->getTranslationJacobian(); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeA_anchor_endT, - 1); - const MatX J_right_anchorA = fkPtr_->getTranslationJacobian(); - J.middleRows(9, 3) = J_right_rodA - J_right_anchorA; - - // right toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_B2"), - q, nullptr, - &right_toeB_rod_endT, - 1); - const MatX J_right_rodB = fkPtr_->getTranslationJacobian(); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeB_anchor_endT, - 1); - const MatX J_right_anchorB = fkPtr_->getTranslationJacobian(); - J.middleRows(12, 3) = J_right_rodB - J_right_anchorB; - - // right knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_ach2"), - q, nullptr, - &right_knee_rod_endT, - 1); - const MatX J_right_rodKnee = fkPtr_->getTranslationJacobian(); - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_tarsus"), - q, nullptr, - &right_knee_anchor_endT, - 1); - const MatX J_right_anchorKnee = fkPtr_->getTranslationJacobian(); - J.middleRows(15, 3) = J_right_rodKnee - J_right_anchorKnee; - - // stance foot contact constraint - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 1); - - J.block(18, 0, 3, modelPtr_->nv) = fkPtr_->getTranslationJacobian(); - J.block(21, 0, 3, modelPtr_->nv) = fkPtr_->getRPYJacobian(); -} - -void DigitDynamicsConstraints::get_Jx_partial_dq(const VecX& q, const VecX& x) { - assert(x.size() == modelPtr_->nv); - Jx_partial_dq.resize(NUM_DEPENDENT_JOINTS, modelPtr_->nv); - - Eigen::Array H_rod_translation; - Eigen::Array H_anchor_translation; - - // left toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_A2"), - q, nullptr, - &left_toeA_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeA_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - Jx_partial_dq.row(0) = (H_rod_translation(0) - H_anchor_translation(0)) * x; - Jx_partial_dq.row(1) = (H_rod_translation(1) - H_anchor_translation(1)) * x; - Jx_partial_dq.row(2) = (H_rod_translation(2) - H_anchor_translation(2)) * x; - - // left toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_B2"), - q, nullptr, - &left_toeB_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeB_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - Jx_partial_dq.row(3) = (H_rod_translation(0) - H_anchor_translation(0)) * x; - Jx_partial_dq.row(4) = (H_rod_translation(1) - H_anchor_translation(1)) * x; - Jx_partial_dq.row(5) = (H_rod_translation(2) - H_anchor_translation(2)) * x; - - // left knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_ach2"), - q, nullptr, - &left_knee_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_tarsus"), - q, nullptr, - &left_knee_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - Jx_partial_dq.row(6) = (H_rod_translation(0) - H_anchor_translation(0)) * x; - Jx_partial_dq.row(7) = (H_rod_translation(1) - H_anchor_translation(1)) * x; - Jx_partial_dq.row(8) = (H_rod_translation(2) - H_anchor_translation(2)) * x; - - // right toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_A2"), - q, nullptr, - &right_toeA_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeA_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - Jx_partial_dq.row(9) = (H_rod_translation(0) - H_anchor_translation(0)) * x; - Jx_partial_dq.row(10) = (H_rod_translation(1) - H_anchor_translation(1)) * x; - Jx_partial_dq.row(11) = (H_rod_translation(2) - H_anchor_translation(2)) * x; - - // right toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_B2"), - q, nullptr, - &right_toeB_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeB_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - Jx_partial_dq.row(12) = (H_rod_translation(0) - H_anchor_translation(0)) * x; - Jx_partial_dq.row(13) = (H_rod_translation(1) - H_anchor_translation(1)) * x; - Jx_partial_dq.row(14) = (H_rod_translation(2) - H_anchor_translation(2)) * x; - - // right knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_ach2"), - q, nullptr, - &right_knee_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_tarsus"), - q, nullptr, - &right_knee_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - Jx_partial_dq.row(15) = (H_rod_translation(0) - H_anchor_translation(0)) * x; - Jx_partial_dq.row(16) = (H_rod_translation(1) - H_anchor_translation(1)) * x; - Jx_partial_dq.row(17) = (H_rod_translation(2) - H_anchor_translation(2)) * x; - - // stance foot contact constraint - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 2); - - fkPtr_->getTranslationHessian(H_translation); - fkPtr_->getRPYHessian(H_rotation); - - Jx_partial_dq.row(18) = H_translation(0) * x; - Jx_partial_dq.row(19) = H_translation(1) * x; - Jx_partial_dq.row(20) = H_translation(2) * x; - Jx_partial_dq.row(21) = H_rotation(0) * x; - Jx_partial_dq.row(22) = H_rotation(1) * x; - Jx_partial_dq.row(23) = H_rotation(2) * x; -} - -void DigitDynamicsConstraints::get_JTx_partial_dq(const VecX& q, const VecX& x) { - assert(x.size() == NUM_DEPENDENT_JOINTS); - JTx_partial_dq.resize(modelPtr_->nv, modelPtr_->nv); - JTx_partial_dq.setZero(); - - Eigen::Array H_rod_translation; - Eigen::Array H_anchor_translation; - - // left toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_A2"), - q, nullptr, - &left_toeA_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeA_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - JTx_partial_dq += (H_rod_translation(0) - H_anchor_translation(0)) * x(0); - JTx_partial_dq += (H_rod_translation(1) - H_anchor_translation(1)) * x(1); - JTx_partial_dq += (H_rod_translation(2) - H_anchor_translation(2)) * x(2); - - // left toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_B2"), - q, nullptr, - &left_toeB_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeB_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - JTx_partial_dq += (H_rod_translation(0) - H_anchor_translation(0)) * x(3); - JTx_partial_dq += (H_rod_translation(1) - H_anchor_translation(1)) * x(4); - JTx_partial_dq += (H_rod_translation(2) - H_anchor_translation(2)) * x(5); - - // left knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_ach2"), - q, nullptr, - &left_knee_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_tarsus"), - q, nullptr, - &left_knee_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - JTx_partial_dq += (H_rod_translation(0) - H_anchor_translation(0)) * x(6); - JTx_partial_dq += (H_rod_translation(1) - H_anchor_translation(1)) * x(7); - JTx_partial_dq += (H_rod_translation(2) - H_anchor_translation(2)) * x(8); - - // right toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_A2"), - q, nullptr, - &right_toeA_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeA_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - JTx_partial_dq += (H_rod_translation(0) - H_anchor_translation(0)) * x(9); - JTx_partial_dq += (H_rod_translation(1) - H_anchor_translation(1)) * x(10); - JTx_partial_dq += (H_rod_translation(2) - H_anchor_translation(2)) * x(11); - - // right toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_B2"), - q, nullptr, - &right_toeB_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeB_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - JTx_partial_dq += (H_rod_translation(0) - H_anchor_translation(0)) * x(12); - JTx_partial_dq += (H_rod_translation(1) - H_anchor_translation(1)) * x(13); - JTx_partial_dq += (H_rod_translation(2) - H_anchor_translation(2)) * x(14); - - // right knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_ach2"), - q, nullptr, - &right_knee_rod_endT, - 2); - fkPtr_->getTranslationHessian(H_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_tarsus"), - q, nullptr, - &right_knee_anchor_endT, - 2); - fkPtr_->getTranslationHessian(H_anchor_translation); - JTx_partial_dq += (H_rod_translation(0) - H_anchor_translation(0)) * x(15); - JTx_partial_dq += (H_rod_translation(1) - H_anchor_translation(1)) * x(16); - JTx_partial_dq += (H_rod_translation(2) - H_anchor_translation(2)) * x(17); - - // stance foot contact constraint - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 2); - - fkPtr_->getTranslationHessian(H_translation); - fkPtr_->getRPYHessian(H_rotation); - - JTx_partial_dq += H_translation(0) * x(18); - JTx_partial_dq += H_translation(1) * x(19); - JTx_partial_dq += H_translation(2) * x(20); - JTx_partial_dq += H_rotation(0) * x(21); - JTx_partial_dq += H_rotation(1) * x(22); - JTx_partial_dq += H_rotation(2) * x(23); -} - -void DigitDynamicsConstraints::get_Jxy_partial_dq(const VecX& q, const VecX& x, const VecX& y) { - assert(x.size() == modelPtr_->nv); - assert(y.size() == modelPtr_->nv); - Jxy_partial_dq.resize(NUM_DEPENDENT_JOINTS, modelPtr_->nv); - - Eigen::Array TOx_rod_translation; - Eigen::Array TOx_anchor_translation; - - // left toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_A2"), - q, nullptr, - &left_toeA_rod_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeA_anchor_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_anchor_translation); - Jxy_partial_dq.row(0) = (TOx_rod_translation(0) - TOx_anchor_translation(0)) * y; - Jxy_partial_dq.row(1) = (TOx_rod_translation(1) - TOx_anchor_translation(1)) * y; - Jxy_partial_dq.row(2) = (TOx_rod_translation(2) - TOx_anchor_translation(2)) * y; - - // left toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_B2"), - q, nullptr, - &left_toeB_rod_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_tarsus"), - modelPtr_->getJointId("left_toe_roll"), - q, nullptr, - &left_toeB_anchor_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_anchor_translation); - Jxy_partial_dq.row(3) = (TOx_rod_translation(0) - TOx_anchor_translation(0)) * y; - Jxy_partial_dq.row(4) = (TOx_rod_translation(1) - TOx_anchor_translation(1)) * y; - Jxy_partial_dq.row(5) = (TOx_rod_translation(2) - TOx_anchor_translation(2)) * y; - - // left knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_ach2"), - q, nullptr, - &left_knee_rod_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("left_hip_pitch"), - modelPtr_->getJointId("left_tarsus"), - q, nullptr, - &left_knee_anchor_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_anchor_translation); - Jxy_partial_dq.row(6) = (TOx_rod_translation(0) - TOx_anchor_translation(0)) * y; - Jxy_partial_dq.row(7) = (TOx_rod_translation(1) - TOx_anchor_translation(1)) * y; - Jxy_partial_dq.row(8) = (TOx_rod_translation(2) - TOx_anchor_translation(2)) * y; - - // right toe A closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_A2"), - q, nullptr, - &right_toeA_rod_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeA_anchor_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_anchor_translation); - Jxy_partial_dq.row(9) = (TOx_rod_translation(0) - TOx_anchor_translation(0)) * y; - Jxy_partial_dq.row(10) = (TOx_rod_translation(1) - TOx_anchor_translation(1)) * y; - Jxy_partial_dq.row(11) = (TOx_rod_translation(2) - TOx_anchor_translation(2)) * y; - - // right toe B closed loop - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_B2"), - q, nullptr, - &right_toeB_rod_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_tarsus"), - modelPtr_->getJointId("right_toe_roll"), - q, nullptr, - &right_toeB_anchor_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_anchor_translation); - Jxy_partial_dq.row(12) = (TOx_rod_translation(0) - TOx_anchor_translation(0)) * y; - Jxy_partial_dq.row(13) = (TOx_rod_translation(1) - TOx_anchor_translation(1)) * y; - Jxy_partial_dq.row(14) = (TOx_rod_translation(2) - TOx_anchor_translation(2)) * y; - - // right knee-tarsus closed loop - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_ach2"), - q, nullptr, - &right_knee_rod_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_rod_translation); - fkPtr_->compute(modelPtr_->getJointId("right_hip_pitch"), - modelPtr_->getJointId("right_tarsus"), - q, nullptr, - &right_knee_anchor_endT, - 3); - fkPtr_->getTranslationThirdOrderTensor(x, TOx_anchor_translation); - Jxy_partial_dq.row(15) = (TOx_rod_translation(0) - TOx_anchor_translation(0)) * y; - Jxy_partial_dq.row(16) = (TOx_rod_translation(1) - TOx_anchor_translation(1)) * y; - Jxy_partial_dq.row(17) = (TOx_rod_translation(2) - TOx_anchor_translation(2)) * y; - - // stance foot contact constraint - fkPtr_->compute(0, - contact_joint_id, - q, - nullptr, - &stance_foot_endT, - 3); - - fkPtr_->getTranslationThirdOrderTensor(x, TOx_translation); - fkPtr_->getRPYThirdOrderTensor(x, TOx_rotation); - - Jxy_partial_dq.row(18) = TOx_translation(0) * y; - Jxy_partial_dq.row(19) = TOx_translation(1) * y; - Jxy_partial_dq.row(20) = TOx_translation(2) * y; - Jxy_partial_dq.row(21) = TOx_rotation(0) * y; - Jxy_partial_dq.row(22) = TOx_rotation(1) * y; - Jxy_partial_dq.row(23) = TOx_rotation(2) * y; -} - -}; // namespace Digit -}; // namespace RAPTOR diff --git a/Examples/Digit/src/DigitMultipleStepOptimizer.cpp b/Examples/Digit/src/DigitMultipleStepOptimizer.cpp deleted file mode 100644 index 79c138e1..00000000 --- a/Examples/Digit/src/DigitMultipleStepOptimizer.cpp +++ /dev/null @@ -1,489 +0,0 @@ -#include "DigitMultipleStepOptimizer.h" - -namespace RAPTOR { -namespace Digit { - -// // constructor -// DigitMultipleStepOptimizer::DigitMultipleStepOptimizer() -// { -// } - - -// // destructor -// DigitMultipleStepOptimizer::~DigitMultipleStepOptimizer() -// { -// } - -bool DigitMultipleStepOptimizer::set_parameters( - const int NSteps_input, - const VecX& x0_input, - const double T_input, - const int N_input, - const TimeDiscretization time_discretization_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::vector& gps_input -) -{ - if (gps_input.size() != NSteps_input) { - THROW_EXCEPTION(IpoptException, "*** Error wrong size of gps_input in set_parameters!"); - } - - x0 = x0_input; - - stepOptVec_.reserve(NSteps_input); - for (int i = 0; i < NSteps_input; i++) { - stepOptVec_.push_back(new DigitSingleStepOptimizer()); - - char stanceLeg = (i % 2 == 0) ? - 'L' : - 'R'; - Transform stance_foot_T_des = (i % 2 == 0) ? - Transform(3, -M_PI / 2) : - Transform(3, M_PI / 2); - - stepOptVec_[i]->set_parameters(x0_input, - T_input, - N_input, - time_discretization_input, - degree_input, - model_input, - jtype_input, - gps_input[i], - stanceLeg, - stance_foot_T_des, - false); - } - - const frictionParams FRICTION_PARAMS(MU, GAMMA, FOOT_WIDTH, FOOT_LENGTH); - - periodConsVec_.reserve(NSteps_input); - for (int i = 0; i < NSteps_input - 1; i++) { - periodConsVec_.push_back( - std::make_shared( - stepOptVec_[i]->trajPtr_, - stepOptVec_[i + 1]->trajPtr_, - stepOptVec_[i]->dcidPtr_, - stepOptVec_[i + 1]->dcidPtr_, - FRICTION_PARAMS - )); - } - - return true; -} -// [TNLP_set_parameters] - -// [TNLP_get_nlp_info] -bool DigitMultipleStepOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) -{ - numVars = 0; - numCons = 0; - nnz_jac_g = 0; - - n_local.resize(stepOptVec_.size()); - n_position.resize(stepOptVec_.size() + 1); - m_local.resize(2 * stepOptVec_.size() - 1); - m_position.resize(2 * stepOptVec_.size()); - - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - Index nnz_jac_g_local; - stepOptVec_[i]->get_nlp_info(n_local[i], m_local[i], nnz_jac_g_local, nnz_h_lag, index_style); - - n_position[i] = numVars; - m_position[i] = numCons; - - numVars += n_local[i]; - numCons += m_local[i]; - nnz_jac_g += nnz_jac_g_local; - } - - for ( Index i = 0; i < stepOptVec_.size() - 1; i++ ) { - m_position[stepOptVec_.size() + i] = numCons; - - m_local[stepOptVec_.size() + i] = periodConsVec_[i]->m; - numCons += periodConsVec_[i]->m; - nnz_jac_g += periodConsVec_[i]->m * n_local[i] + - NUM_INDEPENDENT_JOINTS * n_local[i] + - NUM_INDEPENDENT_JOINTS * n_local[i + 1]; - } - m_position[2 * stepOptVec_.size() - 1] = numCons; - - n = numVars; - m = numCons; - - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - std::cout << "n = " << n << ", m = " << m << ", nnz_jac_g = " << nnz_jac_g << ", nnz_h_lag = " << nnz_h_lag << std::endl; - - return true; -} -// [TNLP_get_nlp_info] - -// [TNLP_get_bounds_info] -// returns the variable bounds -bool DigitMultipleStepOptimizer::get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u -) -{ - // here, the n and m we gave IPOPT in get_nlp_info are passed back to us. - // If desired, we could assert to make sure they are what we think they are. - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in get_bounds_info!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in get_bounds_info!"); - } - - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - std::cout << "gait " << i << " bounds infomation:" << std::endl; - stepOptVec_[i]->get_bounds_info(n_local[i], x_l + n_position[i], x_u + n_position[i], - m_local[i], g_l + m_position[i], g_u + m_position[i]); - } - - for ( Index i = 0; i < stepOptVec_.size() - 1; i++ ) { - const Index start_pos = m_position[stepOptVec_.size() + i]; - const Index end_pos = m_position[stepOptVec_.size() + i + 1]; - - std::cout << "gait " << i << " - " << i + 1 << " continuous constraint: " - << periodConsVec_[i]->m - << " [" << start_pos << " " << end_pos << "]" << std::endl; - - periodConsVec_[i]->compute_bounds(); - - for ( Index j = start_pos; j < end_pos; j++ ) { - g_l[j] = periodConsVec_[i]->g_lb(j - start_pos); - g_u[j] = periodConsVec_[i]->g_ub(j - start_pos); - } - } - - return true; -} -// [TNLP_get_bounds_info] - -// [TNLP_eval_f] -// returns the value of the objective function -bool DigitMultipleStepOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - obj_value = 0; - - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - Number obj_value_local; - stepOptVec_[i]->eval_f(n_local[i], x + n_position[i], new_x, obj_value_local); - - obj_value += obj_value_local; - } - - obj_value /= stepOptVec_.size(); - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} -// [TNLP_eval_f] - -// [TNLP_eval_grad_f] -// return the gradient of the objective function grad_{x} f(x) -bool DigitMultipleStepOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - stepOptVec_[i]->eval_grad_f(n_local[i], x + n_position[i], new_x, grad_f + n_position[i]); - } - - for ( Index i = 0; i < n; i++) { - grad_f[i] /= stepOptVec_.size(); - } - - return true; -} -// [TNLP_eval_grad_f] - -// [TNLP_eval_g] -// return the value of the constraints: g(x) -bool DigitMultipleStepOptimizer::eval_g( - Index n, - const Number* x, - bool new_x, - Index m, - Number* g -) -{ - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_g!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in eval_g!"); - } - - ifFeasibleCurrIter = true; - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - stepOptVec_[i]->eval_g(n_local[i], x + n_position[i], new_x, m_local[i], g + m_position[i]); - ifFeasibleCurrIter = ifFeasibleCurrIter & stepOptVec_[i]->ifFeasibleCurrIter; - } - - for ( Index i = 0; i < stepOptVec_.size() - 1; i++ ) { - VecX z_curr = Utils::initializeEigenVectorFromArray(x + n_position[i], n_local[i]); - periodConsVec_[i]->compute(z_curr, false); - - const Index start_pos = m_position[stepOptVec_.size() + i]; - for ( Index j = 0; j < periodConsVec_[i]->m; j++ ) { - g[start_pos + j] = periodConsVec_[i]->g(j); - - if (g[start_pos + j] < periodConsVec_[i]->g_lb(j) - constr_viol_tol || - g[start_pos + j] > periodConsVec_[i]->g_ub(j) + constr_viol_tol) { - ifFeasibleCurrIter = false; - } - } - } - - // the following code is for updating the optimal solution, directly copied from Optimizer.cpp - VecX z = Utils::initializeEigenVectorFromArray(x, n); - // update status of the current solution - if (new_x) { // directly assign currentIpoptSolution if this x has never been evaluated before - currentIpoptSolution = z; - currentIpoptObjValue = std::numeric_limits::max(); - ifCurrentIpoptFeasible = ifFeasibleCurrIter ? - OptimizerConstants::FeasibleState::FEASIBLE : - OptimizerConstants::FeasibleState::INFEASIBLE; - } - else { // update currentIpoptSolution - if (Utils::ifTwoVectorEqual(currentIpoptSolution, z, 0)) { - if (currentIpoptObjValue == std::numeric_limits::max()) { - THROW_EXCEPTION(IpoptException, "*** Error currentIpoptObjValue is not initialized!"); - } - else { // this has been evaluated in eval_f, just need to update the feasibility - ifCurrentIpoptFeasible = ifFeasibleCurrIter ? - OptimizerConstants::FeasibleState::FEASIBLE : - OptimizerConstants::FeasibleState::INFEASIBLE; - } - } - else { - currentIpoptSolution = z; - currentIpoptObjValue = std::numeric_limits::max(); - ifCurrentIpoptFeasible = ifFeasibleCurrIter ? - OptimizerConstants::FeasibleState::FEASIBLE : - OptimizerConstants::FeasibleState::INFEASIBLE; - } - } - - // update the status of the optimal solution - if (ifCurrentIpoptFeasible == OptimizerConstants::FeasibleState::FEASIBLE && - currentIpoptObjValue < optimalIpoptObjValue) { - optimalIpoptSolution = currentIpoptSolution; - optimalIpoptObjValue = currentIpoptObjValue; - ifOptimalIpoptFeasible = ifCurrentIpoptFeasible; - } - - return true; -} -// [TNLP_eval_g] - -// [TNLP_eval_jac_g] -// return the structure or values of the Jacobian -bool DigitMultipleStepOptimizer::eval_jac_g( - Index n, - const Number* x, - bool new_x, - Index m, - Index nele_jac, - Index* iRow, - Index* jCol, - Number* values -) -{ - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_jac_g!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in eval_jac_g!"); - } - - if( values == NULL ) { - // return the structure of the Jacobian - // this particular Jacobian is dense in blocks - Index idx = 0; - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - for ( Index j = 0; j < m_local[i]; j++ ) { - for ( Index k = 0; k < n_local[i]; k++ ) { - iRow[idx] = m_position[i] + j; - jCol[idx] = n_position[i] + k; - idx++; - } - } - } - - for ( Index i = 0; i < stepOptVec_.size() - 1; i++ ) { - const Index start_pos = m_position[stepOptVec_.size() + i]; - const Index end_pos = m_position[stepOptVec_.size() + i + 1]; - - for ( Index j = 0; j < periodConsVec_[i]->m; j++ ) { - for ( Index k = 0; k < n_local[i]; k++ ) { - iRow[idx] = start_pos + j; - jCol[idx] = n_position[i] + k; - idx++; - } - } - - for ( Index j = 0; j < NUM_INDEPENDENT_JOINTS; j++ ) { - for ( Index k = 0; k < n_local[i + 1]; k++ ) { - iRow[idx] = start_pos + NUM_JOINTS + NUM_DEPENDENT_JOINTS + j; - jCol[idx] = n_position[i + 1] + k; - idx++; - } - } - - for ( Index j = 0; j < NUM_INDEPENDENT_JOINTS; j++ ) { - for ( Index k = 0; k < n_local[i + 1]; k++ ) { - iRow[idx] = start_pos + NUM_JOINTS + NUM_DEPENDENT_JOINTS + NUM_INDEPENDENT_JOINTS + j; - jCol[idx] = n_position[i + 1] + k; - idx++; - } - } - } - } - else { - Index idx = 0; - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - stepOptVec_[i]->eval_jac_g(n_local[i], - x + n_position[i], - new_x, - m_local[i], - n_local[i] * m_local[i], - nullptr, - nullptr, - values + idx); - - idx += n_local[i] * m_local[i]; - } - - for ( Index i = 0; i < stepOptVec_.size() - 1; i++ ) { - VecX z_curr = Utils::initializeEigenVectorFromArray(x + n_position[i], n_local[i]); - periodConsVec_[i]->compute(z_curr, true); - - for ( Index j = 0; j < periodConsVec_[i]->m; j++ ) { - for ( Index k = 0; k < n_local[i]; k++ ) { - values[idx] = periodConsVec_[i]->pg_pz(j, k); - idx++; - } - } - - for ( Index j = 0; j < NUM_INDEPENDENT_JOINTS; j++ ) { - for ( Index k = 0; k < n_local[i + 1]; k++ ) { - values[idx] = periodConsVec_[i]->pg3_pz2(j, k); - idx++; - } - } - - for ( Index j = 0; j < NUM_INDEPENDENT_JOINTS; j++ ) { - for ( Index k = 0; k < n_local[i + 1]; k++ ) { - values[idx] = periodConsVec_[i]->pg4_pz2(j, k); - idx++; - } - } - } - } - - return true; -} -// [TNLP_eval_jac_g] - -// [TNLP_summarize_constraints] -void DigitMultipleStepOptimizer::summarize_constraints( - Index m, - const Number* g, - const bool verbose -) -{ - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in summarize_constraints!"); - } - - if (verbose) std::cout << "Constraint violation report:" << std::endl; - - ifFeasible = true; - final_constr_violation = 0; - - for ( Index i = 0; i < stepOptVec_.size(); i++ ) { - if (verbose) std::cout << "gait " << i << " "; - stepOptVec_[i]->summarize_constraints(m_local[i], g + m_position[i], verbose); - - ifFeasible = ifFeasible & stepOptVec_[i]->ifFeasible; - final_constr_violation = std::max(final_constr_violation, stepOptVec_[i]->final_constr_violation); - } - - for ( Index i = 0; i < stepOptVec_.size() - 1; i++ ) { - VecX z_curr = solution.segment(n_position[i], n_local[i]); - periodConsVec_[i]->compute(z_curr, false); - - Number max_constr_violation = 0; - Index max_constr_violation_id = 0; - const Index start_pos = m_position[stepOptVec_.size() + i]; - for ( Index j = 0; j < periodConsVec_[i]->m; j++ ) { - Number constr_violation = std::max(periodConsVec_[i]->g_lb(j) - periodConsVec_[i]->g(j), - periodConsVec_[i]->g(j) - periodConsVec_[i]->g_ub(j)); - - if (constr_violation > max_constr_violation) { - max_constr_violation_id = j; - max_constr_violation = constr_violation; - } - - if (constr_violation > final_constr_violation) { - final_constr_violation = constr_violation; - } - } - - if (max_constr_violation > constr_viol_tol) { - if (verbose) { - std::cout << "gait " << i << " - " << i + 1 << " continuous constraint: " - << max_constr_violation << std::endl; - std::cout << " range: [" << periodConsVec_[i]->g_lb[max_constr_violation_id] - << ", " - << periodConsVec_[i]->g_ub[max_constr_violation_id] - << "], value: " << periodConsVec_[i]->g(max_constr_violation_id) << std::endl; - } - - if (verbose) periodConsVec_[i]->print_violation_info(); - - ifFeasible = false; - } - } - - if (verbose) std::cout << "Total constraint violation: " << final_constr_violation << std::endl; -} -// [TNLP_summarize_constraints] - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/src/DigitMultipleStepPeriodicityConstraints.cpp b/Examples/Digit/src/DigitMultipleStepPeriodicityConstraints.cpp deleted file mode 100644 index 3ff74d19..00000000 --- a/Examples/Digit/src/DigitMultipleStepPeriodicityConstraints.cpp +++ /dev/null @@ -1,316 +0,0 @@ -#include "DigitMultipleStepPeriodicityConstraints.h" - -namespace RAPTOR { -namespace Digit { - -DigitMultipleStepPeriodicityConstraints::DigitMultipleStepPeriodicityConstraints(std::shared_ptr& currTrajPtr_input, - std::shared_ptr& nextTrajPtr_input, - std::shared_ptr currDcidPtr_input, - std::shared_ptr nextDcidPtr_input, - const frictionParams& fp_input) : - currTrajPtr_(currTrajPtr_input), - nextTrajPtr_(nextTrajPtr_input), - currDcidPtr_(currDcidPtr_input), - nextDcidPtr_(nextDcidPtr_input), - fp(fp_input) { - if (currTrajPtr_->varLength != nextTrajPtr_->varLength) { - throw std::invalid_argument("DigitMultipleStepPeriodicityConstraints: currTrajPtr_ and nextTrajPtr_ should have the same varLength"); - } - - m = NUM_JOINTS + // H * (v+ - v-) = J * lambda - NUM_DEPENDENT_JOINTS + // J*v+ = 0 - NUM_INDEPENDENT_JOINTS + // position reset - NUM_INDEPENDENT_JOINTS + // velocity reset - 7; // lambda contact constraints - - initialize_memory(m, currTrajPtr_->varLength, false); - - // initialize intermediate variables - const Model& model = *(currDcidPtr_->modelPtr_); - prnea_pq = MatX::Zero(model.nv, model.nv); - prnea_pv = MatX::Zero(model.nv, model.nv); - prnea_pa = MatX::Zero(model.nv, model.nv); - zeroVec = VecX::Zero(model.nv); - - g1 = VecX::Zero(NUM_JOINTS); - g2 = VecX::Zero(NUM_DEPENDENT_JOINTS); - g3 = VecX::Zero(NUM_INDEPENDENT_JOINTS); - g4 = VecX::Zero(NUM_INDEPENDENT_JOINTS); - g5 = VecX::Zero(7); - - pg1_pz = MatX::Zero(NUM_JOINTS, currTrajPtr_->varLength); - pg2_pz = MatX::Zero(NUM_DEPENDENT_JOINTS, currTrajPtr_->varLength); - pg3_pz = MatX::Zero(NUM_INDEPENDENT_JOINTS, currTrajPtr_->varLength); - pg3_pz2 = MatX::Zero(NUM_INDEPENDENT_JOINTS, nextTrajPtr_->varLength); - pg4_pz = MatX::Zero(NUM_INDEPENDENT_JOINTS, currTrajPtr_->varLength); - pg4_pz2 = MatX::Zero(NUM_INDEPENDENT_JOINTS, nextTrajPtr_->varLength); - pg5_pz = MatX::Zero(7, currTrajPtr_->varLength); - - pv_plus_pz = MatX::Zero(NUM_JOINTS, currTrajPtr_->varLength); - for (int i = 0; i < NUM_JOINTS; i++) { - pv_plus_pz(i, currTrajPtr_->varLength - NUM_JOINTS - NUM_DEPENDENT_JOINTS + i) = 1; - } - - plambda_pz = MatX::Zero(NUM_DEPENDENT_JOINTS, currTrajPtr_->varLength); - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - plambda_pz(i, currTrajPtr_->varLength - NUM_DEPENDENT_JOINTS + i) = 1; - } -} - -void DigitMultipleStepPeriodicityConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (compute_hessian) { - throw std::invalid_argument("DigitMultipleStepPeriodicityConstraints does not support hessian computation"); - } - - Model& model = *(currDcidPtr_->modelPtr_); - Data& data = *(currDcidPtr_->dataPtr_); - std::shared_ptr nextDdcPtr_ = nextDcidPtr_->ddcPtr_; - - const int lastIdx = currTrajPtr_->N - 1; - - const VecX& q_minus = currDcidPtr_->q(lastIdx); - const MatX& pq_minus_pz = currDcidPtr_->pq_pz(lastIdx); - - const VecX& v_minus = currDcidPtr_->v(lastIdx); - const MatX& pv_minus_pz = currDcidPtr_->pv_pz(lastIdx); - - const VecX& v_plus = z.segment(currTrajPtr_->varLength - NUM_JOINTS - NUM_DEPENDENT_JOINTS, NUM_JOINTS); - // MatX pv_plus_pz is constant and has been defined in the constructor - - const VecX& q_next = nextDcidPtr_->q(0); - const MatX& pq_next_pz2 = nextDcidPtr_->pq_pz(0); - - const VecX& v_next = nextDcidPtr_->v(0); - const MatX& pv_next_pz2 = nextDcidPtr_->pv_pz(0); - - const VecX& lambda = z.tail(NUM_DEPENDENT_JOINTS); - - // evaluate constraint jacobian using next step - nextDdcPtr_->get_c(q_minus); - nextDdcPtr_->get_J(q_minus); - - // (1) H * (v+ - v-) = J * lambda - crba(model, data, q_minus); - - MatX H = data.M; - for (size_t j = 0; j < model.nv; j++) { - for (size_t k = j + 1; k < model.nv; k++) { - H(k, j) = H(j, k); - } - } - - g1 = H * (v_plus - v_minus) - nextDdcPtr_->J.transpose() * lambda; - - if (compute_derivatives) { - // compute derivatives with gravity turned off, we just need prnea_pq here - const double original_gravity = model.gravity.linear()(2); - model.gravity.linear()(2) = 0; - pinocchio::computeRNEADerivatives(model, data, - q_minus, zeroVec, v_plus - v_minus, - prnea_pq, prnea_pv, prnea_pa); - - // restore gravity - model.gravity.linear()(2) = original_gravity; - - nextDdcPtr_->get_JTx_partial_dq(q_minus, lambda); - const MatX& JTx_partial_dq = nextDdcPtr_->JTx_partial_dq; - - pg1_pz = (prnea_pq - JTx_partial_dq) * pq_minus_pz + - H * (pv_plus_pz - pv_minus_pz) - - nextDdcPtr_->J.transpose() * plambda_pz; - } - - // (2) J * v+ = 0 - g2 = nextDdcPtr_->J * v_plus; - - if (compute_derivatives) { - nextDdcPtr_->get_Jx_partial_dq(q_minus, v_plus); - const MatX& Jx_partial_dq = nextDdcPtr_->Jx_partial_dq; - - pg2_pz = Jx_partial_dq * pq_minus_pz + - nextDdcPtr_->J * pv_plus_pz; - } - - // (3) position reset - g3 = nextDdcPtr_->get_independent_vector(q_next) - - nextDdcPtr_->get_independent_vector(q_minus); - - if (compute_derivatives) { - nextDdcPtr_->get_independent_rows(pg3_pz2, pq_next_pz2); - nextDdcPtr_->get_independent_rows(pg3_pz, pq_minus_pz); - pg3_pz = -pg3_pz; - } - - // (4) velocity reset - g4 = nextDdcPtr_->get_independent_vector(v_next) - - nextDdcPtr_->get_independent_vector(v_plus); - - if (compute_derivatives) { - nextDdcPtr_->get_independent_rows(pg4_pz2, pv_next_pz2); - nextDdcPtr_->get_independent_rows(pg4_pz, pv_plus_pz); - pg4_pz = -pg4_pz; - } - - // (5) contact constraints - VecX contactLambda = lambda.tail(6); - - double contact_force = contactLambda(2); - double friction_force_sq = pow(contactLambda(0), 2) + pow(contactLambda(1), 2); - double max_friction_force_sq = pow(fp.mu * contact_force, 2); - double max_moment_z_sq = pow(fp.gamma * contact_force, 2); - double mx_lower_limit = -fp.Lx * contact_force; - double mx_upper_limit = fp.Lx * contact_force; - double my_lower_limit = -fp.Ly * contact_force; - double my_upper_limit = fp.Ly * contact_force; - - // (1) positive contact force - g5(0) = contact_force; - - // (2) translation friction cone - g5(1) = friction_force_sq - max_friction_force_sq; - - // (3) rotation friction cone - g5(2) = pow(contactLambda(5), 2) - max_moment_z_sq; - - // (4, 5) ZMP on one axis - g5(3) = contactLambda(3) - mx_upper_limit; - g5(4) = mx_lower_limit - contactLambda(3); - - // (6, 7) ZMP on the other axis - g5(5) = contactLambda(4) - my_upper_limit; - g5(6) = my_lower_limit - contactLambda(4); - - if (compute_derivatives) { - // assume the contact wrench is always located at the end - MatX pcontactLambda_pz = plambda_pz.bottomRows(6); - - // (1) positive contact force - pg5_pz.row(0) = pcontactLambda_pz.row(2); - - // (2) translation friction cone - pg5_pz.row(1) = 2 * contactLambda(0) * pcontactLambda_pz.row(0) + - 2 * contactLambda(1) * pcontactLambda_pz.row(1) - - 2 * pow(fp.mu, 2) * contact_force * pcontactLambda_pz.row(2); - - // (3) rotation friction cone - pg5_pz.row(2) = 2 * contactLambda(5) * pcontactLambda_pz.row(5) - - 2 * pow(fp.gamma, 2) * contact_force * pcontactLambda_pz.row(2);; - - // (4, 5) ZMP on one axis - pg5_pz.row(3) = pcontactLambda_pz.row(3) - fp.Lx * pcontactLambda_pz.row(2); - pg5_pz.row(4) = -fp.Lx * pcontactLambda_pz.row(2) - pcontactLambda_pz.row(3); - - // (6, 7) ZMP on the other axis - pg5_pz.row(5) = pcontactLambda_pz.row(4) - fp.Ly * pcontactLambda_pz.row(2); - pg5_pz.row(6) = -fp.Ly * pcontactLambda_pz.row(2) - pcontactLambda_pz.row(4); - } - - // combine all constraints together - g << g1, g2, g3, g4, g5; - - if (compute_derivatives) { - pg_pz << pg1_pz, pg2_pz, pg3_pz, pg4_pz, pg5_pz; - - // pg3_pz2, pg4_pz2 is for the next walking step - // they are directly extracted from this class and then composed to the values passed to ipopt - } -} - -void DigitMultipleStepPeriodicityConstraints::compute_bounds() { - // everything before this are equality constraints, so just zeros - // and g_lb, g_ub are already initialized to zeros in the constructor - - // g_lb(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS) = 0; - g_ub(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS) = 1e19; - - g_lb.segment(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS + 1, 6).setConstant(-1e19); - // g_ub.segment(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS + 1, 6).setZero(); -} - -void DigitMultipleStepPeriodicityConstraints::print_violation_info() { - // (1) H * (v+ - v-) = J * lambda - for (int i = 0; i < NUM_JOINTS; i++) { - if (abs(g1(i)) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: H * (v+ - v-) = J * lambda: dim " - << i - << " is violated: " - << g(i) - << std::endl; - } - } - - // (2) J * v+ = 0 - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - if (abs(g2(i)) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: J * v+ = 0: dim " - << i - << " is violated: " - << g2(i) - << std::endl; - } - } - - // (3) position reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (abs(g3(i)) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: position reset: dim " - << i - << " is violated: " - << g3(i) - << std::endl; - } - } - - // (4) velocity reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (abs(g4(i)) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: velocity reset: dim " - << i - << " is violated: " - << g4(i) - << std::endl; - } - } - - // (5) contact constraints - if (g5(0) <= -1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: positive contact force is violated: " - << g5(0) - << std::endl; - } - if (g5(1) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: translation friction cone is violated: " - << g5(1) - << std::endl; - } - if (g5(2) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: rotation friction cone is violated: " - << g5(2) - << std::endl; - } - if (g5(3) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: ZMP on one axis is violated: " - << g5(3) - << std::endl; - } - if (g5(4) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: ZMP on one axis is violated: " - << g5(4) - << std::endl; - } - if (g5(5) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: ZMP on the other axis is violated: " - << g5(5) - << std::endl; - } - if (g5(6) >= 1e-4) { - std::cout << " DigitMultipleStepPeriodicityConstraints.cpp: ZMP on the other axis is violated: " - << g5(6) - << std::endl; - } -} - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/src/DigitSingleStepOptimizer.cpp b/Examples/Digit/src/DigitSingleStepOptimizer.cpp deleted file mode 100644 index f4272d95..00000000 --- a/Examples/Digit/src/DigitSingleStepOptimizer.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "DigitSingleStepOptimizer.h" - -namespace RAPTOR { -namespace Digit { - -// // constructor -// DigitSingleStepOptimizer::DigitSingleStepOptimizer() -// { -// } - - -// // destructor -// DigitSingleStepOptimizer::~DigitSingleStepOptimizer() -// { -// } - -bool DigitSingleStepOptimizer::set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const TimeDiscretization time_discretization_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const GaitParameters& gp_input, - const char stanceLeg, - const Transform& stance_foot_T_des, - bool periodic - ) -{ - x0 = x0_input; - - trajPtr_ = std::make_shared(T_input, - N_input, - NUM_INDEPENDENT_JOINTS, - time_discretization_input, - degree_input); - // add v_reset and lambda_reset to the end of the decision variables - trajPtr_->varLength += NUM_JOINTS + NUM_DEPENDENT_JOINTS; - - dcidPtr_ = std::make_shared(model_input, - trajPtr_, - NUM_DEPENDENT_JOINTS, - jtype_input, - stanceLeg, - stance_foot_T_des); - cidPtr_ = dcidPtr_; // convert to base class - - // convert joint limits from degree to radian - VecX JOINT_LIMITS_LOWER_VEC(NUM_JOINTS); - for (int i = 0; i < NUM_JOINTS; i++) { - JOINT_LIMITS_LOWER_VEC(i) = Utils::deg2rad(JOINT_LIMITS_LOWER[i]); - } - - // convert joint limits from degree to radian - VecX JOINT_LIMITS_UPPER_VEC(NUM_JOINTS); - for (int i = 0; i < NUM_JOINTS; i++) { - JOINT_LIMITS_UPPER_VEC(i) = Utils::deg2rad(JOINT_LIMITS_UPPER[i]); - } - - VecX TORQUE_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_LOWER, NUM_INDEPENDENT_JOINTS); - VecX TORQUE_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_UPPER, NUM_INDEPENDENT_JOINTS); - - constraintsPtrVec_.clear(); - - // Torque limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - cidPtr_, - TORQUE_LIMITS_LOWER_VEC, - TORQUE_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("torque limits"); - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - cidPtr_->dcPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // Surface contact constraints - const frictionParams FRICTION_PARAMS(MU, GAMMA, FOOT_WIDTH, FOOT_LENGTH); - constraintsPtrVec_.push_back(std::make_unique(cidPtr_, - FRICTION_PARAMS)); - constraintsNameVec_.push_back("contact constraints"); - - // kinematics constraints - constraintsPtrVec_.push_back(std::make_unique(model_input, - jtype_input, - trajPtr_, - dcidPtr_->ddcPtr_, - gp_input)); - constraintsNameVec_.push_back("customized constraints"); - - // periodic reset map constraints - if (periodic) { - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - dcidPtr_, - FRICTION_PARAMS)); - constraintsNameVec_.push_back("reset map constraints"); - } - - return true; -} -// [TNLP_set_parameters] - -// [TNLP_get_nlp_info] -bool DigitSingleStepOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) -{ - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} -// [TNLP_get_nlp_info] - -// [TNLP_eval_f] -// returns the value of the objective function -bool DigitSingleStepOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - cidPtr_->compute(z, false); - - obj_value = 0; - - // minimize control torque - for ( Index i = 0; i < cidPtr_->N; i++ ) { - obj_value += sqrt(cidPtr_->tau(i).dot(cidPtr_->tau(i))); - } - obj_value /= cidPtr_->N; - - // minimize initial velocity - const VecX& initial_velocity = cidPtr_->trajPtr_->q_d(0); - obj_value += 40 * sqrt(initial_velocity.dot(initial_velocity)); - - // minimize initial acceleration - const VecX& initial_acceleration = cidPtr_->trajPtr_->q_dd(0); - obj_value += 20 * sqrt(initial_acceleration.dot(initial_acceleration)); - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} -// [TNLP_eval_f] - -// [TNLP_eval_grad_f] -// return the gradient of the objective function grad_{x} f(x) -bool DigitSingleStepOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - cidPtr_->compute(z, true); - - for ( Index i = 0; i < n; i++ ) { - grad_f[i] = 0; - } - - for ( Index i = 0; i < cidPtr_->N; i++ ) { - VecX v = cidPtr_->ptau_pz(i).transpose() * cidPtr_->tau(i); - double norm = sqrt(cidPtr_->tau(i).dot(cidPtr_->tau(i))); - - for ( Index j = 0; j < n; j++ ) { - grad_f[j] += v(j) / norm; - } - } - for ( Index i = 0; i < n; i++ ) { - grad_f[i] /= cidPtr_->N; - } - - const VecX& initial_velocity = cidPtr_->trajPtr_->q_d(0); - const VecX& initial_velocity_pz = cidPtr_->trajPtr_->pq_d_pz(0).transpose() * initial_velocity; - const double initial_velocity_norm = sqrt(initial_velocity.dot(initial_velocity)); - for ( Index i = 0; i < n; i++ ) { - grad_f[i] += 40 * initial_velocity_pz(i) / initial_velocity_norm; - } - - const VecX& initial_acceleration = cidPtr_->trajPtr_->q_dd(0); - const VecX& initial_acceleration_pz = cidPtr_->trajPtr_->pq_dd_pz(0).transpose() * initial_acceleration; - const double initial_acceleration_norm = sqrt(initial_acceleration.dot(initial_acceleration)); - for ( Index i = 0; i < n; i++ ) { - grad_f[i] += 20 * initial_acceleration_pz(i) / initial_acceleration_norm; - } - - return true; -} -// [TNLP_eval_grad_f] - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Digit/src/DigitSingleStepPeriodicityConstraints.cpp b/Examples/Digit/src/DigitSingleStepPeriodicityConstraints.cpp deleted file mode 100644 index ede4a81e..00000000 --- a/Examples/Digit/src/DigitSingleStepPeriodicityConstraints.cpp +++ /dev/null @@ -1,312 +0,0 @@ -#include "DigitSingleStepPeriodicityConstraints.h" - -namespace RAPTOR { -namespace Digit { - -DigitSingleStepPeriodicityConstraints::DigitSingleStepPeriodicityConstraints(std::shared_ptr& trajPtr_input, - std::shared_ptr dcidPtr_input, - const frictionParams& fp_input) : - trajPtr_(trajPtr_input), - dcidPtr_(dcidPtr_input), - fp(fp_input) { - m = NUM_JOINTS + // H * (v+ - v-) = J * lambda - NUM_DEPENDENT_JOINTS + // J*v+ = 0 - NUM_INDEPENDENT_JOINTS + // position reset - NUM_INDEPENDENT_JOINTS + // velocity reset - 7; // lambda contact constraints - g = VecX::Zero(m); - g_lb = VecX::Zero(m); - g_ub = VecX::Zero(m); - pg_pz.resize(m, trajPtr_->varLength); - - // initialize intermediate variables - prnea_pq = MatX::Zero(dcidPtr_->modelPtr_->nv, dcidPtr_->modelPtr_->nv); - prnea_pv = MatX::Zero(dcidPtr_->modelPtr_->nv, dcidPtr_->modelPtr_->nv); - prnea_pa = MatX::Zero(dcidPtr_->modelPtr_->nv, dcidPtr_->modelPtr_->nv); - zeroVec = VecX::Zero(dcidPtr_->modelPtr_->nv); - - g1 = VecX::Zero(NUM_JOINTS); - g2 = VecX::Zero(NUM_DEPENDENT_JOINTS); - g3 = VecX::Zero(NUM_INDEPENDENT_JOINTS); - g4 = VecX::Zero(NUM_INDEPENDENT_JOINTS); - g5 = VecX::Zero(7); - - pg1_pz = MatX::Zero(NUM_JOINTS, trajPtr_->varLength); - pg2_pz = MatX::Zero(NUM_DEPENDENT_JOINTS, trajPtr_->varLength); - pg3_pz = MatX::Zero(NUM_INDEPENDENT_JOINTS, trajPtr_->varLength); - pg4_pz = MatX::Zero(NUM_INDEPENDENT_JOINTS, trajPtr_->varLength); - pg5_pz = MatX::Zero(7, trajPtr_->varLength); - - // the following are going to be constant - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - joint_id1[i] = dcidPtr_->modelPtr_->getJointId(JOINT_MAP[i][0]) - 1; - joint_id2[i] = dcidPtr_->modelPtr_->getJointId(JOINT_MAP[i][1]) - 1; - } - - pv_plus_pz = MatX::Zero(NUM_JOINTS, trajPtr_->varLength); - for (int i = 0; i < NUM_JOINTS; i++) { - pv_plus_pz(i, trajPtr_->varLength - NUM_JOINTS - NUM_DEPENDENT_JOINTS + i) = 1; - } - - plambda_pz = MatX::Zero(NUM_DEPENDENT_JOINTS, trajPtr_->varLength); - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - plambda_pz(i, trajPtr_->varLength - NUM_DEPENDENT_JOINTS + i) = 1; - } -} - -void DigitSingleStepPeriodicityConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (compute_hessian) { - throw std::invalid_argument("DigitSingleStepPeriodicityConstraints does not support hessian computation"); - } - - // We assume that surface contact constraints always come after torque limits constraints for now - // The following line has been called in TorqueLimits::compute() already - // So we directly pull out the lambda values from idPtr_ - // dcidPtr_->compute(z, compute_derivatives); - - const int lastIdx = trajPtr_->N - 1; - - const VecX& q_minus = dcidPtr_->q(lastIdx); - const VecX& v_minus = dcidPtr_->v(lastIdx); - - const VecX& v_plus = z.block(trajPtr_->varLength - NUM_JOINTS - NUM_DEPENDENT_JOINTS, 0, NUM_JOINTS, 1); - - const VecX& q_0 = dcidPtr_->q(0); - const VecX& v_0 = dcidPtr_->v(0); - - const VecX& lambda = z.tail(NUM_DEPENDENT_JOINTS); - - // swap stance leg for reset map - dcidPtr_->ddcPtr_->reinitialize(); - - // re-evaluate constraint jacobian - dcidPtr_->dcPtr_->get_c(q_minus); - dcidPtr_->dcPtr_->get_J(q_minus); - - // (1) H * (v+ - v-) = J * lambda - crba(*(dcidPtr_->modelPtr_), *(dcidPtr_->dataPtr_), q_minus); - - MatX H = dcidPtr_->dataPtr_->M; - for (size_t j = 0; j < dcidPtr_->modelPtr_->nv; j++) { - for (size_t k = j + 1; k < dcidPtr_->modelPtr_->nv; k++) { - H(k, j) = H(j, k); - } - } - - g1 = H * (v_plus - v_minus) - dcidPtr_->dcPtr_->J.transpose() * lambda; - - if (compute_derivatives) { - // compute derivatives with gravity turned off, we just need prnea_pq here - const double original_gravity = dcidPtr_->modelPtr_->gravity.linear()(2); - dcidPtr_->modelPtr_->gravity.linear()(2) = 0; - pinocchio::computeRNEADerivatives(*(dcidPtr_->modelPtr_), *(dcidPtr_->dataPtr_), - q_minus, zeroVec, v_plus - v_minus, - prnea_pq, prnea_pv, prnea_pa); - - // restore gravity - dcidPtr_->modelPtr_->gravity.linear()(2) = original_gravity; - - dcidPtr_->dcPtr_->get_JTx_partial_dq(q_minus, lambda); - const MatX& JTx_partial_dq = dcidPtr_->dcPtr_->JTx_partial_dq; - - pg1_pz = (prnea_pq - JTx_partial_dq) * dcidPtr_->pq_pz(lastIdx) + - H * (pv_plus_pz - dcidPtr_->pv_pz(lastIdx)) - - dcidPtr_->dcPtr_->J.transpose() * plambda_pz; - } - - // (2) J * v+ = 0 - g2 = dcidPtr_->dcPtr_->J * v_plus; - - if (compute_derivatives) { - dcidPtr_->dcPtr_->get_Jx_partial_dq(q_minus, v_plus); - const MatX& Jx_partial_dq = dcidPtr_->dcPtr_->Jx_partial_dq; - - pg2_pz = Jx_partial_dq * dcidPtr_->pq_pz(lastIdx) + - dcidPtr_->dcPtr_->J * pv_plus_pz; - } - - // (3) position reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - g3(i) = q_0(joint_id1[i]) + q_minus(joint_id2[i]); - } - - if (compute_derivatives) { - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - pg3_pz.row(i) = dcidPtr_->pq_pz(0).row(joint_id1[i]) + dcidPtr_->pq_pz(lastIdx).row(joint_id2[i]); - } - } - - // (4) velocity reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - g4(i) = v_0(joint_id1[i]) + v_plus(joint_id2[i]); - } - - if (compute_derivatives) { - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - pg4_pz.row(i) = dcidPtr_->pv_pz(0).row(joint_id1[i]) + pv_plus_pz.row(joint_id2[i]); - } - } - - // (5) contact constraints - VecX contactLambda = lambda.tail(6); - - double contact_force = contactLambda(2); - double friction_force_sq = pow(contactLambda(0), 2) + pow(contactLambda(1), 2); - double max_friction_force_sq = pow(fp.mu * contact_force, 2); - double max_moment_z_sq = pow(fp.gamma * contact_force, 2); - double mx_lower_limit = -fp.Lx * contact_force; - double mx_upper_limit = fp.Lx * contact_force; - double my_lower_limit = -fp.Ly * contact_force; - double my_upper_limit = fp.Ly * contact_force; - - // (1) positive contact force - g5(0) = contact_force; - - // (2) translation friction cone - g5(1) = friction_force_sq - max_friction_force_sq; - - // (3) rotation friction cone - g5(2) = pow(contactLambda(5), 2) - max_moment_z_sq; - - // (4, 5) ZMP on one axis - g5(3) = contactLambda(3) - mx_upper_limit; - g5(4) = mx_lower_limit - contactLambda(3); - - // (6, 7) ZMP on the other axis - g5(5) = contactLambda(4) - my_upper_limit; - g5(6) = my_lower_limit - contactLambda(4); - - if (compute_derivatives) { - // assume the contact wrench is always located at the end - MatX pcontactLambda_pz = plambda_pz.bottomRows(6); - - // (1) positive contact force - pg5_pz.row(0) = pcontactLambda_pz.row(2); - - // (2) translation friction cone - pg5_pz.row(1) = 2 * contactLambda(0) * pcontactLambda_pz.row(0) + - 2 * contactLambda(1) * pcontactLambda_pz.row(1) - - 2 * pow(fp.mu, 2) * contact_force * pcontactLambda_pz.row(2); - - // (3) rotation friction cone - pg5_pz.row(2) = 2 * contactLambda(5) * pcontactLambda_pz.row(5) - - 2 * pow(fp.gamma, 2) * contact_force * pcontactLambda_pz.row(2);; - - // (4, 5) ZMP on one axis - pg5_pz.row(3) = pcontactLambda_pz.row(3) - fp.Lx * pcontactLambda_pz.row(2); - pg5_pz.row(4) = -fp.Lx * pcontactLambda_pz.row(2) - pcontactLambda_pz.row(3); - - // (6, 7) ZMP on the other axis - pg5_pz.row(5) = pcontactLambda_pz.row(4) - fp.Ly * pcontactLambda_pz.row(2); - pg5_pz.row(6) = -fp.Ly * pcontactLambda_pz.row(2) - pcontactLambda_pz.row(4); - } - - // swap back for next round evaluation - dcidPtr_->ddcPtr_->reinitialize(); - - // combine all constraints together - g << g1, g2, g3, g4, g5; - - if (compute_derivatives) { - pg_pz << pg1_pz, pg2_pz, pg3_pz, pg4_pz, pg5_pz; - } -} - -void DigitSingleStepPeriodicityConstraints::compute_bounds() { - // everything before this are equality constraints, so just zeros - // and g_lb, g_ub are already initialized to zeros in the constructor - - // g_lb(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS) = 0; - g_ub(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS) = 1e19; - - g_lb.block(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS + 1, 0, 6, 1).setConstant(-1e19); - // g_ub.block(NUM_JOINTS + NUM_DEPENDENT_JOINTS + 2 * NUM_INDEPENDENT_JOINTS + 1, 0, 6, 1).setZero(); -} - -void DigitSingleStepPeriodicityConstraints::print_violation_info() { - // (1) H * (v+ - v-) = J * lambda - for (int i = 0; i < NUM_JOINTS; i++) { - if (abs(g1(i)) > 1e-5) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: H * (v+ - v-) = J * lambda: dim " - << i - << " is violated: " - << g(i) - << std::endl; - } - } - - // (2) J * v+ = 0 - for (int i = 0; i < NUM_DEPENDENT_JOINTS; i++) { - if (abs(g2(i)) > 1e-5) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: J * v+ = 0: dim " - << i - << " is violated: " - << g2(i) - << std::endl; - } - } - - // (3) position reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (abs(g3(i)) > 1e-5) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: position reset: dim " - << i - << " is violated: " - << g3(i) - << std::endl; - } - } - - // (4) velocity reset - for (int i = 0; i < NUM_INDEPENDENT_JOINTS; i++) { - if (abs(g4(i)) > 1e-5) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: velocity reset: dim " - << i - << " is violated: " - << g4(i) - << std::endl; - } - } - - // (5) contact constraints - if (g5(0) <= -1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: positive contact force is violated: " - << g5(0) - << std::endl; - } - if (g5(1) >= 1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: translation friction cone is violated: " - << g5(1) - << std::endl; - } - if (g5(2) >= 1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: rotation friction cone is violated: " - << g5(2) - << std::endl; - } - if (g5(3) >= 1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: ZMP on one axis is violated: " - << g5(3) - << std::endl; - } - if (g5(4) >= 1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: ZMP on one axis is violated: " - << g5(4) - << std::endl; - } - if (g5(5) >= 1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: ZMP on the other axis is violated: " - << g5(5) - << std::endl; - } - if (g5(6) >= 1e-4) { - std::cout << " DigitSingleStepPeriodicityConstraints.cpp: ZMP on the other axis is violated: " - << g5(6) - << std::endl; - } -} - -}; // namespace Digit -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/ArmourUnsafe/KinovaExample.cpp b/Examples/Kinova/ArmourUnsafe/KinovaExample.cpp deleted file mode 100644 index 3a25726c..00000000 --- a/Examples/Kinova/ArmourUnsafe/KinovaExample.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include "KinovaOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -using namespace RAPTOR; -using namespace Kinova; -using namespace Ipopt; - -int main() { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - const std::string urdf_filename = "../Examples/Kinova/ArmourUnsafe/kinova.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.81; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3; - - // Define obstacles - const int num_obstacles = 5; - std::vector boxCenters; - std::vector boxOrientation; - std::vector boxSize; - - boxCenters.resize(num_obstacles); - boxOrientation.resize(num_obstacles); - boxSize.resize(num_obstacles); - - for (int i = 0; i < num_obstacles; i++) { - boxCenters[i] << 10, 0, 0.3 * i; - boxOrientation[i] << 0, 0, 0; - boxSize[i] << 0.1, 0.1, 0.1; - } - - // Define trajectories - ArmourTrajectoryParameters atp; - atp.q0 = Eigen::VectorXd::Zero(model.nq); - atp.q_d0 = Eigen::VectorXd::Zero(model.nq); - atp.q_dd0 = Eigen::VectorXd::Zero(model.nq); - - const double T = 1; - const int N = 16; - const int degree = ARMOUR_BEZIER_CURVE_DEGREE; - - // Define target - Eigen::VectorXd qdes(model.nq); - qdes.setConstant(1.0); - const int tplan_n = N / 2; - - // Define initial guess - Eigen::VectorXd z(model.nq); - z.setRandom(); - - // Define limits buffer - Eigen::VectorXd joint_limits_buffer(model.nq); - joint_limits_buffer.setConstant(0.0); - Eigen::VectorXd velocity_limits_buffer(model.nq); - velocity_limits_buffer.setConstant(0.0); - Eigen::VectorXd torque_limits_buffer(model.nq); - torque_limits_buffer.setConstant(0.0); - - // Initialize Kinova optimizer - SmartPtr mynlp = new KinovaOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - degree, - model, - jtype, - atp, - boxCenters, - boxOrientation, - boxSize, - qdes, - tplan_n, - joint_limits_buffer, - velocity_limits_buffer, - torque_limits_buffer); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("tol", 1e-4); - app->Options()->SetNumericValue("constr_viol_tol", 1e-4); - // app->Options()->SetNumericValue("obj_scaling_factor", 1e-3); - app->Options()->SetNumericValue("max_wall_time", 0.2); - app->Options()->SetIntegerValue("print_level", 5); - app->Options()->SetIntegerValue("max_iter", 50); - app->Options()->SetStringValue("mu_strategy", "monotone"); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - - // For gradient checking - // app->Options()->SetStringValue("output_file", "ipopt.out"); - // app->Options()->SetStringValue("derivative_test", "second-order"); - // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - // app->Options()->SetNumericValue("derivative_test_tol", 1e-5); - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // // Print the solution - // if (mynlp->solution.size() == mynlp->numVars) { - // std::ofstream solution("solution-kinova.txt"); - // solution << std::setprecision(20); - // for (int i = 0; i < mynlp->numVars; i++) { - // solution << mynlp->solution[i] << std::endl; - // } - // solution.close(); - - // std::ofstream trajectory("trajectory-kinova.txt"); - // trajectory << std::setprecision(20); - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->trajPtr_->q(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->trajPtr_->q_d(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->trajPtr_->q_dd(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // for (int i = 0; i < NUM_JOINTS; i++) { - // for (int j = 0; j < N; j++) { - // trajectory << mynlp->idPtr_->tau(j)(i) << ' '; - // } - // trajectory << std::endl; - // } - // trajectory.close(); - // } - - return 0; -} diff --git a/Examples/Kinova/ArmourUnsafe/KinovaPybind.cpp b/Examples/Kinova/ArmourUnsafe/KinovaPybind.cpp deleted file mode 100644 index 9ed337e1..00000000 --- a/Examples/Kinova/ArmourUnsafe/KinovaPybind.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include -#include - -#include "KinovaPybindWrapper.h" - -namespace py = pybind11; - -using namespace RAPTOR; -using namespace Kinova; - -PYBIND11_MODULE(oracle_pybind, m) { - m.doc() = "pybind11 oracle_pybind plugin"; - - py::class_(m, "KinovaPybindWrapper") - .def(py::init()) - .def("set_obstacles", &KinovaPybindWrapper::set_obstacles) - .def("set_ipopt_parameters", &KinovaPybindWrapper::set_ipopt_parameters) - .def("set_trajectory_parameters", &KinovaPybindWrapper::set_trajectory_parameters) - .def("set_buffer", &KinovaPybindWrapper::set_buffer) - .def("set_target", &KinovaPybindWrapper::set_target) - .def("optimize", &KinovaPybindWrapper::optimize); -} \ No newline at end of file diff --git a/Examples/Kinova/ArmourUnsafe/README.md b/Examples/Kinova/ArmourUnsafe/README.md deleted file mode 100644 index 50261d4e..00000000 --- a/Examples/Kinova/ArmourUnsafe/README.md +++ /dev/null @@ -1 +0,0 @@ -This folder contains an implementation of an unsafe version of [ARMOUR](https://roahmlab.github.io/armour/). \ No newline at end of file diff --git a/Examples/Kinova/ArmourUnsafe/include/KinovaCustomizedConstraints.h b/Examples/Kinova/ArmourUnsafe/include/KinovaCustomizedConstraints.h deleted file mode 100644 index ef95ab5c..00000000 --- a/Examples/Kinova/ArmourUnsafe/include/KinovaCustomizedConstraints.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef KINOVA_CUSTOMIZED_CONSTRAINTS_H -#define KINOVA_CUSTOMIZED_CONSTRAINTS_H - -#include "Constraints.h" -#include "Trajectories.h" -#include "ForwardKinematics.h" -#include "BoxCollisionAvoidance.h" - -#include - -namespace RAPTOR { -namespace Kinova { - -/* -This class implements an example of customized constraints for Kinova. -It uses spheres to represent forward occupancy of the robot, -computes the distance between the robot and the customized obstacles, -and make sure the distance are larger than 0 to achieve collision avoidance. -*/ - -#define NUM_SPHERES 14 - -// based on which joint we compute the positions of the spheres -const int sphere_joint_id[NUM_SPHERES] = {2, 2, 2, 2, 2, 2, 2, // spheres on link 2 - 4, 4, 4, 4, 4, // spheres on link 4 - 6, 6}; // spheres on link 6 - -// the translation axis of the spheres to cover the links (they are all extended along the y axis) -const int sphere_offset_axis[NUM_SPHERES] = {5, 5, 5, 5, 5, 5, 5, // spheres on link 2 - 5, 5, 5, 5, 5, // spheres on link 4 - 5, 5}; // spheres on link 6 - -// the translation offset of the spheres to cover the links -const double sphere_offset[NUM_SPHERES] = {-0.05, -0.11, -0.17, -0.24, -0.30, -0.36, -0.43, // spheres on link 2 - -0.07, -0.14, -0.21, -0.28, -0.32, // spheres on link 4 - -0.07, -0.14}; // spheres on link 6 - -const double sphere_radius[NUM_SPHERES] = {0.06, 0.06, 0.06, 0.06, 0.06, 0.06, 0.08, // spheres on link 2 - 0.07, 0.06, 0.06, 0.06, 0.07, // spheres on link 4 - 0.05, 0.05}; // spheres on link 6 - -class KinovaCustomizedConstraints : public Constraints { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - KinovaCustomizedConstraints() = default; - - // Constructor - KinovaCustomizedConstraints(std::shared_ptr& trajPtr_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input); - - // Destructor - ~KinovaCustomizedConstraints() = default; - - // class methods: - // compute constraints - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // compute constraints lower bounds and upper bounds - void compute_bounds() override; - - // print violation information - void print_violation_info() override; - - // class variables: - std::shared_ptr& trajPtr_; - - std::shared_ptr collisionAvoidancePtr_; - - std::unique_ptr modelPtr_; - - std::unique_ptr fkPtr_; - - // jtype copy - Eigen::VectorXi jtype; - - // the transform matrix at the beginning and at the end - Transform startT; - Transform endT; - - // updated in compute() - Transform jointT; - MatX jointTJ; - MatX pq_pz; - - // forward kinematics derivatives - std::vector dTdq; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // KINOVA_CUSTOMIZED_CONSTRAINTS_H diff --git a/Examples/Kinova/ArmourUnsafe/include/KinovaOptimizer.h b/Examples/Kinova/ArmourUnsafe/include/KinovaOptimizer.h deleted file mode 100644 index cd14ef53..00000000 --- a/Examples/Kinova/ArmourUnsafe/include/KinovaOptimizer.h +++ /dev/null @@ -1,116 +0,0 @@ -#ifndef KINOVA_OPTIMIZER_H -#define KINOVA_OPTIMIZER_H - -#include "KinovaConstants.h" - -#include "Optimizer.h" - -#include "ArmourBezierCurves.h" - -#include "InverseDynamics.h" -#include "JointLimits.h" -#include "VelocityLimits.h" -#include "TorqueLimits.h" -#include "KinovaCustomizedConstraints.h" - -namespace RAPTOR { -namespace Kinova { - -class KinovaOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using Vec3 = Eigen::Vector3d; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - KinovaOptimizer() = default; - - /** Default destructor */ - ~KinovaOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const ArmourTrajectoryParameters& atp_input, - const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input, - const VecX& qdes_input, - const int tplan_n_input, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /** Method to return the hessian of the objective */ - bool eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - KinovaOptimizer( - const KinovaOptimizer& - ); - - KinovaOptimizer& operator=( - const KinovaOptimizer& - ); - - std::shared_ptr trajPtr_; - - std::shared_ptr idPtr_; - - VecX qdes; - int tplan_n = 0; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // KINOVA_OPTIMIZER_H diff --git a/Examples/Kinova/ArmourUnsafe/include/KinovaPybindWrapper.h b/Examples/Kinova/ArmourUnsafe/include/KinovaPybindWrapper.h deleted file mode 100644 index a2e69adb..00000000 --- a/Examples/Kinova/ArmourUnsafe/include/KinovaPybindWrapper.h +++ /dev/null @@ -1,97 +0,0 @@ -#ifndef KINOVA_PYBIND_WRAPPER_H -#define KINOVA_PYBIND_WRAPPER_H - -#include -#include - -#include "KinovaOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -namespace RAPTOR { -namespace Kinova { - -namespace py = pybind11; - -class KinovaPybindWrapper { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - KinovaPybindWrapper() = default; - - KinovaPybindWrapper(const std::string urdf_filename); - - // Destructor - ~KinovaPybindWrapper() = default; - - // Class methods - void set_obstacles(const int num_obstacles_inp, - const py::array_t obstacles_inp); - - void set_ipopt_parameters(const double tol, - const double obj_scaling_factor, - const double max_wall_time, - const int print_level, - const std::string mu_strategy, - const std::string linear_solver, - const bool gradient_check); - - void set_trajectory_parameters(const py::array_t q0_inp, - const py::array_t qd0_inp, - const py::array_t qdd0_inp, - const double duration_inp); - - void set_buffer(const py::array_t joint_limits_buffer_inp, - const py::array_t velocity_limits_buffer_inp, - const py::array_t torque_limits_buffer_inp); - - void set_target(const py::array_t q_des_inp, - const double tplan_inp); - - py::tuple optimize(); - - // Class members - // robot model - Model model; - Eigen::VectorXi jtype; - - // obstacle information - int num_obstacles = 0; - std::vector boxCenters; - std::vector boxOrientation; - std::vector boxSize; - - // trajectory information - ArmourTrajectoryParameters atp; - double T = 1; - int N = 16; - int degree = 5; - VecX qdes; - double tplan = 0; - int tplan_n = 0; - - // buffer information - VecX joint_limits_buffer; - VecX velocity_limits_buffer; - VecX torque_limits_buffer; - - SmartPtr mynlp; - SmartPtr app; - - // Flags to check if the parameters are set - bool set_obstacles_check = false; - bool set_ipopt_parameters_check = false; - bool set_trajectory_parameters_check = false; - bool set_buffer_check = false; - bool set_target_check = false; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // KINOVA_PYBIND_WRAPPER_H diff --git a/Examples/Kinova/ArmourUnsafe/kinova.urdf b/Examples/Kinova/ArmourUnsafe/kinova.urdf deleted file mode 100644 index 0aaf5f5c..00000000 --- a/Examples/Kinova/ArmourUnsafe/kinova.urdf +++ /dev/null @@ -1,336 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 8.03 - - - - - - 11.996202461530364 - - - - - - 9.002542786175152 - - - - - - 11.580643931670636 - - - - - - 8.466504091791412 - - - - - - 8.853706937374243 - - - - - - 8.858730366468532 - - diff --git a/Examples/Kinova/ArmourUnsafe/src/KinovaCustomizedConstraints.cpp b/Examples/Kinova/ArmourUnsafe/src/KinovaCustomizedConstraints.cpp deleted file mode 100644 index cfbafc49..00000000 --- a/Examples/Kinova/ArmourUnsafe/src/KinovaCustomizedConstraints.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "KinovaCustomizedConstraints.h" - -namespace RAPTOR { -namespace Kinova { - -KinovaCustomizedConstraints::KinovaCustomizedConstraints(std::shared_ptr& trajPtr_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input) : - trajPtr_(trajPtr_input), - jtype(jtype_input) { - modelPtr_ = std::make_unique(model_input); - fkPtr_ = std::make_unique(modelPtr_.get(), jtype); - collisionAvoidancePtr_ = std::make_shared(boxCenters_input, - boxOrientation_input, - boxSize_input); - - // m = trajPtr_->N * NUM_SPHERES * collisionAvoidancePtr_->numObstacles; - - collisionAvoidancePtr_->onlyComputeDerivativesForMinimumDistance = true; - m = trajPtr_->N * NUM_SPHERES; - - jointTJ = MatX::Zero(3, trajPtr_->Nact); - - initialize_memory(m, trajPtr_->varLength); -} - -void KinovaCustomizedConstraints::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - const int numObstacles = collisionAvoidancePtr_->numObstacles; - - if (numObstacles == 0) { - return; - } - - Vec3 sphereCenter; - MatX psphereCenter_pz; - Eigen::Array psphereCenter_pz_pz; - - for (int i = 0; i < trajPtr_->N; i++) { - const VecX& q = trajPtr_->q(i).head(trajPtr_->Nact); - const MatX& pq_pz = trajPtr_->pq_pz(i); - const Eigen::Array& pq_pz_pz = trajPtr_->pq_pz_pz.col(i); - - for (int j = 0; j < NUM_SPHERES; j++) { - // define the transform matrix of the sphere center with respect to the joint - endT = Transform(sphere_offset_axis[j], sphere_offset[j]); - - if (compute_hessian) { - fkPtr_->compute(0, sphere_joint_id[j], q, &startT, &endT, 2); - } - else if (compute_derivatives) { - fkPtr_->compute(0, sphere_joint_id[j], q, &startT, &endT, 1); - } - else { - fkPtr_->compute(0, sphere_joint_id[j], q, &startT, &endT, 0); - } - - sphereCenter = fkPtr_->getTranslation(); - - if (compute_derivatives) { - psphereCenter_pz = fkPtr_->getTranslationJacobian() * pq_pz; - } - - if (compute_hessian) { - Eigen::Array psphereCenter_pq_pq; - fkPtr_->getTranslationHessian(psphereCenter_pq_pq); - - // (1) p2_FK_pq2 * pq_pz1 * pq_pz2 - for (int k = 0; k < 3; k++) { - psphereCenter_pz_pz(k) = pq_pz.transpose() * psphereCenter_pq_pq(k) * pq_pz; - } - - // (2) p_FK_pq * p2_q_pz2 (this should be zero for most trajectories) - for (int k = 0; k < 3; k++) { - for (int p = 0; p < trajPtr_->Nact; p++) { - psphereCenter_pz_pz(k) += jointTJ(k, p) * pq_pz_pz(p); - } - } - } - - if (compute_hessian) { - collisionAvoidancePtr_->computeDistance(sphereCenter, psphereCenter_pz, psphereCenter_pz_pz); - } - else if (compute_derivatives) { - collisionAvoidancePtr_->computeDistance(sphereCenter, psphereCenter_pz); - } - else { - collisionAvoidancePtr_->computeDistance(sphereCenter); - } - - const auto& minIndex = collisionAvoidancePtr_->minimumDistanceIndex; - g(i * NUM_SPHERES + j) = collisionAvoidancePtr_->minimumDistance - sphere_radius[j]; - - if (compute_derivatives) { - pg_pz.row(i * NUM_SPHERES + j) = - collisionAvoidancePtr_->pdistances_pz.row(minIndex); - } - - if (compute_hessian) { - pg_pz_pz(i * NUM_SPHERES + j) = - collisionAvoidancePtr_->pdistances_pz_pz(minIndex); - } - } - } -} - -void KinovaCustomizedConstraints::compute_bounds() { - // distance with obstacles larger than 0 - g_lb.setConstant(0); - g_ub.setConstant(1e19); -} - -void KinovaCustomizedConstraints::print_violation_info() { - for (int i = 0; i < trajPtr_->N; i++) { - for (int j = 0; j < NUM_SPHERES; j++) { - if (g(i * NUM_SPHERES + j) <= 0) { - std::cout << " KinovaCustomizedConstraints.cpp: Sphere " << j - << " corresponding to link " << sphere_joint_id[j] - << " at time instance " << i - << " is in collision with the environment" - << std::endl; - } - } - } -} - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/ArmourUnsafe/src/KinovaOptimizer.cpp b/Examples/Kinova/ArmourUnsafe/src/KinovaOptimizer.cpp deleted file mode 100644 index 5752d038..00000000 --- a/Examples/Kinova/ArmourUnsafe/src/KinovaOptimizer.cpp +++ /dev/null @@ -1,242 +0,0 @@ -#include "KinovaOptimizer.h" - -namespace RAPTOR { -namespace Kinova { - -// // constructor -// KinovaOptimizer::KinovaOptimizer() -// { -// } - - -// // destructor -// KinovaOptimizer::~KinovaOptimizer() -// { -// } - -// [TNLP_set_parameters] -bool KinovaOptimizer::set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const ArmourTrajectoryParameters& atp_input, - const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input, - const VecX& qdes_input, - const int tplan_n_input, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input - ) -{ - enable_hessian = true; - x0 = x0_input; - qdes = qdes_input; - tplan_n = tplan_n_input; - - trajPtr_ = std::make_shared(T_input, - N_input, - model_input.nq, - Chebyshev, - atp_input); - - idPtr_ = std::make_shared(model_input, - trajPtr_); - - // read joint limits from KinovaConstants.h - VecX JOINT_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS) + - joint_limits_buffer_input; - - VecX JOINT_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS) - - joint_limits_buffer_input; - - // read velocity limits from KinovaConstants.h - VecX VELOCITY_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_LOWER, NUM_JOINTS) + - velocity_limits_buffer_input; - - VecX VELOCITY_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_UPPER, NUM_JOINTS) - - velocity_limits_buffer_input; - - // read torque limits from KinovaConstants.h - VecX TORQUE_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_LOWER, NUM_JOINTS) + - torque_limits_buffer_input; - - VecX TORQUE_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_UPPER, NUM_JOINTS) - - torque_limits_buffer_input; - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // Velocity limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - VELOCITY_LIMITS_LOWER_VEC, - VELOCITY_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("velocity limits"); - - // // Torque limits - // constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - // idPtr_, - // TORQUE_LIMITS_LOWER_VEC, - // TORQUE_LIMITS_UPPER_VEC)); - // constraintsNameVec_.push_back("torque limits"); - - // Customized constraints (collision avoidance with obstacles) - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - model_input, - jtype_input, - boxCenters_input, - boxOrientation_input, - boxSize_input)); - constraintsNameVec_.push_back("obstacle avoidance constraints"); - - assert(x0.size() == trajPtr_->varLength); - assert(qdes.size() == trajPtr_->Nact); - assert(tplan_n >= 0 && tplan_n < trajPtr_->N); - - return true; -} -// [TNLP_set_parameters] - -// [TNLP_get_nlp_info] -// returns some info about the nlp -bool KinovaOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) -{ - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} -// [TNLP_get_nlp_info] - -// [TNLP_eval_f] -// returns the value of the objective function -bool KinovaOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, false); - - const VecX& qplan = trajPtr_->q(tplan_n); - - obj_value = pow(Utils::wrapToPi(qplan[0] - qdes[0]), 2) + // These are continuous joints - pow(Utils::wrapToPi(qplan[2] - qdes[2]), 2) + - pow(Utils::wrapToPi(qplan[4] - qdes[4]), 2) + - pow(Utils::wrapToPi(qplan[6] - qdes[6]), 2) + - pow(qplan[1] - qdes[1], 2) + // These are not continuous joints - pow(qplan[3] - qdes[3], 2) + - pow(qplan[5] - qdes[5], 2); - - obj_value = 0.5 * obj_value; - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} -// [TNLP_eval_f] - -// [TNLP_eval_grad_f] -// return the gradient of the objective function grad_{x} f(x) -bool KinovaOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true); - - const VecX& qplan = trajPtr_->q(tplan_n); - const MatX& pqplan_pz = trajPtr_->pq_pz(tplan_n); - - Number qdiff = 0; - for(Index i = 0; i < n; i++){ - if (i % 2 == 0) { - qdiff = Utils::wrapToPi(qplan[i] - qdes[i]); - } - else { - qdiff = qplan[i] - qdes[i]; - } - - grad_f[i] = qdiff * pqplan_pz(i, i); - } - - return true; -} -// [TNLP_eval_grad_f] - -// [TNLP_eval_hess_f] -// return the hessian of the objective function hess_{x} f(x) as a dense matrix -bool KinovaOptimizer::eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_hess_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true, true); - - const VecX& qplan = trajPtr_->q(tplan_n); - const MatX& pqplan_pz = trajPtr_->pq_pz(tplan_n); - // we know the following is just 0 - // const Eigen::Array& pqplan_pz_pz = trajPtr_->pq_pz_pz.col(tplan_n); - - hess_f = MatX::Zero(n, n); - - for(Index i = 0; i < n; i++){ - hess_f(i, i) = pqplan_pz(i, i) * pqplan_pz(i, i); - } - - return true; -} -// [TNLP_eval_hess_f] - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/ArmourUnsafe/src/KinovaPybindWrapper.cpp b/Examples/Kinova/ArmourUnsafe/src/KinovaPybindWrapper.cpp deleted file mode 100644 index 680ed094..00000000 --- a/Examples/Kinova/ArmourUnsafe/src/KinovaPybindWrapper.cpp +++ /dev/null @@ -1,262 +0,0 @@ -#include "KinovaPybindWrapper.h" - -namespace RAPTOR { -namespace Kinova { - -KinovaPybindWrapper::KinovaPybindWrapper(const std::string urdf_filename) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.81; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - jtype.resize(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3; - - qdes.resize(model.nv); - - mynlp = new KinovaOptimizer(); - app = IpoptApplicationFactory(); - - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - joint_limits_buffer = VecX::Zero(model.nv); - velocity_limits_buffer = VecX::Zero(model.nv); - torque_limits_buffer = VecX::Zero(model.nv); -} - -void KinovaPybindWrapper::set_obstacles(const int num_obstacles_inp, - const py::array_t obstacles_inp) { - num_obstacles = num_obstacles_inp; - - if (num_obstacles < 0) { - throw std::invalid_argument("Number of obstacles must be non-negative"); - } - - py::buffer_info obstacles_buf = obstacles_inp.request(); - - if (obstacles_buf.size != num_obstacles * (3 + 3 + 3)) { - throw std::invalid_argument("Number of obstacles does not match the input array size"); - } - - const double* obstacles_ptr = (const double*)obstacles_buf.ptr; - - boxCenters.resize(num_obstacles); - boxOrientation.resize(num_obstacles); - boxSize.resize(num_obstacles); - - for (int i = 0; i < num_obstacles; i++) { - boxCenters[i] << obstacles_ptr[i * 9 + 0], - obstacles_ptr[i * 9 + 1], - obstacles_ptr[i * 9 + 2]; - boxOrientation[i] << obstacles_ptr[i * 9 + 3], - obstacles_ptr[i * 9 + 4], - obstacles_ptr[i * 9 + 5]; - boxSize[i] << obstacles_ptr[i * 9 + 6], - obstacles_ptr[i * 9 + 7], - obstacles_ptr[i * 9 + 8]; - } - - set_obstacles_check = true; -} - -void KinovaPybindWrapper::set_ipopt_parameters(const double tol, - const double obj_scaling_factor, - const double max_wall_time, - const int print_level, - const std::string mu_strategy, - const std::string linear_solver, - const bool gradient_check) { - app->Options()->SetNumericValue("tol", tol); - app->Options()->SetNumericValue("obj_scaling_factor", obj_scaling_factor); - app->Options()->SetNumericValue("max_wall_time", max_wall_time); - app->Options()->SetIntegerValue("print_level", print_level); - app->Options()->SetStringValue("mu_strategy", mu_strategy); - app->Options()->SetStringValue("linear_solver", linear_solver); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - - if (gradient_check) { - app->Options()->SetStringValue("output_file", "ipopt.out"); - app->Options()->SetStringValue("derivative_test", "first-order"); - app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - app->Options()->SetNumericValue("derivative_test_tol", 1e-6); - } - - set_ipopt_parameters_check = true; -} - -void KinovaPybindWrapper::set_trajectory_parameters(const py::array_t q0_inp, - const py::array_t qd0_inp, - const py::array_t qdd0_inp, - const double duration_inp) { - py::buffer_info q0_buf = q0_inp.request(); - py::buffer_info qd0_buf = qd0_inp.request(); - py::buffer_info qdd0_buf = qdd0_inp.request(); - - T = duration_inp; - - if (q0_buf.size != model.nv) { - throw std::invalid_argument("q0 must be of size model.nv"); - } - - if (qd0_buf.size != model.nv) { - throw std::invalid_argument("qd0 must be of size model.nv"); - } - - if (qdd0_buf.size != model.nv) { - throw std::invalid_argument("qdd0 must be of size model.nv"); - } - - if (T <= 0.0) { - throw std::invalid_argument("Duration must be positive"); - } - - const double *q0_ptr = (const double*)q0_buf.ptr; - const double *qd0_ptr = (const double*)qd0_buf.ptr; - const double *qdd0_ptr = (const double*)qdd0_buf.ptr; - - atp.q0.resize(model.nv); - atp.q_d0.resize(model.nv); - atp.q_dd0.resize(model.nv); - - for (int i = 0; i < model.nv; i++) { - atp.q0(i) = q0_ptr[i]; - atp.q_d0(i) = qd0_ptr[i]; - atp.q_dd0(i) = qdd0_ptr[i]; - } - - set_trajectory_parameters_check = true; -} - -void KinovaPybindWrapper::set_buffer(const py::array_t joint_limits_buffer_inp, - const py::array_t velocity_limits_buffer_inp, - const py::array_t torque_limits_buffer_inp) { - py::buffer_info joint_limits_buffer_buf = joint_limits_buffer_inp.request(); - py::buffer_info velocity_limits_buffer_buf = velocity_limits_buffer_inp.request(); - py::buffer_info torque_limits_buffer_buf = torque_limits_buffer_inp.request(); - - if (joint_limits_buffer_buf.size != model.nv) { - throw std::invalid_argument("joint_limits_buffer must be of size model.nv"); - } - - if (velocity_limits_buffer_buf.size != model.nv) { - throw std::invalid_argument("velocity_limits_buffer must be of size model.nv"); - } - - if (torque_limits_buffer_buf.size != model.nv) { - throw std::invalid_argument("torque_limits_buffer must be of size model.nv"); - } - - const double* joint_limits_buffer_ptr = (const double*)joint_limits_buffer_buf.ptr; - const double* velocity_limits_buffer_ptr = (const double*)velocity_limits_buffer_buf.ptr; - const double* torque_limits_buffer_ptr = (const double*)torque_limits_buffer_buf.ptr; - - for (int i = 0; i < model.nv; i++) { - joint_limits_buffer(i) = joint_limits_buffer_ptr[i]; - velocity_limits_buffer(i) = velocity_limits_buffer_ptr[i]; - torque_limits_buffer(i) = torque_limits_buffer_ptr[i]; - } - - set_buffer_check = true; -} - -void KinovaPybindWrapper::set_target(const py::array_t q_des_inp, - const double tplan_inp) { - tplan = tplan_inp; - - if (tplan <= 0.0 || tplan > T) { - throw std::invalid_argument("tplan must be greater than 0.0 or smaller than duration"); - } - - py::buffer_info q_des_buf = q_des_inp.request(); - - if (q_des_buf.size != model.nv) { - throw std::invalid_argument("q_des must be of size model.nv"); - } - - const double *q_des_ptr = (const double*)q_des_buf.ptr; - - for (int i = 0; i < model.nv; i++) { - qdes(i) = q_des_ptr[i]; - } - tplan_n = int(tplan / T * N); - - set_target_check = true; -} - -py::tuple KinovaPybindWrapper::optimize() { - if (!set_obstacles_check || - !set_ipopt_parameters_check || - !set_trajectory_parameters_check || - !set_buffer_check || - !set_target_check) { - throw std::runtime_error("parameters not set properly!"); - } - - // Define initial guess - Eigen::VectorXd z(model.nv); - z.setZero(); - - // Initialize Kinova optimizer - try { - mynlp->set_parameters(z, - T, - N, - degree, - model, - jtype, - atp, - boxCenters, - boxOrientation, - boxSize, - qdes, - tplan_n, - joint_limits_buffer, - velocity_limits_buffer, - torque_limits_buffer); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - set_ipopt_parameters_check = false; - set_trajectory_parameters_check = false; - set_target_check = false; - - py::array_t result(model.nv, mynlp->solution.data()); - return py::make_tuple(result, mynlp->ifFeasible); -} - -}; // namespace Kinova -}; // namespace RAPTOR diff --git a/Examples/Kinova/InverseKinematics/KinovaIKExample.cpp b/Examples/Kinova/InverseKinematics/KinovaIKExample.cpp deleted file mode 100644 index 675e891a..00000000 --- a/Examples/Kinova/InverseKinematics/KinovaIKExample.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "KinovaIKSolver.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -using namespace RAPTOR; -using namespace Kinova; -using namespace Ipopt; - -int main() { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - const std::string urdf_filename = "../Examples/Kinova/ArmourUnsafe/kinova.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.81; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3; - - // Compute forward kinematics at a random configuration first - ForwardKinematicsSolver fkSolver(&model, jtype); - - std::srand(std::time(nullptr)); - Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq); - fkSolver.compute(0, model.nq, q); - const Transform desiredTransform = fkSolver.getTransform(); - - // Define initial guess - Eigen::VectorXd z = Eigen::VectorXd::Random(model.nq); - - // Initialize Kinova optimizer - SmartPtr mynlp = new KinovaIKSolver(); - try { - mynlp->set_parameters(z, - model, - jtype, - desiredTransform); - mynlp->constr_viol_tol = 1e-8; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("tol", 1e-4); - app->Options()->SetNumericValue("constr_viol_tol", 1e-6); - app->Options()->SetNumericValue("max_wall_time", 2.0); - app->Options()->SetIntegerValue("print_level", 5); - app->Options()->SetIntegerValue("max_iter", 100); - app->Options()->SetStringValue("mu_strategy", "adaptive"); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - - // For gradient checking - // app->Options()->SetStringValue("output_file", "ipopt.out"); - // app->Options()->SetStringValue("derivative_test", "second-order"); - // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - // app->Options()->SetNumericValue("derivative_test_tol", 1e-4); - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - std::cout << q.transpose() << std::endl; - std::cout << mynlp->solution.transpose() << std::endl; - - std::cout << "Desired transform: " << desiredTransform << std::endl; - fkSolver.compute(0, model.nq, mynlp->solution); - std::cout << "Solved transform: " << fkSolver.getTransform() << std::endl; - - return 0; -} diff --git a/Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp b/Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp deleted file mode 100644 index 0bb80d4e..00000000 --- a/Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp +++ /dev/null @@ -1,244 +0,0 @@ -#include "KinovaIKSolver.h" - -namespace RAPTOR { -namespace Kinova { - -// // constructor -// KinovaIKSolver::KinovaIKSolver() -// { -// } - - -// // destructor -// KinovaIKSolver::~KinovaIKSolver() -// { -// } - -// [TNLP_set_parameters] -bool KinovaIKSolver::set_parameters( - const VecX& x0_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const Transform& desiredTransform_input -) -{ - enable_hessian = true; - x0 = x0_input; - - trajPtr_ = std::make_shared(model_input.nq); - - // read joint limits from KinovaConstants.h - VecX JOINT_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS); - VecX JOINT_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS); - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // End effector kinematics constraints - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - &model_input, - jtype_input, - model_input.nq, // the last joint - 0, - desiredTransform_input)); - constraintsNameVec_.push_back("kinematics constraints"); - - assert(x0.size() == trajPtr_->varLength); - - return true; -} -// [TNLP_set_parameters] - -// [TNLP_get_nlp_info] -// returns some info about the nlp -bool KinovaIKSolver::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) -{ - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} -// [TNLP_get_nlp_info] - -// [TNLP_get_bounds_info] -// returns the variable bounds -bool KinovaIKSolver::get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u -) -{ - // here, the n and m we gave IPOPT in get_nlp_info are passed back to us. - // If desired, we could assert to make sure they are what we think they are. - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in get_bounds_info!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in get_bounds_info!"); - } - - // lower bounds - for( Index i = 0; i < n; i++ ) { - x_l[i] = JOINT_LIMITS_LOWER[i]; - } - - // upper bounds - for( Index i = 0; i < n; i++ ) { - x_u[i] = JOINT_LIMITS_UPPER[i]; - } - - if (constraintsPtrVec_.size() != constraintsNameVec_.size()) { - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ and constraintsNameVec_ have different sizes!"); - } - - // compute bounds for all constraints - Index iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - try { - constraintsPtrVec_[c]->compute_bounds(); - } - catch (std::exception& e) { - std::cerr << "Error in " << constraintsNameVec_[c] << ": " << std::endl; - std::cerr << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in get_bounds_info! Check previous error message."); - } - - if (constraintsPtrVec_[c]->m != constraintsPtrVec_[c]->g_lb.size() || - constraintsPtrVec_[c]->m != constraintsPtrVec_[c]->g_ub.size()) { - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ have different sizes!"); - } - - for ( Index i = 0; i < constraintsPtrVec_[c]->m; i++ ) { - g_l[iter] = constraintsPtrVec_[c]->g_lb(i); - g_u[iter] = constraintsPtrVec_[c]->g_ub(i); - iter++; - } - } - - // report constraints distribution - std::cout << "Dimension of each constraints and their locations: \n"; - iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - std::cout << constraintsNameVec_[c] << ": "; - std::cout << constraintsPtrVec_[c]->m << " ["; - std::cout << iter << " "; - iter += constraintsPtrVec_[c]->m; - std::cout << iter << "]\n"; - } - - return true; -} -// [TNLP_get_bounds_info] - -// [TNLP_eval_f] -// returns the value of the objective function -bool KinovaIKSolver::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, false); - - const VecX& q = trajPtr_->q(0); - - obj_value = 0.5 * q.dot(q); - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} -// [TNLP_eval_f] - -// [TNLP_eval_grad_f] -// return the gradient of the objective function grad_{x} f(x) -bool KinovaIKSolver::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true); - - const VecX& q = trajPtr_->q(0); - const MatX& pq_pz = trajPtr_->pq_pz(0); - - VecX grad = q.transpose() * pq_pz; - for(Index i = 0; i < n; i++){ - grad_f[i] = grad(i); - } - - return true; -} -// [TNLP_eval_grad_f] - -// [TNLP_eval_hess_f] -// return the hessian of the objective function hess_{x} f(x) as a dense matrix -bool KinovaIKSolver::eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_hess_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true, true); - - const VecX& q = trajPtr_->q(0); - const MatX& pq_pz = trajPtr_->pq_pz(0); - // we know the following is just 0 - // const Eigen::Array& pq_pz_pz = trajPtr_->pq_pz_pz.col(0); - - hess_f = pq_pz.transpose() * pq_pz; - - return true; -} -// [TNLP_eval_hess_f] - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/InverseKinematics/KinovaIKSolver.h b/Examples/Kinova/InverseKinematics/KinovaIKSolver.h deleted file mode 100644 index c4cb38f7..00000000 --- a/Examples/Kinova/InverseKinematics/KinovaIKSolver.h +++ /dev/null @@ -1,107 +0,0 @@ -#ifndef KINOVA_IK_SOLVER_H -#define KINOVA_IK_SOLVER_H - -#include "KinovaConstants.h" - -#include "Optimizer.h" - -#include "Plain.h" - -#include "JointLimits.h" -#include "KinematicsConstraints.h" - -namespace RAPTOR { -namespace Kinova { - -class KinovaIKSolver : public Optimizer { -public: - using Model = pinocchio::Model; - using VecX = Eigen::VectorXd; - using Vec3 = Eigen::Vector3d; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - KinovaIKSolver() = default; - - /** Default destructor */ - ~KinovaIKSolver() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const Transform& desiredTransform_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the bounds for my problem */ - bool get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /** Method to return the hessian of the objective */ - bool eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - KinovaIKSolver( - const KinovaIKSolver& - ); - - KinovaIKSolver& operator=( - const KinovaIKSolver& - ); - - std::shared_ptr trajPtr_; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // KINOVA_IK_SOLVER_H diff --git a/Examples/Kinova/KinovaConstants.h b/Examples/Kinova/KinovaConstants.h deleted file mode 100644 index 0fdeacc8..00000000 --- a/Examples/Kinova/KinovaConstants.h +++ /dev/null @@ -1,19 +0,0 @@ -namespace RAPTOR { -namespace Kinova { - -constexpr int NUM_JOINTS = 7; - -constexpr double JOINT_LIMITS_LOWER[NUM_JOINTS] = {-1e19, -2.41, -1e19, -2.66, -1e19, -2.23, -1e19}; // radian - -constexpr double JOINT_LIMITS_UPPER[NUM_JOINTS] = {1e19, 2.41, 1e19, 2.66, 1e19, 2.23, 1e19}; // radian - -constexpr double VELOCITY_LIMITS_LOWER[NUM_JOINTS] = {-1.3963, -1.3963, -1.3963, -1.3963, -1.2218, -1.2218, -1.2218}; // radian/s - -constexpr double VELOCITY_LIMITS_UPPER[NUM_JOINTS] = {1.3963, 1.3963, 1.3963, 1.3963, 1.2218, 1.2218, 1.2218}; // radian/s - -constexpr double TORQUE_LIMITS_LOWER[NUM_JOINTS] = {-56.7, -56.7, -56.7, -56.7, -29.4, -29.4, -29.4}; // N*m - -constexpr double TORQUE_LIMITS_UPPER[NUM_JOINTS] = {56.7, 56.7, 56.7, 56.7, 29.4, 29.4, 29.4}; // N*m - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/README.md b/Examples/Kinova/README.md deleted file mode 100644 index 6912fe60..00000000 --- a/Examples/Kinova/README.md +++ /dev/null @@ -1 +0,0 @@ -This folder contains all Kinova-related scripts \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/DataFilter/KinovaAccelerationEstimator.cpp b/Examples/Kinova/SystemIdentification/DataFilter/KinovaAccelerationEstimator.cpp deleted file mode 100644 index 097f4957..00000000 --- a/Examples/Kinova/SystemIdentification/DataFilter/KinovaAccelerationEstimator.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include "DataFilterOptimizer.h" - -using namespace RAPTOR; -using namespace Kinova; -using namespace Ipopt; - -int main () { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define Data - Eigen::MatrixXd temp; - temp = Utils::initializeEigenMatrixFromFile("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/feedback_pos.csv"); - Eigen::MatrixXd q_data = temp.transpose(); - temp = Utils::initializeEigenMatrixFromFile("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/feedback_vel.csv"); - Eigen::MatrixXd q_d_data = temp.transpose(); - temp = Utils::initializeEigenMatrixFromFile("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/frame_ts.csv"); - Eigen::VectorXd tspan = temp.array() - temp(0); - - const int num_samples = 2000; - - // Define trajectory parameters - const int degree = 10; - const int base_frequency = 5; - - // Define initial guess - Eigen::VectorXd z = Eigen::VectorXd::Zero((2 * degree + 1) * 7); - - // Eigen::VectorXd z = Eigen::VectorXd::Zero((2 * degree + 2) * 7).array(); - // for (int i = 0; i < 7; i++) { - // z((2 * degree + 2) * i + (2 * degree + 1)) = 5 * (i + 1); - // } - - // Initialize Kinova optimizer - SmartPtr mynlp = new DataFilterOptimizer(); - try { - // mynlp->set_parameters(z, - // Utils::uniformlySampleVector(tspan.head(25000), num_samples), - // Utils::uniformlySampleMatrixInCols(q_data.leftCols(25000), num_samples), - // Utils::uniformlySampleMatrixInCols(q_d_data.leftCols(25000), num_samples), - // degree, - // base_frequency); - mynlp->set_parameters(z, - tspan.head(num_samples), - q_data.leftCols(num_samples), - q_d_data.leftCols(num_samples), - degree, - base_frequency); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("tol", 1e-6); - app->Options()->SetNumericValue("constr_viol_tol", 1e-6); - app->Options()->SetNumericValue("max_wall_time", 20.0); - app->Options()->SetIntegerValue("print_level", 5); - // app->Options()->SetIntegerValue("max_iter", 50); - app->Options()->SetStringValue("mu_strategy", "adaptive"); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - - // // For gradient checking - // app->Options()->SetStringValue("output_file", "ipopt.out"); - // app->Options()->SetStringValue("derivative_test", "second-order"); - // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - // app->Options()->SetNumericValue("derivative_test_tol", 1e-4); - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Print the solution - std::ofstream filtered_time("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/filtered_time.csv"); - std::ofstream filtered_position("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/filtered_pos.csv"); - std::ofstream filtered_velocity("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/filtered_vel.csv"); - std::ofstream filtered_acceleration("../Examples/Kinova/SystemIdentification/DataFilter/data/SinExperiment_1703028519/filtered_acc.csv"); - - const auto trajPtr_ = mynlp->trajPtr_; - for (int i = 0; i < trajPtr_->N; i++) { - filtered_time << trajPtr_->tspan(i) << std::endl; - filtered_position << trajPtr_->q(i).transpose() << std::endl; - filtered_velocity << trajPtr_->q_d(i).transpose() << std::endl; - filtered_acceleration << trajPtr_->q_dd(i).transpose() << std::endl; - } - - return 0; -} \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/DataFilter/README.md b/Examples/Kinova/SystemIdentification/DataFilter/README.md deleted file mode 100644 index baf765ec..00000000 --- a/Examples/Kinova/SystemIdentification/DataFilter/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Data Filter - -Estimate acceleration based on position and velocity data. \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/DataFilter/include/DataFilterOptimizer.h b/Examples/Kinova/SystemIdentification/DataFilter/include/DataFilterOptimizer.h deleted file mode 100644 index 289e5d9c..00000000 --- a/Examples/Kinova/SystemIdentification/DataFilter/include/DataFilterOptimizer.h +++ /dev/null @@ -1,116 +0,0 @@ -#ifndef CONDITION_NUMBER_OPTIMIZER_H -#define CONDITION_NUMBER_OPTIMIZER_H - -#include "KinovaConstants.h" - -#include "Optimizer.h" - -#include "FixedFrequencyFourierCurves.h" -#include "FourierCurves.h" - -#include "JointLimits.h" - -namespace RAPTOR { -namespace Kinova { - -class DataFilterOptimizer : public Optimizer { -public: - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - DataFilterOptimizer() = default; - - /** Default destructor */ - ~DataFilterOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const VecX& tspan_input, - const MatX& q_input, - const MatX& q_d_input, - const int degree_input, - const int base_frequency_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the bounds for my problem */ - bool get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /** Method to return the hessian of the objective */ - bool eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - DataFilterOptimizer( - const DataFilterOptimizer& - ); - - DataFilterOptimizer& operator=( - const DataFilterOptimizer& - ); - - const Number SQUARE_ROOT_THRESHOLD = 1e-8; - - MatX q_data; - MatX q_d_data; - - Number position_weight = 5.0; - Number velocity_weight = 1.0; - - std::shared_ptr trajPtr_; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // CONDITION_NUMBER_OPTIMIZER_H \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp b/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp deleted file mode 100644 index 13da6352..00000000 --- a/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp +++ /dev/null @@ -1,300 +0,0 @@ -#include "DataFilterOptimizer.h" - -namespace RAPTOR { -namespace Kinova { - -bool DataFilterOptimizer::set_parameters( - const VecX& x0_input, - const VecX& tspan_input, - const MatX& q_input, - const MatX& q_d_input, - const int degree_input, - const int base_frequency_input) { - enable_hessian = true; - - x0 = x0_input; - - if (q_input.rows() != q_d_input.rows() || - q_input.cols() != q_d_input.cols()) { - THROW_EXCEPTION(IpoptException, "*** q_input and q_d_input should have the same size!"); - } - - if (q_input.cols() != tspan_input.size()) { - THROW_EXCEPTION(IpoptException, "*** q_input and tspan_input should have the same size!"); - } - - q_data = q_input; - q_d_data = q_d_input; - - // read joint limits from KinovaConstants.h - VecX JOINT_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS); - - VecX JOINT_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS); - - // fixed frequency fourier curves with fixed initial position and velocity - trajPtr_ = std::make_shared(tspan_input, - q_data.rows(), - degree_input, - base_frequency_input, - q_data.col(0), - q_d_data.col(0)); - // trajPtr_ = std::make_shared(tspan_input, - // q_data.rows(), - // degree_input, - // q_data.col(0), - // q_d_data.col(0)); - - // // Joint limits - // constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - // JOINT_LIMITS_LOWER_VEC, - // JOINT_LIMITS_UPPER_VEC)); - // constraintsNameVec_.push_back("joint limits"); - - return true; -} - -bool DataFilterOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) { - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} - -bool DataFilterOptimizer::get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u -) -{ - // here, the n and m we gave IPOPT in get_nlp_info are passed back to us. - // If desired, we could assert to make sure they are what we think they are. - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in get_bounds_info!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in get_bounds_info!"); - } - - // lower bounds - for( Index i = 0; i < n; i++ ) { - x_l[i] = -100; - } - - // upper bounds - for( Index i = 0; i < n; i++ ) { - x_u[i] = 100; - } - - if (constraintsPtrVec_.size() != constraintsNameVec_.size()) { - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ and constraintsNameVec_ have different sizes!"); - } - - // compute bounds for all constraints - Index iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - try { - constraintsPtrVec_[c]->compute_bounds(); - } - catch (std::exception& e) { - std::cerr << "Error in " << constraintsNameVec_[c] << ": " << std::endl; - std::cerr << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in get_bounds_info! Check previous error message."); - } - - if (constraintsPtrVec_[c]->m != constraintsPtrVec_[c]->g_lb.size() || - constraintsPtrVec_[c]->m != constraintsPtrVec_[c]->g_ub.size()) { - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ have different sizes!"); - } - - for ( Index i = 0; i < constraintsPtrVec_[c]->m; i++ ) { - g_l[iter] = constraintsPtrVec_[c]->g_lb(i); - g_u[iter] = constraintsPtrVec_[c]->g_ub(i); - iter++; - } - } - - // report constraints distribution - std::cout << "Dimension of each constraints and their locations: \n"; - iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - std::cout << constraintsNameVec_[c] << ": "; - std::cout << constraintsPtrVec_[c]->m << " ["; - std::cout << iter << " "; - iter += constraintsPtrVec_[c]->m; - std::cout << iter << "]\n"; - } - - return true; -} - -bool DataFilterOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, false); - - // position estimation error - Number position_error = 0; - for ( Index i = 0; i < q_data.cols(); i++ ) { - VecX qdiff = trajPtr_->q(i) - q_data.col(i); - position_error += sqrt(qdiff.dot(qdiff)); - } - - // velocity estimation error - Number velocity_error = 0; - for ( Index i = 0; i < q_d_data.cols(); i++ ) { - VecX q_d_diff = trajPtr_->q_d(i) - q_d_data.col(i); - velocity_error += sqrt(q_d_diff.dot(q_d_diff)); - } - - obj_value = position_weight * position_error + - velocity_weight * velocity_error; - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} - -bool DataFilterOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true); - - // gradient of position estimation error - VecX grad_position_error = VecX::Zero(n); - for ( Index i = 0; i < q_data.cols(); i++ ) { - VecX q_diff = trajPtr_->q(i) - q_data.col(i); - Number error = sqrt(q_diff.dot(q_diff)); - if (error > SQUARE_ROOT_THRESHOLD) { - grad_position_error += q_diff.transpose() * trajPtr_->pq_pz(i) / error; - } - // else { - // grad_position_error += VecX::Zero(n); - // } - } - - // gradient of velocity estimation error - VecX grad_velocity_error = VecX::Zero(n); - for ( Index i = 0; i < q_d_data.cols(); i++ ) { - VecX q_d_diff = trajPtr_->q_d(i) - q_d_data.col(i); - Number error = sqrt(q_d_diff.dot(q_d_diff)); - if (error > SQUARE_ROOT_THRESHOLD) { - grad_velocity_error += q_d_diff.transpose() * trajPtr_->pq_d_pz(i) / error; - } - // else { - // grad_velocity_error += VecX::Zero(n); - // } - } - - for ( Index i = 0; i < n; i++ ) { - grad_f[i] = position_weight * grad_position_error(i) + - velocity_weight * grad_velocity_error(i); - } - - return true; -} - -bool DataFilterOptimizer::eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_hess_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true, true); - - // hessian of position estimation error - MatX hess_position_error = MatX::Zero(n, n); - for ( Index i = 0; i < q_data.cols(); i++ ) { - VecX q_diff = trajPtr_->q(i) - q_data.col(i); - Number error = sqrt(q_diff.dot(q_diff)); - - if (error > SQUARE_ROOT_THRESHOLD) { - hess_position_error += trajPtr_->pq_pz(i).transpose() * trajPtr_->pq_pz(i) / error; - - for (Index j = 0; j < q_diff.size(); j++) { - hess_position_error += q_diff(j) * trajPtr_->pq_pz_pz(j, i) / error; - } - - MatX pdiffSquare_pz = q_diff.transpose() * trajPtr_->pq_pz(i); - hess_position_error -= pdiffSquare_pz.transpose() * pdiffSquare_pz / std::pow(error, 3.0); - } - // else { - // hess_position_error += MatX::Zero(n, n); - // } - } - - // hessian of velocity estimation error - MatX hess_velocity_error = MatX::Zero(n, n); - for ( Index i = 0; i < q_d_data.cols(); i++ ) { - VecX q_d_diff = trajPtr_->q_d(i) - q_d_data.col(i); - Number error = sqrt(q_d_diff.dot(q_d_diff)); - - if (error > SQUARE_ROOT_THRESHOLD) { - hess_velocity_error += trajPtr_->pq_d_pz(i).transpose() * trajPtr_->pq_d_pz(i) / error; - - for (Index j = 0; j < q_d_diff.size(); j++) { - hess_velocity_error += q_d_diff(j) * trajPtr_->pq_d_pz_pz(j, i) / error; - } - - MatX pdiffSquare_pz = q_d_diff.transpose() * trajPtr_->pq_d_pz(i); - hess_velocity_error -= pdiffSquare_pz.transpose() * pdiffSquare_pz / std::pow(error, 3.0); - } - // else { - // hess_position_error += MatX::Zero(n, n); - // } - } - - hess_f = position_weight * hess_position_error + - velocity_weight * hess_velocity_error; - - return true; -} - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExample.cpp b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExample.cpp deleted file mode 100644 index b9df7da1..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExample.cpp +++ /dev/null @@ -1,178 +0,0 @@ -#include "ConditionNumberOptimizer.h" - -using namespace RAPTOR; -using namespace Kinova; -using namespace Ipopt; - -int main(int argc, char* argv[]) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix.csv"; - // const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv"; - - // Define robot model - const std::string urdf_filename = "../Examples/Kinova/ArmourUnsafe/kinova.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = -9.81; - model.friction.setZero(); - // model.damping.setZero(); - // model.rotorInertia.setZero(); - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3; - - // Define trajectory parameters - const double T = 10.0; - const int N = 50; - const int degree = 5; - const double base_frequency = 2.0 * M_PI / T; - - // Define initial guess - std::srand(static_cast(time(0))); - Eigen::VectorXd z = 2 * 0.2 * Eigen::VectorXd::Random((2 * degree + 3) * model.nv).array() - 0.1; - z.block((2 * degree + 1) * model.nv, 0, model.nv, 1) = - 2 * 1.0 * Eigen::VectorXd::Random(model.nv).array() - 1.0; - z.block((2 * degree + 1) * model.nv + model.nv, 0, model.nv, 1) = - 2 * 0.5 * Eigen::VectorXd::Random(model.nv).array() - 0.5; - - // Define limits buffer - Eigen::VectorXd joint_limits_buffer(model.nq); - joint_limits_buffer.setConstant(0.02); - Eigen::VectorXd velocity_limits_buffer(model.nq); - velocity_limits_buffer.setConstant(0.05); - Eigen::VectorXd torque_limits_buffer(model.nq); - torque_limits_buffer.setConstant(0.5); - - // Initialize Kinova optimizer - SmartPtr mynlp = new ConditionNumberOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - degree, - base_frequency, - model, - jtype, - regroupMatrixFileName, - joint_limits_buffer, - velocity_limits_buffer, - torque_limits_buffer); - mynlp->constr_viol_tol = 1e-5; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("tol", 1e-6); - app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol); - app->Options()->SetNumericValue("max_wall_time", 60.0); - app->Options()->SetIntegerValue("print_level", 5); - app->Options()->SetStringValue("mu_strategy", "adaptive"); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - - // For gradient checking - // app->Options()->SetStringValue("output_file", "ipopt.out"); - // app->Options()->SetStringValue("derivative_test", "first-order"); - // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - // app->Options()->SetNumericValue("derivative_test_tol", 1e-3); - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - - const Eigen::VectorXd initial_position_part = mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv); - mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv) = - Utils::wrapToPi(initial_position_part); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Re-evaluate the solution at a higher resolution and print the results. - if (mynlp->ifFeasible) { - std::shared_ptr traj = std::make_shared(T, - 2000, - model.nv, - TimeDiscretization::Uniform, - degree, - base_frequency); - traj->compute(mynlp->solution, false); - - if (argc > 1) { - const std::string outputfolder = "../Examples/Kinova/SystemIdentification/ExcitingTrajectories/data/T10_d5_slower/"; - std::ofstream solution(outputfolder + "exciting-solution-" + std::string(argv[1]) + ".csv"); - std::ofstream position(outputfolder + "exciting-position-" + std::string(argv[1]) + ".csv"); - std::ofstream velocity(outputfolder + "exciting-velocity-" + std::string(argv[1]) + ".csv"); - std::ofstream acceleration(outputfolder + "exciting-acceleration-" + std::string(argv[1]) + ".csv"); - - solution << std::setprecision(16); - position << std::setprecision(16); - velocity << std::setprecision(16); - acceleration << std::setprecision(16); - - for (int i = 0; i < traj->N; i++) { - position << traj->q(i).transpose() << std::endl; - velocity << traj->q_d(i).transpose() << std::endl; - acceleration << traj->q_dd(i).transpose() << std::endl; - } - - for (int i = 0; i < mynlp->solution.size(); i++) { - solution << mynlp->solution(i) << std::endl; - } - solution << base_frequency << std::endl; - } - else { - std::ofstream solution("exciting-solution.csv"); - std::ofstream position("exciting-position.csv"); - std::ofstream velocity("exciting-velocity.csv"); - std::ofstream acceleration("exciting-acceleration.csv"); - - for (int i = 0; i < traj->N; i++) { - position << traj->q(i).transpose() << std::endl; - velocity << traj->q_d(i).transpose() << std::endl; - acceleration << traj->q_dd(i).transpose() << std::endl; - } - - for (int i = 0; i < mynlp->solution.size(); i++) { - solution << mynlp->solution(i) << std::endl; - } - } - } - - return 0; -} \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/README.md b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/README.md deleted file mode 100644 index 6216634b..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Getting the Most Exciting Trajectories for System Identification - -Minimizing the condition number of the observation matrix. \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/ConditionNumberOptimizer.h b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/ConditionNumberOptimizer.h deleted file mode 100644 index 73eb519b..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/ConditionNumberOptimizer.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef CONDITION_NUMBER_OPTIMIZER_H -#define CONDITION_NUMBER_OPTIMIZER_H - -#include "KinovaConstants.h" - -#include "Optimizer.h" - -#include "RegressorInverseDynamics.h" -#include "FixedFrequencyFourierCurves.h" - -#include "JointLimits.h" -#include "VelocityLimits.h" -#include "TorqueLimits.h" -#include "KinovaCustomizedConstraints.h" - -namespace RAPTOR { -namespace Kinova { - -class ConditionNumberOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - ConditionNumberOptimizer() = default; - - /** Default destructor */ - ~ConditionNumberOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const int degree_input, - const double base_frequency_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::string& regroupMatrixFileName, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - ConditionNumberOptimizer( - const ConditionNumberOptimizer& - ); - - ConditionNumberOptimizer& operator=( - const ConditionNumberOptimizer& - ); - - MatX regroupMatrix; - - std::shared_ptr trajPtr_; - - std::shared_ptr ridPtr_; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // CONDITION_NUMBER_OPTIMIZER_H \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp deleted file mode 100644 index 7716a511..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp +++ /dev/null @@ -1,205 +0,0 @@ -#include "ConditionNumberOptimizer.h" - -namespace RAPTOR { -namespace Kinova { - -// // constructor -// ConditionNumberOptimizer::ConditionNumberOptimizer() -// { -// } - - -// // destructor -// ConditionNumberOptimizer::~ConditionNumberOptimizer() -// { -// } - -bool ConditionNumberOptimizer::set_parameters( - const VecX& x0_input, - const Number T_input, - const int N_input, - const int degree_input, - const double base_frequency_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::string& regroupMatrixFileName, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input -) { - enable_hessian = false; - x0 = x0_input; - regroupMatrix = Utils::initializeEigenMatrixFromFile(regroupMatrixFileName); - - // fixed frequency fourier curves with 0 initial velocity - trajPtr_ = std::make_shared(T_input, - N_input, - model_input.nv, - TimeDiscretization::Uniform, - degree_input, - base_frequency_input); - - ridPtr_ = std::make_shared(model_input, - jtype_input, - trajPtr_); - - // read joint limits from KinovaConstants.h - VecX JOINT_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS) + - joint_limits_buffer_input; - - VecX JOINT_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS) - - joint_limits_buffer_input; - - // read velocity limits from KinovaConstants.h - VecX VELOCITY_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_LOWER, NUM_JOINTS) + - velocity_limits_buffer_input; - - VecX VELOCITY_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_UPPER, NUM_JOINTS) - - velocity_limits_buffer_input; - - // read torque limits from KinovaConstants.h - VecX TORQUE_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_LOWER, NUM_JOINTS) + - torque_limits_buffer_input; - - VecX TORQUE_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_UPPER, NUM_JOINTS) - - torque_limits_buffer_input; - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // Velocity limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - VELOCITY_LIMITS_LOWER_VEC, - VELOCITY_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("velocity limits"); - - // Torque limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - ridPtr_, - TORQUE_LIMITS_LOWER_VEC, - TORQUE_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("torque limits"); - - // // Customized constraints (collision avoidance with ground) - std::vector groundCenter = {Vec3(0.0, 0.0, 0.04)}; - std::vector groundOrientation = {Vec3(0.0, 0.0, 0.0)}; - std::vector groundSize = {Vec3(5.0, 5.0, 0.01)}; - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - model_input, - jtype_input, - groundCenter, - groundOrientation, - groundSize)); - constraintsNameVec_.push_back("obstacle avoidance constraints"); - - // check dimensions of regroupMatrix - if (ridPtr_->Y.cols() != regroupMatrix.rows()) { - throw std::invalid_argument("ConditionNumberOptimizer: regroupMatrix has wrong dimensions!"); - } - - return true; -} - -bool ConditionNumberOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) { - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} - -bool ConditionNumberOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - ridPtr_->compute(z, false); - - MatX regroupedObservationMatrix = ridPtr_->Y * regroupMatrix; - Eigen::JacobiSVD svd(regroupedObservationMatrix, - Eigen::ComputeThinU | Eigen::ComputeThinV); - const VecX& singularValues = svd.singularValues(); - const MatX& U = svd.matrixU(); - const MatX& V = svd.matrixV(); - - Number sigmaMax = singularValues(0); - Number sigmaMin = singularValues(singularValues.size() - 1); - - // log of 2-norm condition number (sigmaMax / sigmaMin) - obj_value = std::log(sigmaMax) - std::log(sigmaMin); - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} - -bool ConditionNumberOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - ridPtr_->compute(z, true); - - MatX regroupedObservationMatrix = ridPtr_->Y * regroupMatrix; - Eigen::JacobiSVD svd(regroupedObservationMatrix, - Eigen::ComputeThinU | Eigen::ComputeThinV); - const VecX& singularValues = svd.singularValues(); - const MatX& U = svd.matrixU(); - const MatX& V = svd.matrixV(); - - Index lastRow = singularValues.size() - 1; - Number sigmaMax = singularValues(0); - Number sigmaMin = singularValues(lastRow); - - // refer to https://j-towns.github.io/papers/svd-derivative.pdf - // for analytical gradient of singular values - for (Index i = 0; i < n; i++) { - MatX gradRegroupedObservationMatrix = ridPtr_->pY_pz(i) * regroupMatrix; - - Number gradSigmaMax = U.col(0).transpose() * gradRegroupedObservationMatrix * V.col(0); - Number gradSigmaMin = U.col(lastRow).transpose() * gradRegroupedObservationMatrix * V.col(lastRow); - - grad_f[i] = gradSigmaMax / sigmaMax - gradSigmaMin / sigmaMin; - } - - return true; -} - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/README.md b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/README.md deleted file mode 100644 index 1d117bda..00000000 --- a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Iterative System Identification - -Estimate the optimal system inertial parameter that minimizes the inverse dynamics residual error. \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/README.md b/Examples/Kinova/SystemIdentification/README.md deleted file mode 100644 index a50ba12f..00000000 --- a/Examples/Kinova/SystemIdentification/README.md +++ /dev/null @@ -1 +0,0 @@ -# System Identification Example \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/RegroupMatrix.csv b/Examples/Kinova/SystemIdentification/RegroupMatrix.csv deleted file mode 100644 index 53d95f80..00000000 --- a/Examples/Kinova/SystemIdentification/RegroupMatrix.csv +++ /dev/null @@ -1,91 +0,0 @@ -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.999999999999999 0 0 0.999999999999999 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 --0.999999999999999 0 0 0 0 0 0.999999999999999 0 0 0.999999999999999 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 -0.00637499999999998 0 0.42076 0 0 0.42076 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 --0.01075 0 0 -0.01075 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -2.8890625e-05 0 0 2.8890625e-05 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 1 0 -1 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 -0.21038 0 0 0 0 -0.01275 0 0.01275 0 -1 -0.01275 0 0 -0.01275 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062500000007 0 0 0.000138062500000002 0 0 -0.0442597444 0 0 -0.0442597444 0 0 -0.21038 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.00637500000000002 0 0 0 0.41686 0 0 0.41686 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062500000263 0 0 0.000138062500000025 0 0 0 0 0 0 0 0 -0.42076 0 0 -4.06406249999991e-05 0 4.06406249999991e-05 0 -0.006375 -4.0640625e-05 0 0 -4.06406249999996e-05 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062500000088 0 0 0.000138062500000044 0 0 0 0 0 0 0 0 -0.42076 0 0 0 0 0 0 -0.01275 -0.0434430649000001 0 0 -0.0434430649000001 0 0 0 0 -0.20843 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000350099999999959 0 0 0 -0.10593 -0.000350099999999996 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062499999742 0 0 0.00013806250000009 0 0 0 0 0 0 0 0 -0.42076 0 0 0 0 0 0 -0.01275 -0.0322218693574976 0 0 -0.0322219000000001 1.85430464999976e-05 3.06425025005768e-08 0 0.000175049999999997 -0.31436 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.000175049999999989 0 0 1 -0.21186 0 0 -0.21186 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062499999917 0 0 0.000138062499999967 0 0 0 0 0 0 0 0 -0.42076 0 0 0 0 0 0 -0.01275 -0.03222177742999 0 0 -0.0322219 3.70860930000035e-05 1.22570009996632e-07 0 0.000350099999999987 -0.31436 0 0 -1.85430464999991e-05 0 0 -0.10593 0.0112211649 0 0 0.0112211649 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 --1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 diff --git a/Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv b/Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv deleted file mode 100644 index 21745eb5..00000000 --- a/Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv +++ /dev/null @@ -1,70 +0,0 @@ -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 -1 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 --0.01075 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -2.88906249999997e-05 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0.00637500000000034 0 0 1 -0.42076 0 0 -0.42076 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062499999938 0 0 -0.00134117250000017 0 0 -0.21038 0.0442597443999999 0 0 0.0442597444 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 -0.0127499999999986 0 0 0 -0.210380000000001 -0.0127500000000014 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062499999838 0 0 -0.00268234499999989 0 0 -0.42076 0.177079618225 0 0 0.1770389776 0.00134117250000001 4.06406249999886e-05 0 0.006375 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.416860000000001 0 0 0.00637499999999964 -0.416859999999997 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062500000008 0 0 -0.00268234499999955 0 0 -0.42076 0.1772015401 0 0 0.1770389776 0.00268234500000013 0.000162562500000079 0 0.01275 0.0434430648999997 0 0 -0.00132874124999978 0.0434430649 0 -0.20843 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.999999999999999 0 0.999999999999999 0 0 0.999999999999999 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.999999999999999 0 0 0 1 0 0 0 0 0 0.999999999999999 0 -0.999999999999999 0 0 -0.999999999999999 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000175050000003007 0 0 0.211860000000002 0 0 0.211860000000002 0 0 0 0 1 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000350100000000258 0 0 -0.10593 -0.000350100000000744 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.000138062499999825 0 0 -0.00268234499999955 0 0 -0.42076 0.1772015401 0 0 0.177038977600001 0.00268234500000094 0.00016256250000029 0 0.0127499999999999 0.0988222095999996 0 0 -0.00200404499999922 0.0988222095999996 0 -0.31436 3.0642502618833e-08 0 0 1.85430464999503e-05 3.06425025059477e-08 0 0.000175049999999998 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 -0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -0.000138062500001796 0 0 -0.00268234499999807 0 0 -0.42076 0.1772015401 0 0 0.177038977600001 0.00268234500000081 0.000162562500000196 0 0.0127499999999999 0.0988222095999993 0 0 -0.00200404499999936 0.0988222095999989 0 -0.31436 1.22570010597523e-07 0 0 3.70860930003708e-05 1.22570010072456e-07 0 0.000350099999999977 0 0 0 0 0 -0.0112211649000002 0 0 -0.0112211649000002 0 0 0 0 -0.10593 diff --git a/Examples/Kinova/WaitrUnsafe/KinovaWaitrExample.cpp b/Examples/Kinova/WaitrUnsafe/KinovaWaitrExample.cpp deleted file mode 100644 index 007bc9ed..00000000 --- a/Examples/Kinova/WaitrUnsafe/KinovaWaitrExample.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "KinovaWaitrOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include "CustomizedInverseDynamics.h" - -using namespace RAPTOR; -using namespace Kinova; -using namespace Ipopt; - -int main() { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - const std::string urdf_filename = "../Examples/Kinova/WaitrUnsafe/kinova_grasp.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - const int actual_model_nq = model.nq - 1; - - model.gravity.linear()(2) = -9.81; - model.rotorInertia.setZero(); - model.damping.setZero(); - model.friction.setZero(); - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3, - 0; // the last joint is a fixed joint - - // Define obstacles - const int num_obstacles = 2; - std::vector boxCenters; - std::vector boxOrientation; - std::vector boxSize; - - boxCenters.resize(num_obstacles); - boxOrientation.resize(num_obstacles); - boxSize.resize(num_obstacles); - - for (int i = 0; i < num_obstacles; i++) { - boxCenters[i] << 10, 0, 0.3 * i; - boxOrientation[i] << 0, 0, 0; - boxSize[i] << 0.1, 0.1, 0.1; - } - - // Define trajectories - WaitrTrajectoryParameters atp; - atp.q0 = Eigen::VectorXd::Zero(actual_model_nq); - atp.q_d0 = Eigen::VectorXd::Zero(actual_model_nq); - atp.q_dd0 = Eigen::VectorXd::Zero(actual_model_nq); - - const double T = 1; - const int N = 16; - const int degree = WAITR_BEZIER_CURVE_DEGREE; - - // Define contact surface parameters - contactSurfaceParams csp; - // TODO: define contact surface parameters - - // Define target - Eigen::VectorXd qdes(actual_model_nq); - qdes.setConstant(1.0); - const int tplan_n = N / 2; - - // Define initial guess - Eigen::VectorXd z(actual_model_nq); - z.setZero(); - - // Define limits buffer - Eigen::VectorXd joint_limits_buffer(actual_model_nq); - joint_limits_buffer.setConstant(0.0); - Eigen::VectorXd velocity_limits_buffer(actual_model_nq); - velocity_limits_buffer.setConstant(0.0); - Eigen::VectorXd torque_limits_buffer(actual_model_nq); - torque_limits_buffer.setConstant(0.0); - - // std::shared_ptr trajPtr_ = std::make_shared(T, - // N, - // actual_model_nq, - // Chebyshev, - // atp); - // CustomizedInverseDynamics id(model, jtype, trajPtr_); - - // Eigen::VectorXd x = -5 * Eigen::VectorXd::Ones(actual_model_nq); - // auto start = std::chrono::high_resolution_clock::now(); - // id.compute(x, true); - // auto end = std::chrono::high_resolution_clock::now(); - // std::cout << "Total compute time: " << std::chrono::duration_cast(end - start).count() << " microseconds.\n"; - // const Eigen::MatrixXd& pv_pz = id.ptau_pz(10); - // Eigen::MatrixXd pv_pz_ref = Eigen::MatrixXd::Zero(pv_pz.rows(), pv_pz.cols()); - // for (int i = 0; i < actual_model_nq; i++) { - // Eigen::VectorXd x_plus = x; - // x_plus(i) += 1e-8; - // id.compute(x_plus, false); - // const Eigen::VectorXd v_plus = id.tau(10); - // Eigen::VectorXd x_minus = x; - // x_minus(i) -= 1e-8; - // id.compute(x_minus, false); - // const Eigen::VectorXd v_minus = id.tau(10); - // pv_pz_ref.col(i) = (v_plus - v_minus) / 2e-8; - // } - // // std::cout << "pv_pz: " << pv_pz << std::endl; - // // std::cout << std::endl; - // // std::cout << "pv_pz_ref: " << pv_pz_ref << std::endl; - // std::cout << pv_pz - pv_pz_ref << std::endl; - - // std::cout << std::setprecision(20); - // for (int i = 0; i < N; i++) { - // std::cout << "tau(" << i << ") = " << id.tau(i).transpose() << std::endl; - // } - // return 0; - - // Initialize Kinova optimizer - SmartPtr mynlp = new KinovaWaitrOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - degree, - model, - jtype, - atp, - csp, - boxCenters, - boxOrientation, - boxSize, - qdes, - tplan_n, - joint_limits_buffer, - velocity_limits_buffer, - torque_limits_buffer); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("tol", 1e-4); - app->Options()->SetNumericValue("constr_viol_tol", 1e-4); - // app->Options()->SetNumericValue("obj_scaling_factor", 1e-3); - app->Options()->SetNumericValue("max_wall_time", 0.2); - app->Options()->SetIntegerValue("print_level", 5); - app->Options()->SetIntegerValue("max_iter", 50); - app->Options()->SetStringValue("mu_strategy", "monotone"); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - // For gradient checking - // app->Options()->SetStringValue("output_file", "ipopt.out"); - // app->Options()->SetStringValue("derivative_test", "first-order"); - // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-8); - // app->Options()->SetNumericValue("derivative_test_tol", 1e-5); - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Print the solution - if (mynlp->solution.size() == mynlp->numVars) { - std::ofstream solution("solution-kinova-waitr.txt"); - solution << std::setprecision(20); - for (int i = 0; i < mynlp->numVars; i++) { - solution << mynlp->solution[i] << std::endl; - } - solution.close(); - - std::ofstream trajectory("trajectory-kinova-waitr.txt"); - trajectory << std::setprecision(20); - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < N; j++) { - trajectory << mynlp->trajPtr_->q(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < N; j++) { - trajectory << mynlp->trajPtr_->q_d(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < N; j++) { - trajectory << mynlp->trajPtr_->q_dd(j)(i) << ' '; - } - trajectory << std::endl; - } - for (int i = 0; i < NUM_JOINTS; i++) { - for (int j = 0; j < N; j++) { - trajectory << mynlp->idPtr_->tau(j)(i) << ' '; - } - trajectory << std::endl; - } - trajectory.close(); - } - - return 0; -} diff --git a/Examples/Kinova/WaitrUnsafe/KinovaWaitrPybind.cpp b/Examples/Kinova/WaitrUnsafe/KinovaWaitrPybind.cpp deleted file mode 100644 index bf822a11..00000000 --- a/Examples/Kinova/WaitrUnsafe/KinovaWaitrPybind.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include -#include - -#include "KinovaWaitrPybindWrapper.h" - -namespace py = pybind11; - -using namespace RAPTOR; -using namespace Kinova; - -PYBIND11_MODULE(oracle_waitr_pybind, m) { - m.doc() = "pybind11 oracle_waitr_pybind plugin"; - - py::class_(m, "KinovaWaitrPybindWrapper") - .def(py::init()) - .def("set_obstacles", &KinovaWaitrPybindWrapper::set_obstacles) - .def("set_ipopt_parameters", &KinovaWaitrPybindWrapper::set_ipopt_parameters) - .def("set_trajectory_parameters", &KinovaWaitrPybindWrapper::set_trajectory_parameters) - .def("set_buffer", &KinovaWaitrPybindWrapper::set_buffer) - .def("set_target", &KinovaWaitrPybindWrapper::set_target) - .def("optimize", &KinovaWaitrPybindWrapper::optimize); -} \ No newline at end of file diff --git a/Examples/Kinova/WaitrUnsafe/Kinova_Suction_Plate_Condensed_Oracle.urdf b/Examples/Kinova/WaitrUnsafe/Kinova_Suction_Plate_Condensed_Oracle.urdf deleted file mode 100644 index b466ed05..00000000 --- a/Examples/Kinova/WaitrUnsafe/Kinova_Suction_Plate_Condensed_Oracle.urdf +++ /dev/null @@ -1,279 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Examples/Kinova/WaitrUnsafe/README.md b/Examples/Kinova/WaitrUnsafe/README.md deleted file mode 100644 index d2606b10..00000000 --- a/Examples/Kinova/WaitrUnsafe/README.md +++ /dev/null @@ -1 +0,0 @@ -This folder contains an implementation of an unsafe version of [WAITR](https://roahmlab.github.io/waitr-dev/). \ No newline at end of file diff --git a/Examples/Kinova/WaitrUnsafe/include/KinovaWaitrOptimizer.h b/Examples/Kinova/WaitrUnsafe/include/KinovaWaitrOptimizer.h deleted file mode 100644 index f23300a8..00000000 --- a/Examples/Kinova/WaitrUnsafe/include/KinovaWaitrOptimizer.h +++ /dev/null @@ -1,107 +0,0 @@ -#ifndef KINOVA_WAITR_OPTIMIZER_H -#define KINOVA_WAITR_OPTIMIZER_H - -#include "KinovaCustomizedConstraints.h" -#include "KinovaConstants.h" -#include "CustomizedInverseDynamics.h" -#include "Optimizer.h" -#include "WaitrBezierCurves.h" -#include "JointLimits.h" -#include "VelocityLimits.h" -#include "TorqueLimits.h" -#include "WaitrContactConstraints.h" - -namespace RAPTOR { -namespace Kinova { - -class KinovaWaitrOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - KinovaWaitrOptimizer() = default; - - /** Default destructor */ - ~KinovaWaitrOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const WaitrTrajectoryParameters& atp_input, - const contactSurfaceParams& csp_input, - const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input, - const VecX& qdes_input, - const int tplan_n_input, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - KinovaWaitrOptimizer( - const KinovaWaitrOptimizer& - ); - - KinovaWaitrOptimizer& operator=( - const KinovaWaitrOptimizer& - ); - - std::shared_ptr trajPtr_; - - std::shared_ptr idPtr_; - - VecX qdes; - int tplan_n = 0; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // KINOVA_WAITR_OPTIMIZER_H diff --git a/Examples/Kinova/WaitrUnsafe/include/KinovaWaitrPybindWrapper.h b/Examples/Kinova/WaitrUnsafe/include/KinovaWaitrPybindWrapper.h deleted file mode 100644 index 02c89a82..00000000 --- a/Examples/Kinova/WaitrUnsafe/include/KinovaWaitrPybindWrapper.h +++ /dev/null @@ -1,102 +0,0 @@ -#ifndef KINOVA_WAITR_PYBIND_WRAPPER_H -#define KINOVA_WAITR_PYBIND_WRAPPER_H - -#include -#include - -#include "KinovaWaitrOptimizer.h" - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -namespace RAPTOR { -namespace Kinova { - -namespace py = pybind11; - -class KinovaWaitrPybindWrapper { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - KinovaWaitrPybindWrapper() = default; - - KinovaWaitrPybindWrapper(const std::string urdf_filename); - - // Destructor - ~KinovaWaitrPybindWrapper() = default; - - // Class methods - void set_obstacles(const int num_obstacles_inp, - const py::array_t obstacles_inp); - - void set_ipopt_parameters(const double tol, - const double obj_scaling_factor, - const double max_wall_time, - const int print_level, - const std::string mu_strategy, - const std::string linear_solver, - const bool gradient_check); - - void set_trajectory_parameters(const py::array_t q0_inp, - const py::array_t qd0_inp, - const py::array_t qdd0_inp, - const double duration_inp); - - void set_buffer(const py::array_t joint_limits_buffer_inp, - const py::array_t velocity_limits_buffer_inp, - const py::array_t torque_limits_buffer_inp); - - void set_target(const py::array_t q_des_inp, - const double tplan_inp); - - py::tuple optimize(); - - // Class members - // robot model - Model model; - Eigen::VectorXi jtype; - - int actual_model_nq = 0; - - // obstacle information - int num_obstacles = 0; - std::vector boxCenters; - std::vector boxOrientation; - std::vector boxSize; - - // trajectory information - WaitrTrajectoryParameters atp; - double T = 1; - int N = 16; - int degree = WAITR_BEZIER_CURVE_DEGREE; - VecX qdes; - double tplan = 0; - int tplan_n = 0; - - // contact surface parameters - contactSurfaceParams csp; - - // buffer information - VecX joint_limits_buffer; - VecX velocity_limits_buffer; - VecX torque_limits_buffer; - - SmartPtr mynlp; - SmartPtr app; - - // Flags to check if the parameters are set - bool set_obstacles_check = false; - bool set_ipopt_parameters_check = false; - bool set_trajectory_parameters_check = false; - bool set_buffer_check = false; - bool set_target_check = false; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // KINOVA_WAITR_PYBIND_WRAPPER_H diff --git a/Examples/Kinova/WaitrUnsafe/kinova_grasp.urdf b/Examples/Kinova/WaitrUnsafe/kinova_grasp.urdf deleted file mode 100644 index a46c9fc4..00000000 --- a/Examples/Kinova/WaitrUnsafe/kinova_grasp.urdf +++ /dev/null @@ -1,687 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Examples/Kinova/WaitrUnsafe/src/KinovaWaitrOptimizer.cpp b/Examples/Kinova/WaitrUnsafe/src/KinovaWaitrOptimizer.cpp deleted file mode 100644 index 3b7ebb58..00000000 --- a/Examples/Kinova/WaitrUnsafe/src/KinovaWaitrOptimizer.cpp +++ /dev/null @@ -1,211 +0,0 @@ -#include "KinovaWaitrOptimizer.h" - -namespace RAPTOR { -namespace Kinova { - -// // constructor -// KinovaWaitrOptimizer::KinovaWaitrOptimizer() -// { -// } - - -// // destructor -// KinovaWaitrOptimizer::~KinovaWaitrOptimizer() -// { -// } - -// [TNLP_set_parameters] -bool KinovaWaitrOptimizer::set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const int degree_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input, - const WaitrTrajectoryParameters& atp_input, - const contactSurfaceParams& csp_input, - const std::vector& boxCenters_input, - const std::vector& boxOrientation_input, - const std::vector& boxSize_input, - const VecX& qdes_input, - const int tplan_n_input, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input - ) -{ - x0 = x0_input; - qdes = qdes_input; - tplan_n = tplan_n_input; - - trajPtr_ = std::make_shared(T_input, - N_input, - model_input.nq - 1, - Chebyshev, - atp_input); - - idPtr_ = std::make_shared(model_input, - jtype_input, - trajPtr_); - - // read joint limits from KinovaConstants.h - VecX JOINT_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS) + - joint_limits_buffer_input; - - VecX JOINT_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS) - - joint_limits_buffer_input; - - // read velocity limits from KinovaConstants.h - VecX VELOCITY_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_LOWER, NUM_JOINTS) + - velocity_limits_buffer_input; - - VecX VELOCITY_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_UPPER, NUM_JOINTS) - - velocity_limits_buffer_input; - - // read torque limits from KinovaConstants.h - VecX TORQUE_LIMITS_LOWER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_LOWER, NUM_JOINTS) + - torque_limits_buffer_input; - - VecX TORQUE_LIMITS_UPPER_VEC = Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_UPPER, NUM_JOINTS) - - torque_limits_buffer_input; - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // Velocity limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - VELOCITY_LIMITS_LOWER_VEC, - VELOCITY_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("velocity limits"); - - // Torque limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - idPtr_, - TORQUE_LIMITS_LOWER_VEC, - TORQUE_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("torque limits"); - - // Contact constraints with the object - constraintsPtrVec_.push_back(std::make_unique(idPtr_, - csp_input)); - constraintsNameVec_.push_back("contact constraints"); - - // Customized constraints (collision avoidance with obstacles) - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - model_input, - jtype_input, - boxCenters_input, - boxOrientation_input, - boxSize_input)); - constraintsNameVec_.push_back("obstacle avoidance constraints"); - - assert(x0.size() == trajPtr_->varLength); - assert(qdes.size() == trajPtr_->Nact); - assert(tplan_n >= 0 && tplan_n < trajPtr_->N); - - return true; -} -// [TNLP_set_parameters] - -// [TNLP_get_nlp_info] -// returns some info about the nlp -bool KinovaWaitrOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) -{ - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} -// [TNLP_get_nlp_info] - -// [TNLP_eval_f] -// returns the value of the objective function -bool KinovaWaitrOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, false); - - const VecX& qplan = trajPtr_->q(tplan_n); - - obj_value = pow(Utils::wrapToPi(qplan[0] - qdes[0]), 2) + // These are continuous joints - pow(Utils::wrapToPi(qplan[2] - qdes[2]), 2) + - pow(Utils::wrapToPi(qplan[4] - qdes[4]), 2) + - pow(Utils::wrapToPi(qplan[6] - qdes[6]), 2) + - pow(qplan[1] - qdes[1], 2) + // These are not continuous joints - pow(qplan[3] - qdes[3], 2) + - pow(qplan[5] - qdes[5], 2); - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} -// [TNLP_eval_f] - -// [TNLP_eval_grad_f] -// return the gradient of the objective function grad_{x} f(x) -bool KinovaWaitrOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) -{ - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - trajPtr_->compute(z, true); - - const VecX& qplan = trajPtr_->q(tplan_n); - const MatX& pqplan_pz = trajPtr_->pq_pz(tplan_n); - - for(Index i = 0; i < n; i++){ - if (i % 2 == 0) { - grad_f[i] = (2 * Utils::wrapToPi(qplan[i] - qdes[i]) * pqplan_pz(i, i)); - } - else { - grad_f[i] = (2 * (qplan[i] - qdes[i]) * pqplan_pz(i, i)); - } - } - - return true; -} -// [TNLP_eval_grad_f] - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Examples/Kinova/WaitrUnsafe/src/KinovaWaitrPybindWrapper.cpp b/Examples/Kinova/WaitrUnsafe/src/KinovaWaitrPybindWrapper.cpp deleted file mode 100644 index 9874d963..00000000 --- a/Examples/Kinova/WaitrUnsafe/src/KinovaWaitrPybindWrapper.cpp +++ /dev/null @@ -1,269 +0,0 @@ -#include "KinovaWaitrPybindWrapper.h" - -namespace RAPTOR { -namespace Kinova { - -KinovaWaitrPybindWrapper::KinovaWaitrPybindWrapper(const std::string urdf_filename) { - // set openmp number of threads - int num_threads = 32; // this number is currently hardcoded - omp_set_num_threads(num_threads); - - // Define robot model - pinocchio::urdf::buildModel(urdf_filename, model); - - actual_model_nq = model.nq - 1; - - model.gravity.linear()(2) = -9.81; - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - jtype.resize(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3, - 3; - - qdes.resize(actual_model_nq); - - mynlp = new KinovaWaitrOptimizer(); - app = IpoptApplicationFactory(); - - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - - joint_limits_buffer = VecX::Zero(actual_model_nq); - velocity_limits_buffer = VecX::Zero(actual_model_nq); - torque_limits_buffer = VecX::Zero(actual_model_nq); - - // TODO: define contact surface parameters - // csp.mu = 0.5; -} - -void KinovaWaitrPybindWrapper::set_obstacles(const int num_obstacles_inp, - const py::array_t obstacles_inp) { - num_obstacles = num_obstacles_inp; - - if (num_obstacles < 0) { - throw std::invalid_argument("Number of obstacles must be non-negative"); - } - - py::buffer_info obstacles_buf = obstacles_inp.request(); - - if (obstacles_buf.size != num_obstacles * (3 + 3 + 3)) { - throw std::invalid_argument("Number of obstacles does not match the input array size"); - } - - const double* obstacles_ptr = (const double*)obstacles_buf.ptr; - - boxCenters.resize(num_obstacles); - boxOrientation.resize(num_obstacles); - boxSize.resize(num_obstacles); - - for (int i = 0; i < num_obstacles; i++) { - boxCenters[i] << obstacles_ptr[i * 9 + 0], - obstacles_ptr[i * 9 + 1], - obstacles_ptr[i * 9 + 2]; - boxOrientation[i] << obstacles_ptr[i * 9 + 3], - obstacles_ptr[i * 9 + 4], - obstacles_ptr[i * 9 + 5]; - boxSize[i] << obstacles_ptr[i * 9 + 6], - obstacles_ptr[i * 9 + 7], - obstacles_ptr[i * 9 + 8]; - } - - set_obstacles_check = true; -} - -void KinovaWaitrPybindWrapper::set_ipopt_parameters(const double tol, - const double obj_scaling_factor, - const double max_wall_time, - const int print_level, - const std::string mu_strategy, - const std::string linear_solver, - const bool gradient_check) { - app->Options()->SetNumericValue("tol", tol); - app->Options()->SetNumericValue("obj_scaling_factor", obj_scaling_factor); - app->Options()->SetNumericValue("max_wall_time", max_wall_time); - app->Options()->SetIntegerValue("print_level", print_level); - app->Options()->SetStringValue("mu_strategy", mu_strategy); - app->Options()->SetStringValue("linear_solver", linear_solver); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - - if (gradient_check) { - app->Options()->SetStringValue("output_file", "ipopt.out"); - app->Options()->SetStringValue("derivative_test", "first-order"); - app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - app->Options()->SetNumericValue("derivative_test_tol", 1e-6); - } - - set_ipopt_parameters_check = true; -} - -void KinovaWaitrPybindWrapper::set_trajectory_parameters(const py::array_t q0_inp, - const py::array_t qd0_inp, - const py::array_t qdd0_inp, - const double duration_inp) { - py::buffer_info q0_buf = q0_inp.request(); - py::buffer_info qd0_buf = qd0_inp.request(); - py::buffer_info qdd0_buf = qdd0_inp.request(); - - T = duration_inp; - - if (q0_buf.size != actual_model_nq) { - throw std::invalid_argument("q0 must be of size actual_model_nq"); - } - - if (qd0_buf.size != actual_model_nq) { - throw std::invalid_argument("qd0 must be of size actual_model_nq"); - } - - if (qdd0_buf.size != actual_model_nq) { - throw std::invalid_argument("qdd0 must be of size actual_model_nq"); - } - - if (T <= 0.0) { - throw std::invalid_argument("Duration must be positive"); - } - - const double *q0_ptr = (const double*)q0_buf.ptr; - const double *qd0_ptr = (const double*)qd0_buf.ptr; - const double *qdd0_ptr = (const double*)qdd0_buf.ptr; - - atp.q0.resize(actual_model_nq); - atp.q_d0.resize(actual_model_nq); - atp.q_dd0.resize(actual_model_nq); - - for (int i = 0; i < actual_model_nq; i++) { - atp.q0(i) = q0_ptr[i]; - atp.q_d0(i) = qd0_ptr[i]; - atp.q_dd0(i) = qdd0_ptr[i]; - } - - set_trajectory_parameters_check = true; -} - -void KinovaWaitrPybindWrapper::set_buffer(const py::array_t joint_limits_buffer_inp, - const py::array_t velocity_limits_buffer_inp, - const py::array_t torque_limits_buffer_inp) { - py::buffer_info joint_limits_buffer_buf = joint_limits_buffer_inp.request(); - py::buffer_info velocity_limits_buffer_buf = velocity_limits_buffer_inp.request(); - py::buffer_info torque_limits_buffer_buf = torque_limits_buffer_inp.request(); - - if (joint_limits_buffer_buf.size != actual_model_nq) { - throw std::invalid_argument("joint_limits_buffer must be of size actual_model_nq"); - } - - if (velocity_limits_buffer_buf.size != actual_model_nq) { - throw std::invalid_argument("velocity_limits_buffer must be of size actual_model_nq"); - } - - if (torque_limits_buffer_buf.size != actual_model_nq) { - throw std::invalid_argument("torque_limits_buffer must be of size actual_model_nq"); - } - - const double* joint_limits_buffer_ptr = (const double*)joint_limits_buffer_buf.ptr; - const double* velocity_limits_buffer_ptr = (const double*)velocity_limits_buffer_buf.ptr; - const double* torque_limits_buffer_ptr = (const double*)torque_limits_buffer_buf.ptr; - - for (int i = 0; i < actual_model_nq; i++) { - joint_limits_buffer(i) = joint_limits_buffer_ptr[i]; - velocity_limits_buffer(i) = velocity_limits_buffer_ptr[i]; - torque_limits_buffer(i) = torque_limits_buffer_ptr[i]; - } - - set_buffer_check = true; -} - -void KinovaWaitrPybindWrapper::set_target(const py::array_t q_des_inp, - const double tplan_inp) { - tplan = tplan_inp; - - if (tplan <= 0.0 || tplan > T) { - throw std::invalid_argument("tplan must be greater than 0.0 or smaller than duration"); - } - - py::buffer_info q_des_buf = q_des_inp.request(); - - if (q_des_buf.size != actual_model_nq) { - throw std::invalid_argument("q_des must be of size actual_model_nq"); - } - - const double *q_des_ptr = (const double*)q_des_buf.ptr; - - for (int i = 0; i < actual_model_nq; i++) { - qdes(i) = q_des_ptr[i]; - } - tplan_n = int(tplan / T * N); - - set_target_check = true; -} - -py::tuple KinovaWaitrPybindWrapper::optimize() { - if (!set_obstacles_check || - !set_ipopt_parameters_check || - !set_trajectory_parameters_check || - !set_buffer_check || - !set_target_check) { - throw std::runtime_error("parameters not set properly!"); - } - - // Define initial guess - Eigen::VectorXd z(actual_model_nq); - z.setZero(); - - // Initialize Kinova optimizer - try { - mynlp->set_parameters(z, - T, - N, - degree, - model, - jtype, - atp, - csp, - boxCenters, - boxOrientation, - boxSize, - qdes, - tplan_n, - joint_limits_buffer, - velocity_limits_buffer, - torque_limits_buffer); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - set_ipopt_parameters_check = false; - set_trajectory_parameters_check = false; - set_target_check = false; - - py::array_t result(actual_model_nq, mynlp->solution.data()); - return py::make_tuple(result, mynlp->ifFeasible); -} - -}; // namespace Kinova -}; // namespace RAPTOR diff --git a/KinematicsDynamics/README.md b/KinematicsDynamics/README.md deleted file mode 100644 index dd41d91f..00000000 --- a/KinematicsDynamics/README.md +++ /dev/null @@ -1,10 +0,0 @@ -# Kinematics & Dynamics - -For some of the constraints, such as end effector position, or torque limits, we need to compute the forward kinematics and inverse dynamics of the robot. -The core function is also called `compute(const Eigen::VectorXd& z, bool compute_derivatives)` - - - The forward kinematics or the inverse dynamics class usually requires a shared pointer of a trajectory class in the constructor. - - In `compute`, the `compute` of the trajectory class will first be called, to update the trajectories of all joints. - - Using `q`, `q_d`, `q_dd` in the trajectory class, we will compute the forward kinematics of each joints or the torque requried, depending on which kinematics/dynamics class it is. - - If `compute_derivatives` is true, we will use `pq_pz`, `pq_d_pz`, `pq_dd_pz` in the trajectory class to compute the gradient of the forward kinematics or inverse dynamics with respect to the decision variable `z`. - - The results will be stored inside the class for the constraints classes to access. \ No newline at end of file diff --git a/KinematicsDynamics/include/ConstrainedInverseDynamics.h b/KinematicsDynamics/include/ConstrainedInverseDynamics.h deleted file mode 100644 index 333652f0..00000000 --- a/KinematicsDynamics/include/ConstrainedInverseDynamics.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef CONSTRAINEDINVERSEDYNAMICS_H -#define CONSTRAINEDINVERSEDYNAMICS_H - -#include "DynamicsConstraints.h" - -namespace RAPTOR { - -// Class declaration -class ConstrainedInverseDynamics : public InverseDynamics { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = MatX; - - // Constructor - ConstrainedInverseDynamics() = default; - - // Constructor - ConstrainedInverseDynamics(const Model& model_input, - std::shared_ptr& trajPtr_input, - int numDependentJoints_input); - - // Destructor - ~ConstrainedInverseDynamics() = default; - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // class members: - int numDependentJoints = 0; - int numIndependentJoints = 0; - - // declare a DynamicsConstraints instance outside of this class - // use a shared pointer here to avoid copying - std::shared_ptr dcPtr_; - - // updated in compute() - VecX tau_dep; - VecX tau_indep; - - // compute results are stored here - Eigen::Array q; - Eigen::Array v; - Eigen::Array a; - - Eigen::Array pq_pz; - Eigen::Array pv_pz; - Eigen::Array pa_pz; - - // compute results are stored here - Eigen::Array lambda; - - // compute results are stored here - Eigen::Array plambda_pz; - - // temporary variables updated in compute() - MatX prnea_pq_dep; - MatX prnea_pq_indep; - MatX prnea_pv_dep; - MatX prnea_pv_indep; - MatX prnea_pa_dep; - MatX prnea_pa_indep; - - MatX plambda_pq; - MatX plambda_pv; - MatX plambda_pa; - - MatX JTlambda_partial_dq; - MatX JTlambda_partial_dq_dep; - MatX JTlambda_partial_dq_indep; -}; - -}; // namespace RAPTOR - -#endif // CONSTRAINEDINVERSEDYNAMICS_H diff --git a/KinematicsDynamics/include/CustomizedInverseDynamics.h b/KinematicsDynamics/include/CustomizedInverseDynamics.h deleted file mode 100644 index 1e80cec8..00000000 --- a/KinematicsDynamics/include/CustomizedInverseDynamics.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef CUSTOMIZED_INVERSEDYNAMICS_H -#define CUSTOMIZED_INVERSEDYNAMICS_H - -#include -#include - -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include "InverseDynamics.h" -#include "Spatial.h" -#include "Trajectories.h" - -#include -#include -#include -#include -#include - -namespace RAPTOR { - -// rewrite the inverse dynamics using the original Roy Featherstone's algorithm -// so that we can get the force and the gradient of the force on the fixed joint -class CustomizedInverseDynamics : public InverseDynamics { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - using Vec6 = Vector6d; - using Mat6 = Matrix6d; - - // Constructor - CustomizedInverseDynamics() = default; - - // Constructor - CustomizedInverseDynamics(const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::shared_ptr& trajPtr_input); - - // Destructor - ~CustomizedInverseDynamics() = default; - - // class methods: - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // class members: - Eigen::VectorXi jtype; - Eigen::Array Xtree; - Eigen::Array I; - Vec6 a_grav; - - Eigen::Array Xup; - Eigen::Array dXupdq; - Eigen::Array S; - Eigen::Array v; - Eigen::Array pv_pz; - Eigen::Array a; - Eigen::Array pa_pz; - Eigen::Array f; - Eigen::Array pf_pz; - - // compute results are stored here - Eigen::Array lambda; - Eigen::Array plambda_pz; -}; - -}; // namespace RAPTOR - -#endif // CUSTOMIZED_INVERSEDYNAMICS_H diff --git a/KinematicsDynamics/include/DynamicsConstraints.h b/KinematicsDynamics/include/DynamicsConstraints.h deleted file mode 100644 index 31792234..00000000 --- a/KinematicsDynamics/include/DynamicsConstraints.h +++ /dev/null @@ -1,182 +0,0 @@ -#ifndef DYNAMICSCONSTRAINTS_H -#define DYNAMICSCONSTRAINTS_H - -#include "InverseDynamics.h" -#include - -namespace RAPTOR { - -constexpr int MAX_BUFFER_SIZE = 128; - -class DynamicsConstraints { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - using QRSolver = Eigen::ColPivHouseholderQR; - - struct BufferData { - VecX q; - VecX v; - VecX a; - bool compute_derivatives = false; - - // used outside of this class, so we need to save it - MatX J_dep; - MatX J_indep; - QRSolver J_dep_qr; - QRSolver J_dep_T_qr; - - MatX pq_dep_pq_indep; - MatX pv_dep_pq; - MatX pv_dep_pv_indep; - MatX pa_dep_pq; - MatX pa_dep_pv; - MatX pa_dep_pa_indep; - }; - - // Constructor - DynamicsConstraints() = default; - - // Constructor - DynamicsConstraints(const int numJoints_input, int numDependentJoints_input); - - // Destructor - ~DynamicsConstraints() = default; - - // class methods: - // You need to implement all following methods in your derived class!!! - // return the index of id th dependent joint - virtual int return_dependent_joint_index(const int id) = 0; - - // return the index of id th independent joint - virtual int return_independent_joint_index(const int id) = 0; - - // fill in dependent indeces in a vector - virtual void fill_dependent_vector(VecX& r, const VecX& v, const bool setZero = false) = 0; - - // fill in independent indeces in a vector - virtual void fill_independent_vector(VecX& r, const VecX& v, const bool setZero = false) = 0; - - // fill in dependent columns in a matrix - virtual void fill_dependent_columns(MatX& r, const MatX& m, const bool setZero = false) = 0; - - // fill in independent columns in a matrix - virtual void fill_independent_columns(MatX& r, const MatX& m, const bool setZero = false) = 0; - - // fill in dependent rows in a matrix - virtual void fill_dependent_rows(MatX& r, const MatX& m, const bool setZero = false) = 0; - - // fill in independent rows in a matrix - virtual void fill_independent_rows(MatX& r, const MatX& m, const bool setZero = false) = 0; - - // return dependent indeces in a vector - virtual VecX get_dependent_vector(const VecX& v) = 0; - - // return independent indeces in a vector - virtual VecX get_independent_vector(const VecX& v) = 0; - - // return dependent columns in a matrix - virtual void get_dependent_columns(MatX& r, const MatX& m) = 0; - - // return independent columns in a matrix - virtual void get_independent_columns(MatX& r, const MatX& m) = 0; - - // return dependent rows in a matrix - virtual void get_dependent_rows(MatX& r, const MatX& m) = 0; - - // return independent rows in a matrix - virtual void get_independent_rows(MatX& r, const MatX& m) = 0; - - // class methods: - // fill in dependent joint positions in the full joint vector q - // that satisfies the constraints - // This usually involves solving inverse kinematics. - // You need to implement this method in your derived class!!! - virtual void setupJointPosition(VecX& q, bool compute_derivatives = true) = 0; - - // fill in dependent joint positions and velocities in the full joint vector q and v - // that satisfies the constraints - // this seems to useless currently - // virtual void setupJointPositionVelocity(VecX& q, VecX& v, bool compute_derivatives = true) = 0; - - // fill in dependent joint positions, velocities, and accelerations in the full joint vector q, v, and a - // that satisfies the constraints - virtual void setupJointPositionVelocityAcceleration(VecX& q, VecX& v, VecX& a, bool compute_derivatives = true); - - // constraint c(q) - virtual void get_c(const VecX& q) = 0; - - // J = jacobian(c, q) - virtual void get_J(const VecX& q) = 0; - - // Jx_partial_dq = jacobian(J * x, q) - // where x is a vector that is not dependent on q - virtual void get_Jx_partial_dq(const VecX& q, const VecX& x) = 0; - - // JTx_partial_dq = jacobian(J^T * x, q) - // where x is a vector that is not dependent on q - virtual void get_JTx_partial_dq(const VecX& q, const VecX& x) = 0; - - // Jxy_partial_dq = jacobian(jacobian(J * x, q) * y, q) - // where x and y are vectors that are not dependent on q - virtual void get_Jxy_partial_dq(const VecX& q, const VecX& x, const VecX& y) = 0; - - // The functions above are very expensive, we save the results in a queue - // and return the data if it is saved before - virtual bool recoverSavedData(VecX& q, bool compute_derivatives); - virtual bool recoverSavedData(VecX& q, VecX& v, VecX& a, bool compute_derivatives); - - virtual void updateQueue(const VecX& q, bool compute_derivatives); - virtual void updateQueue(const VecX& q, const VecX& v, const VecX& a, bool compute_derivatives); - - bool ifIndependentPartEqual(const VecX& a, const VecX& b) { - if (a.size() != b.size()) { - return false; - } - - return Utils::ifTwoVectorEqual(get_independent_vector(a), - get_independent_vector(b), - 0); - } - - // class members: - int numJoints = 0; - int numDependentJoints = 0; - int numIndependentJoints = 0; - - // compute results are stored here - VecX c; - MatX J; - MatX Jx_partial_dq; - MatX JTx_partial_dq; - MatX Jxy_partial_dq; - - MatX pq_dep_pq_indep; - MatX pv_dep_pq; - MatX pv_dep_pv_indep; - MatX pa_dep_pq; - MatX pa_dep_pv; - MatX pa_dep_pa_indep; - - std::deque bufferDataQueue; - - // updated in setupJointPositionVelocityAcceleration() - MatX J_dep; - MatX J_indep; - - // updated in setupJointPositionVelocityAcceleration() - QRSolver J_dep_qr; - QRSolver J_dep_T_qr; - - // temporary variables updated in setupJointPositionVelocityAcceleration() - MatX P_dep; - VecX Pa_indep; - MatX temp1; - VecX temp2_1; - MatX temp2; - MatX temp3; -}; - -}; // namespace RAPTOR - -#endif // DYNAMICSCONSTRAINTS_H diff --git a/KinematicsDynamics/include/ForwardKinematics.h b/KinematicsDynamics/include/ForwardKinematics.h deleted file mode 100644 index 685fdb2b..00000000 --- a/KinematicsDynamics/include/ForwardKinematics.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef FORWARD_KINEMATICS_HPP -#define FORWARD_KINEMATICS_HPP - -#include "Transform.h" -#include "HigherOrderDerivatives.h" - -namespace RAPTOR { - -class ForwardKinematicsSolver { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - ForwardKinematicsSolver() = default; - - ForwardKinematicsSolver(const Model* model_input, - const Eigen::VectorXi& jtype_input); - - // Destructor - ~ForwardKinematicsSolver() = default; - - // class methods: - // compute forward kinematics and its high-order derivatives - void compute(const int start, - const int end, - const VecX& q, - const Transform* startT = nullptr, - const Transform* endT = nullptr, - const int order = 0); - - // get the forward kinematics result in different formats - Transform getTransform() const; - - Vec3 getTranslation() const; - - Mat3 getRotation() const; - - Vec3 getRPY() const; // roll, pitch, yaw - - MatX getTranslationJacobian() const; - - void getRotationJacobian(Eigen::Array& result) const; - - MatX getRPYJacobian() const; - - void getTranslationHessian(Eigen::Array& result) const; - - void getRotationHessian(Eigen::Array& result) const; - void getRotationHessian(Eigen::Array& result) const; - - void getRPYHessian(Eigen::Array& result) const; - - // return third-order-tensor * x, which is a 2D matrix - void getTranslationThirdOrderTensor(const VecX& x, Eigen::Array& result) const; - - // return third-order-tensor * x, which is a 2D matrix - void getRPYThirdOrderTensor(const VecX& x, Eigen::Array& result) const; - - // class members: - const Model* modelPtr_ = nullptr; - Eigen::VectorXi jtype; - - // a index vector that stores the kinematics chain - std::vector chain; - - // results - Transform T; - std::vector dTdq; - std::vector> ddTddq; - std::vector>> dddTdddq; - - // // internal copies - // int current_order = -1; - // int end_copy = -1; - // int start_copy = -1; - // VecX q_copy; - // Transform T_copy; - // std::vector dTdq_copy; - // std::vector> ddTddq_copy; - // std::vector>> dddTdddq_copy; -}; - -} // namespace RAPTOR - -#endif \ No newline at end of file diff --git a/KinematicsDynamics/include/InverseDynamics.h b/KinematicsDynamics/include/InverseDynamics.h deleted file mode 100644 index 67f12101..00000000 --- a/KinematicsDynamics/include/InverseDynamics.h +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef INVERSEDYNAMICS_H -#define INVERSEDYNAMICS_H - -#include -#include - -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/jacobian.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include "pinocchio/algorithm/kinematics-derivatives.hpp" -#include "pinocchio/algorithm/rnea.hpp" -#include "pinocchio/algorithm/rnea-derivatives.hpp" -#include "pinocchio/algorithm/crba.hpp" - -#include "Trajectories.h" - -#include -#include -#include -#include -#include - -namespace RAPTOR { - -class InverseDynamics { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - InverseDynamics() = default; - - // Constructor - InverseDynamics(const Model& model_input, - const std::shared_ptr& trajPtr_input); - - // Destructor - ~InverseDynamics() = default; - - // class methods: - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - // determine if the constraints are computed before and save the current decision variable - bool is_computed(const VecX& z, - bool compute_derivatives, - bool compute_hessian); - - // class members: - std::shared_ptr modelPtr_ = nullptr; - std::shared_ptr dataPtr_ = nullptr; - - std::shared_ptr trajPtr_ = nullptr; - - int N = 0; // number of time instances in tspan - - // the decision variable that was evaluated last time - VecX current_z; - bool if_compute_derivatives = false; - bool if_compute_hessian = false; - - MatX prnea_pq; - MatX prnea_pv; - MatX prnea_pa; - - // compute results are stored here - Eigen::Array tau; - - // compute results are stored here - Eigen::Array ptau_pz; -}; - -}; // namespace RAPTOR - -#endif // INVERSEDYNAMICS_H diff --git a/KinematicsDynamics/include/RegressorInverseDynamics.h b/KinematicsDynamics/include/RegressorInverseDynamics.h deleted file mode 100644 index 345ead8e..00000000 --- a/KinematicsDynamics/include/RegressorInverseDynamics.h +++ /dev/null @@ -1,84 +0,0 @@ -#ifndef REGRESSOR_INVERSEDYNAMICS_H -#define REGRESSOR_INVERSEDYNAMICS_H - -#include -#include - -#include "pinocchio/parsers/urdf.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" - -#include "CustomizedInverseDynamics.h" -#include "Spatial.h" -#include "Trajectories.h" - -#include -#include -#include -#include -#include - -namespace RAPTOR { - -// Compute inverse dynamics using tau = Y * phi, -// where Y is the n x 10*n regressor matrix and -// phi is the vector of 10*n dynamic parameters (inertia, com, mass for each of the link). -// phi is constant and directly loaded from the robot. -// The gradient of Y will also be computed. -class RegressorInverseDynamics : public InverseDynamics { -public: - using Model = pinocchio::Model; - using Data = pinocchio::Data; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using Vec6 = Vector6d; - using Mat6 = Matrix6d; - - // Constructor - RegressorInverseDynamics() = default; - - // Constructor - RegressorInverseDynamics(const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::shared_ptr& trajPtr_input); - - // Destructor - ~RegressorInverseDynamics() = default; - - // class methods: - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // class members: - Eigen::VectorXi jtype; - Eigen::Array Xtree; - Eigen::Array I; - Vec6 a_grav; - - Eigen::Array Xup; - Eigen::Array dXupdq; - Eigen::Array S; - Eigen::Array v; - Eigen::Array pv_pz; - Eigen::Array a; - Eigen::Array pa_pz; - - VecX phi; - - int numInertialParams = 0; - int numParams = 0; - - MatX Y; - Eigen::Array pY_pz; - - MatX Yfull; - Eigen::Array pYfull_pz; - - MatX Ycurrent; -}; - -}; // namespace RAPTOR - -#endif // REGRESSOR_INVERSEDYNAMICS_H \ No newline at end of file diff --git a/KinematicsDynamics/include/Spatial.h b/KinematicsDynamics/include/Spatial.h deleted file mode 100644 index 7de9484e..00000000 --- a/KinematicsDynamics/include/Spatial.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef SPATIAL_HPP -#define SPATIAL_HPP - -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/jacobian.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include "pinocchio/algorithm/kinematics-derivatives.hpp" - -#include -#include -#include - -namespace RAPTOR { - -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Vector6d; - -// [Xj,S] = jcalc( jtyp, q ) -void jcalc(Matrix6d& Xj, - Vector6d& S, - const int jtyp, - const double q); - -void jcalc(Matrix6d& Xj, - Matrix6d& dXjdq, - Vector6d& S, - const int jtyp, - const double q); - -void jcalc(Matrix6d& Xj, - Matrix6d& dXjdt, - Vector6d& S, - const int jtyp, - const double q, - const double q_d); - -Matrix6d crm(const Vector6d& v); - -Matrix6d crf(const Vector6d& v); - -}; // namespace RAPTOR - -#endif // SPATIAL_HPP diff --git a/KinematicsDynamics/include/Transform.h b/KinematicsDynamics/include/Transform.h deleted file mode 100644 index b712393e..00000000 --- a/KinematicsDynamics/include/Transform.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef TRANSFORM_H -#define TRANSFORM_H - -#include "pinocchio/parsers/urdf.hpp" - -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/jacobian.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include "pinocchio/algorithm/kinematics-derivatives.hpp" - -#include "HigherOrderDerivatives.h" - -#include -#include -#include - -namespace RAPTOR { - -// 6x6 spatial transform matrix -class Transform { -public: - using SE3 = pinocchio::SE3Tpl; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - - // Constructor - Transform(); - - // Constructor - Transform(const Mat3& R_in, - const Vec3& p_in, - const bool i_in = false); - - // Constructor - Transform(const Vec3& rpy_in, - const Vec3& p_in); - - // Constructor - Transform(const Vec3& p_in); - - // Constructor - Transform(const int jtype, - const double theta, - const int order = 0); - - // Destructor - ~Transform() = default; - - // class methods: - Transform operator=(const Transform& x); - - Transform operator*(const Transform& x) const; - - Transform operator*=(const Transform& x); - - Transform operator*(const SE3& x) const; - - Transform operator*=(const SE3& x); - - Transform inverse() const; - - Vec3 getRPY() const; - - Eigen::Matrix getXYZRPY() const; - - // class members: - Mat3 R; // rotation matrix - Vec3 p; // translation vector - - // this is like the right bottom entry of a 4x4 transformation matrix. - // usually it should be 1, but for derivatives, it should be 0. - bool ifDerivative = false; -}; - -Transform operator*(const pinocchio::SE3Tpl& x, const Transform& sRp); - -std::ostream& operator<<(std::ostream& os, const Transform& sRp); - -} // namespace RAPTOR - -#endif \ No newline at end of file diff --git a/KinematicsDynamics/src/ConstrainedInverseDynamics.cpp b/KinematicsDynamics/src/ConstrainedInverseDynamics.cpp deleted file mode 100644 index b059b6be..00000000 --- a/KinematicsDynamics/src/ConstrainedInverseDynamics.cpp +++ /dev/null @@ -1,194 +0,0 @@ -#include "ConstrainedInverseDynamics.h" - -namespace RAPTOR { - -ConstrainedInverseDynamics::ConstrainedInverseDynamics(const Model& model_input, - std::shared_ptr& trajPtr_input, - int numDependentJoints_input) : - InverseDynamics(model_input, trajPtr_input), - numDependentJoints(numDependentJoints_input) { - numIndependentJoints = modelPtr_->nv - numDependentJoints; - - tau_dep = VecX::Zero(numDependentJoints); - tau_indep = VecX::Zero(numIndependentJoints); - - lambda.resize(1, N); - plambda_pz.resize(1, N); - - q.resize(1, N); - v.resize(1, N); - a.resize(1, N); - - pq_pz.resize(1, N); - pv_pz.resize(1, N); - pa_pz.resize(1, N); - - prnea_pq_dep = MatX::Zero(numDependentJoints, modelPtr_->nv); - prnea_pq_indep = MatX::Zero(numIndependentJoints, modelPtr_->nv); - prnea_pv_dep = MatX::Zero(numDependentJoints, modelPtr_->nv); - prnea_pv_indep = MatX::Zero(numIndependentJoints, modelPtr_->nv); - prnea_pa_dep = MatX::Zero(numDependentJoints, modelPtr_->nv); - prnea_pa_indep = MatX::Zero(numIndependentJoints, modelPtr_->nv); - - plambda_pq = MatX::Zero(numDependentJoints, modelPtr_->nv); - plambda_pv = MatX::Zero(numDependentJoints, modelPtr_->nv); - plambda_pa = MatX::Zero(numDependentJoints, modelPtr_->nv); - - JTlambda_partial_dq = MatX::Zero(modelPtr_->nv, modelPtr_->nv); - JTlambda_partial_dq_dep = MatX::Zero(numDependentJoints, modelPtr_->nv); - JTlambda_partial_dq_indep = MatX::Zero(numIndependentJoints, modelPtr_->nv); - - // dcPtr_ is still empty at this moment!!! - // You need to define it in the constructor of your own derived class!!! -} - -void ConstrainedInverseDynamics::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (trajPtr_ == nullptr) { - throw std::runtime_error("trajPtr_ is not defined yet!"); - } - - if (dcPtr_ == nullptr) { - throw std::runtime_error("dcPtr_ is not defined yet!"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - if (compute_hessian) { - throw std::invalid_argument("ConstrainedInverseDynamics does not support hessian computation"); - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - for (int i = 0; i < N; i++) { - // fill in independent indeces in a vector - q(i) = VecX::Zero(modelPtr_->nv); - v(i) = VecX::Zero(modelPtr_->nv); - a(i) = VecX::Zero(modelPtr_->nv); - dcPtr_->fill_independent_vector(q(i), trajPtr_->q(i)); - dcPtr_->fill_independent_vector(v(i), trajPtr_->q_d(i)); - dcPtr_->fill_independent_vector(a(i), trajPtr_->q_dd(i)); - - // always call this first to update dcPtr_->J_dep and dcPtr_->J_indep!!! - dcPtr_->setupJointPositionVelocityAcceleration(q(i), v(i), a(i), compute_derivatives); - - if (compute_derivatives) { - pq_pz(i) = MatX::Zero(modelPtr_->nv, trajPtr_->varLength); - pv_pz(i) = MatX::Zero(modelPtr_->nv, trajPtr_->varLength); - pa_pz(i) = MatX::Zero(modelPtr_->nv, trajPtr_->varLength); - - // fill in independent joints derivatives directly - for (int j = 0; j < dcPtr_->numIndependentJoints; j++) { - int indenpendentJointIndex = dcPtr_->return_independent_joint_index(j); - pq_pz(i).row(indenpendentJointIndex) = trajPtr_->pq_pz(i).row(j); - pv_pz(i).row(indenpendentJointIndex) = trajPtr_->pq_d_pz(i).row(j); - pa_pz(i).row(indenpendentJointIndex) = trajPtr_->pq_dd_pz(i).row(j); - } - - // derivatives of unactuated joint positions - MatX pq_dep_pz = dcPtr_->pq_dep_pq_indep * trajPtr_->pq_pz(i); - - for (int j = 0; j < dcPtr_->numDependentJoints; j++) { - int denpendentJointIndex = dcPtr_->return_dependent_joint_index(j); - pq_pz(i).row(denpendentJointIndex) = pq_dep_pz.row(j); - } - - // derivatives of unactuated joint velocities - MatX pv_dep_pz = dcPtr_->pv_dep_pq * pq_pz(i) + // all joint positions - dcPtr_->pv_dep_pv_indep * trajPtr_->pq_d_pz(i); // actuated joint velocities - - for (int j = 0; j < dcPtr_->numDependentJoints; j++) { - int denpendentJointIndex = dcPtr_->return_dependent_joint_index(j); - pv_pz(i).row(denpendentJointIndex) = pv_dep_pz.row(j); - } - - // derivatives of unactuated joint accelerations - MatX pa_dep_pz = dcPtr_->pa_dep_pq * pq_pz(i) + // all joint positions - dcPtr_->pa_dep_pv * pv_pz(i) + // all joint velocities - dcPtr_->pa_dep_pa_indep * trajPtr_->pq_dd_pz(i); // actuated joint accelerations - - for (int j = 0; j < dcPtr_->numDependentJoints; j++) { - int denpendentJointIndex = dcPtr_->return_dependent_joint_index(j); - pa_pz(i).row(denpendentJointIndex) = pa_dep_pz.row(j); - } - } - - // compute unconstrained inverse dynamics - if (!compute_derivatives) { - pinocchio::rnea(*modelPtr_, *dataPtr_, - q(i), v(i), a(i)); - } - else { - pinocchio::computeRNEADerivatives(*modelPtr_, *dataPtr_, - q(i), v(i), a(i), - prnea_pq, prnea_pv, prnea_pa); - } - - // adjust with damping force and rotor inertia force - tau(i) = dataPtr_->tau + - modelPtr_->damping.cwiseProduct(v(i)) + - modelPtr_->rotorInertia.cwiseProduct(a(i)); - - if (compute_derivatives) { - // prnea_pa is just the inertia matrix. - // pinocchio only computes the upper triangle part of it. - for (int mi = 0; mi < prnea_pa.rows(); mi++) { - for (int mj = 0; mj <= mi; mj++) { - if (mi == mj) { - // pinocchio rnea does not take rotor inertia into account - prnea_pa(mi, mj) += modelPtr_->rotorInertia(mi); - } - else { - prnea_pa(mi, mj) = prnea_pa(mj, mi); - } - } - } - - // pinocchio rnea does not take damping into account - for (int mi = 0; mi < prnea_pa.rows(); mi++) { - prnea_pv(mi, mi) += modelPtr_->damping(mi); - } - } - - tau_dep = dcPtr_->get_dependent_vector(tau(i)); - tau_indep = dcPtr_->get_independent_vector(tau(i)); - - // assume setupJointPositionVelocityAcceleration() has been called - // compute the reaction force to maintain constraints - lambda(i) = dcPtr_->J_dep_T_qr.solve(tau_dep); - - if (compute_derivatives) { - dcPtr_->get_dependent_rows(prnea_pq_dep, prnea_pq); - dcPtr_->get_independent_rows(prnea_pq_indep, prnea_pq); - dcPtr_->get_dependent_rows(prnea_pv_dep, prnea_pv); - dcPtr_->get_independent_rows(prnea_pv_indep, prnea_pv); - dcPtr_->get_dependent_rows(prnea_pa_dep, prnea_pa); - dcPtr_->get_independent_rows(prnea_pa_indep, prnea_pa); - - dcPtr_->get_JTx_partial_dq(q(i), lambda(i)); - JTlambda_partial_dq = dcPtr_->JTx_partial_dq; - dcPtr_->get_dependent_rows(JTlambda_partial_dq_dep, JTlambda_partial_dq); - dcPtr_->get_independent_rows(JTlambda_partial_dq_indep, JTlambda_partial_dq); - - plambda_pz(i) = dcPtr_->J_dep_T_qr.solve((prnea_pq_dep - JTlambda_partial_dq_dep) * pq_pz(i) + - prnea_pv_dep * pv_pz(i) + - prnea_pa_dep * pa_pz(i)); - } - - // assume setupJointPositionVelocityAcceleration() has been called - tau(i) = tau_indep - dcPtr_->J_indep.transpose() * lambda(i); - - if (compute_derivatives) { - ptau_pz(i) = (prnea_pq_indep - JTlambda_partial_dq_indep) * pq_pz(i) + - prnea_pv_indep * pv_pz(i) + - prnea_pa_indep * pa_pz(i) - - dcPtr_->J_indep.transpose() * plambda_pz(i); - } - } -} - -}; // namespace RAPTOR - diff --git a/KinematicsDynamics/src/CustomizedInverseDynamics.cpp b/KinematicsDynamics/src/CustomizedInverseDynamics.cpp deleted file mode 100644 index 2afc86f0..00000000 --- a/KinematicsDynamics/src/CustomizedInverseDynamics.cpp +++ /dev/null @@ -1,263 +0,0 @@ -#include "CustomizedInverseDynamics.h" - -namespace RAPTOR { - -CustomizedInverseDynamics::CustomizedInverseDynamics(const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::shared_ptr& trajPtr_input) { - jtype = jtype_input; - trajPtr_ = trajPtr_input; - N = trajPtr_->N; - - modelPtr_ = std::make_shared(model_input); - dataPtr_ = std::make_shared(model_input); - - Xtree.resize(1, modelPtr_->nv); - I.resize(1, modelPtr_->nv); - - for (int i = 0; i < modelPtr_->nv; i++) { - const int pinocchio_joint_id = i + 1; // the first joint in pinocchio is the root joint - - // plux in Roy Featherstone's code (transformation matrix from parent to child) - Xtree(i) = Utils::plux(modelPtr_->jointPlacements[pinocchio_joint_id].rotation().transpose(), - modelPtr_->jointPlacements[pinocchio_joint_id].translation()); - - // function mcI in Roy Featherstone's code (parallel axis theorem) - const MatX CC = Utils::skew(modelPtr_->inertias[pinocchio_joint_id].lever()); - const double mm = modelPtr_->inertias[pinocchio_joint_id].mass(); - const MatX II = modelPtr_->inertias[pinocchio_joint_id].inertia().matrix(); - I(i) << mm * CC * CC.transpose() + II, mm * CC, - mm * CC.transpose(), mm * MatX::Identity(3, 3); - } - - a_grav << modelPtr_->gravity.angular(), - modelPtr_->gravity.linear(); - - Xup.resize(1, modelPtr_->nv); - dXupdq.resize(1, modelPtr_->nv); - S.resize(1, modelPtr_->nv); - v.resize(1, modelPtr_->nv); - pv_pz.resize(1, modelPtr_->nv); - a.resize(1, modelPtr_->nv); - pa_pz.resize(1, modelPtr_->nv); - f.resize(1, modelPtr_->nv); - pf_pz.resize(1, modelPtr_->nv); - - tau.resize(1, N); - ptau_pz.resize(1, N); - - lambda.resize(1, N); - plambda_pz.resize(1, N); -} - -void CustomizedInverseDynamics::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (trajPtr_ == nullptr) { - throw std::runtime_error("trajPtr_ is not defined yet!"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - if (compute_hessian) { - throw std::invalid_argument("CustomizedInverseDynamics: Hessian not implemented yet"); - } - - for (int i = 0; i < N; i++) { - const VecX& q = trajPtr_->q(i); - const VecX& q_d = trajPtr_->q_d(i); - const VecX& q_dd = trajPtr_->q_dd(i); - - const MatX& pq_pz = trajPtr_->pq_pz(i); - const MatX& pq_d_pz = trajPtr_->pq_d_pz(i); - const MatX& pq_dd_pz = trajPtr_->pq_dd_pz(i); - - tau(i) = VecX::Zero(trajPtr_->Nact); - - if (compute_derivatives) { - ptau_pz(i) = MatX::Zero(trajPtr_->Nact, z.size()); - plambda_pz(i) = MatX::Zero(6, z.size()); - } - - // below is the original Roy Featherstone's inverse dynamics algorithm - // refer to https://royfeatherstone.org/spatial/v2/index.html#ID - - // forward pass - Vec6 vJ; - Mat6 XJ, dXJdq; - for (int j = 0; j < modelPtr_->nv; j++) { - const int pinocchio_joint_id = j + 1; // the first joint in pinocchio is the root joint - const int parent_id = modelPtr_->parents[pinocchio_joint_id] - 1; - - if (compute_derivatives) { - jcalc(XJ, dXJdq, S(j), jtype(j), q(j)); - } - else { - jcalc(XJ, S(j), jtype(j), q(j)); - } - - vJ = S(j) * q_d(j); - Xup(j) = XJ * Xtree(j); - - if (compute_derivatives) { - dXupdq(j) = dXJdq * Xtree(j); - } - - if (parent_id > -1) { - v(j) = Xup(j) * v(parent_id) + vJ; - Mat6 crm_v_j = crm(v(j)); - a(j) = Xup(j) * a(parent_id) + crm_v_j * vJ + S(j) * q_dd(j); - - if (compute_derivatives) { - // compute pv_pz - pv_pz(j) = Xup(j) * pv_pz(parent_id); - - if (j < trajPtr_->Nact) { - // deal with vJ = S(j) * q_d(j); - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pv_pz(j).row(k) += S(j)(k) * pq_d_pz.row(j); - } - } - - // deal with dXupdq(j) * v(parent_id) - Vec6 dXupdq_v_parent_id = dXupdq(j) * v(parent_id); - for (int k = 0; k < 6; k++) { - pv_pz(j).row(k) += dXupdq_v_parent_id(k) * pq_pz.row(j); - } - } - - // compute pa_pz - pa_pz(j) = Xup(j) * pa_pz(parent_id); - - // deal with crm_v_j * vJ; - pa_pz(j).row(0) += vJ(2) * pv_pz(j).row(1) - vJ(1) * pv_pz(j).row(2); - pa_pz(j).row(1) += vJ(0) * pv_pz(j).row(2) - vJ(2) * pv_pz(j).row(0); - pa_pz(j).row(2) += vJ(1) * pv_pz(j).row(0) - vJ(0) * pv_pz(j).row(1); - pa_pz(j).row(3) += vJ(2) * pv_pz(j).row(4) - vJ(1) * pv_pz(j).row(5) - vJ(4) * pv_pz(j).row(2) + vJ(5) * pv_pz(j).row(1); - pa_pz(j).row(4) += vJ(0) * pv_pz(j).row(5) - vJ(2) * pv_pz(j).row(3) + vJ(3) * pv_pz(j).row(2) - vJ(5) * pv_pz(j).row(0); - pa_pz(j).row(5) += vJ(1) * pv_pz(j).row(3) - vJ(0) * pv_pz(j).row(4) - vJ(3) * pv_pz(j).row(1) + vJ(4) * pv_pz(j).row(0); - - if (j < trajPtr_->Nact) { - Vec6 crm_v_j_S_j = crm_v_j * S(j); - for (int k = 0; k < 6; k++) { - pa_pz(j).row(k) += crm_v_j_S_j(k) * pq_d_pz.row(j); - } - - // deal with S(j) * q_dd(j); - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pa_pz(j).row(k) += S(j)(k) * pq_dd_pz.row(j); - } - } - - // deal with dXupdq(j) * a(parent_id) - Vec6 dXupdq_a_parent_id = dXupdq(j) * a(parent_id); - for (int k = 0; k < 6; k++) { - pa_pz(j).row(k) += dXupdq_a_parent_id(k) * pq_pz.row(j); - } - } - } - } - else { - v(j) = vJ; - a(j) = Xup(j) * (-a_grav) + S(j) * q_dd(j); - - if (compute_derivatives) { - // compute pv_pz - pv_pz(j) = MatX::Zero(6, z.size()); - - if (j < trajPtr_->Nact) { - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pv_pz(j).row(k) = S(j)(k) * pq_d_pz.row(j); - } - } - } - - // compute pa_pz - pa_pz(j) = MatX::Zero(6, z.size()); - - if (j < trajPtr_->Nact) { - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pa_pz(j).row(k) = S(j)(k) * pq_dd_pz.row(j); - } - } - - Vec6 dXupdq_a_grav = dXupdq(j) * (-a_grav); - for (int k = 0; k < 6; k++) { - pa_pz(j).row(k) += dXupdq_a_grav(k) * pq_pz.row(j); - } - } - } - } - - Mat6 crf_v_j = crf(v(j)); - Vec6 I_j_v_j = I(j) * v(j); - f(j) = I(j) * a(j) + crf_v_j * I_j_v_j; - - if (compute_derivatives) { - // compute pf_pz - pf_pz(j) = I(j) * pa_pz(j) + crf_v_j * I(j) * pv_pz(j); - - // deal with d(crf_v_j) * I(j) * v(j) - pf_pz(j).row(0) += I_j_v_j(2) * pv_pz(j).row(1) - I_j_v_j(1) * pv_pz(j).row(2) - I_j_v_j(4) * pv_pz(j).row(5) + I_j_v_j(5) * pv_pz(j).row(4); - pf_pz(j).row(1) += I_j_v_j(0) * pv_pz(j).row(2) - I_j_v_j(2) * pv_pz(j).row(0) + I_j_v_j(3) * pv_pz(j).row(5) - I_j_v_j(5) * pv_pz(j).row(3); - pf_pz(j).row(2) += I_j_v_j(1) * pv_pz(j).row(0) - I_j_v_j(0) * pv_pz(j).row(1) - I_j_v_j(3) * pv_pz(j).row(4) + I_j_v_j(4) * pv_pz(j).row(3); - pf_pz(j).row(3) += I_j_v_j(5) * pv_pz(j).row(1) - I_j_v_j(4) * pv_pz(j).row(2); - pf_pz(j).row(4) += I_j_v_j(3) * pv_pz(j).row(2) - I_j_v_j(5) * pv_pz(j).row(0); - pf_pz(j).row(5) += I_j_v_j(4) * pv_pz(j).row(0) - I_j_v_j(3) * pv_pz(j).row(1); - } - } - - // backward pass - for (int j = modelPtr_->nv - 1; j >= 0; j--) { - const int pinocchio_joint_id = j + 1; // the first joint in pinocchio is the root joint - const int parent_id = modelPtr_->parents[pinocchio_joint_id] - 1; - - if (j < trajPtr_->Nact) { - tau(i)(j) = S(j).transpose() * f(j) + - modelPtr_->rotorInertia(j) * q_dd(j) + - modelPtr_->damping(j) * q_d(j) + - modelPtr_->friction(j) * Utils::sign(q_d(j)); - - if (compute_derivatives) { - ptau_pz(i).row(j) = S(j).transpose() * pf_pz(j) + - modelPtr_->rotorInertia(j) * pq_dd_pz.row(j) + - modelPtr_->damping(j) * pq_d_pz.row(j); - } - } - else { - lambda(i) = f(j); // record the contact force at the last joint - - if (compute_derivatives) { - plambda_pz(i) = pf_pz(j); - } - } - - if (parent_id > -1) { - f(parent_id) += Xup(j).transpose() * f(j); - - if (compute_derivatives) { - pf_pz(parent_id) += Xup(j).transpose() * pf_pz(j); - - // deal with Xup(j).transpose() * f(j) - if (j < trajPtr_->Nact) { - Vec6 dXupdq_T_f_j = dXupdq(j).transpose() * f(j); - for (int k = 0; k < 6; k++) { - pf_pz(parent_id).row(k) += dXupdq_T_f_j(k) * pq_pz.row(j); - } - } - } - } - } - } -} - -}; // namespace RAPTOR - diff --git a/KinematicsDynamics/src/DynamicsConstraints.cpp b/KinematicsDynamics/src/DynamicsConstraints.cpp deleted file mode 100644 index 11231419..00000000 --- a/KinematicsDynamics/src/DynamicsConstraints.cpp +++ /dev/null @@ -1,207 +0,0 @@ -#include "DynamicsConstraints.h" - -namespace RAPTOR { - -DynamicsConstraints::DynamicsConstraints(const int numJoints_input, int numDependentJoints_input) : - numJoints(numJoints_input), - numDependentJoints(numDependentJoints_input) { - numIndependentJoints = numJoints - numDependentJoints; - - c = VecX::Zero(numJoints); - J = MatX::Zero(numDependentJoints, numJoints); - Jx_partial_dq = MatX::Zero(numDependentJoints, numJoints); - JTx_partial_dq = MatX::Zero(numJoints, numJoints); - Jxy_partial_dq = MatX::Zero(numDependentJoints, numJoints); - - pq_dep_pq_indep = MatX::Zero(numDependentJoints, numIndependentJoints); - pv_dep_pq = MatX::Zero(numDependentJoints, numJoints); - pv_dep_pv_indep = MatX::Zero(numDependentJoints, numIndependentJoints); - pa_dep_pq = MatX::Zero(numDependentJoints, numJoints); - pa_dep_pv = MatX::Zero(numDependentJoints, numJoints); - pa_dep_pa_indep = MatX::Zero(numDependentJoints, numIndependentJoints); - - J_dep = MatX::Zero(numDependentJoints, numDependentJoints); - J_indep = MatX::Zero(numDependentJoints, numJoints - numDependentJoints); - - P_dep = MatX::Zero(numDependentJoints, numIndependentJoints); - Pa_indep = VecX::Zero(numJoints); - temp1 = MatX::Zero(numDependentJoints, numJoints); - temp2_1 = VecX::Zero(numJoints); - temp2 = MatX::Zero(numDependentJoints, numJoints); - temp3 = MatX::Zero(numDependentJoints, numJoints); - - bufferDataQueue.resize(0); -} - -void DynamicsConstraints::setupJointPositionVelocityAcceleration(VecX& q, VecX& v, VecX& a, bool compute_derivatives) { - if (recoverSavedData(q, v, a, compute_derivatives)) { - return; - } - - setupJointPosition(q, compute_derivatives); - - if (!compute_derivatives) { - get_c(q); // c is updated - get_J(q); // J is updated - - get_dependent_columns(J_dep, J); - get_independent_columns(J_indep, J); - - J_dep_qr = QRSolver(J_dep); - J_dep_T_qr = QRSolver(J_dep.transpose()); - - // sanity check on uniqueness (these two arguments are actually equivalent) - if (J_dep_qr.rank() != J_dep.rows() || - J_dep_qr.rank() != J_dep.cols() || - J_dep_T_qr.rank() != J_dep.rows() || - J_dep_T_qr.rank() != J_dep.cols()) { - throw std::runtime_error("constraint jacobian is not full rank!"); - } - - // get the dependent columns of the projection matrix - P_dep = -J_dep_qr.solve(J_indep); - } - // else { - // // should have been called in setupJointPosition - // } - - // fill in depuated joints velocities - fill_dependent_vector(v, P_dep * get_independent_vector(v)); - - get_Jx_partial_dq(q, v); - MatX Jv_partial_dq = Jx_partial_dq; - - // fill in depuated joints accelerations - fill_dependent_vector(a, -J_dep_qr.solve(J_indep * get_independent_vector(a) + - Jv_partial_dq * v)); - - if (compute_derivatives) { - pq_dep_pq_indep = P_dep; - - pv_dep_pq = -J_dep_qr.solve(Jx_partial_dq); - pv_dep_pv_indep = P_dep; - - fill_dependent_vector(Pa_indep, P_dep * get_independent_vector(a)); - fill_independent_vector(Pa_indep, get_independent_vector(a)); - - get_Jx_partial_dq(q, Pa_indep); // be careful here, Jx_partial_dq has been changed here! - temp1 = Jx_partial_dq; - - fill_dependent_vector(temp2_1, J_dep_qr.solve(Jv_partial_dq * v), true); - get_Jx_partial_dq(q, temp2_1); // be careful here, Jx_partial_dq has been changed here! - temp2 = Jx_partial_dq; - - get_Jxy_partial_dq(q, v, v); - temp3 = Jxy_partial_dq; - - pa_dep_pq = J_dep_qr.solve(-temp1 + temp2 - temp3); - pa_dep_pv = -J_dep_qr.solve(2 * Jv_partial_dq); - pa_dep_pa_indep = P_dep; - } - - updateQueue(q, v, a, compute_derivatives); -} - -bool DynamicsConstraints::recoverSavedData(VecX& q, bool compute_derivatives) { - for (auto& it : bufferDataQueue) { - if (ifIndependentPartEqual(it.q, q) && - (it.compute_derivatives == compute_derivatives || it.compute_derivatives == true)) { - q = it.q; - - if (compute_derivatives) { - J_dep = it.J_dep; - J_indep = it.J_indep; - J_dep_qr = it.J_dep_qr; - J_dep_T_qr = it.J_dep_T_qr; - - pq_dep_pq_indep = it.pq_dep_pq_indep; - } - - return true; - } - } - - return false; -} - -bool DynamicsConstraints::recoverSavedData(VecX& q, VecX& v, VecX& a, bool compute_derivatives) { - for (auto& it : bufferDataQueue) { - if (ifIndependentPartEqual(it.q, q) && - ifIndependentPartEqual(it.v, v) && - ifIndependentPartEqual(it.a, a) && - (it.compute_derivatives == compute_derivatives || it.compute_derivatives == true)) { - q = it.q; - v = it.v; - a = it.a; - - J_dep = it.J_dep; - J_indep = it.J_indep; - J_dep_qr = it.J_dep_qr; - J_dep_T_qr = it.J_dep_T_qr; - - if (compute_derivatives) { - pq_dep_pq_indep = it.pq_dep_pq_indep; - pv_dep_pq = it.pv_dep_pq; - pv_dep_pv_indep = it.pv_dep_pv_indep; - pa_dep_pq = it.pa_dep_pq; - pa_dep_pv = it.pa_dep_pv; - pa_dep_pa_indep = it.pa_dep_pa_indep; - } - - return true; - } - } - - return false; -} - -void DynamicsConstraints::updateQueue(const VecX& q, bool compute_derivatives) { - BufferData newData; - newData.q = q; - newData.compute_derivatives = compute_derivatives; - - if (compute_derivatives) { - newData.J_dep = J_dep; - newData.J_indep = J_indep; - newData.J_dep_qr = J_dep_qr; - newData.J_dep_T_qr = J_dep_T_qr; - - newData.pq_dep_pq_indep = pq_dep_pq_indep; - } - - if (bufferDataQueue.size() > MAX_BUFFER_SIZE) { - bufferDataQueue.pop_front(); - } - - bufferDataQueue.push_back(newData); -} - -void DynamicsConstraints::updateQueue(const VecX& q, const VecX& v, const VecX& a, bool compute_derivatives) { - BufferData newData; - newData.q = q; - newData.v = v; - newData.a = a; - newData.compute_derivatives = compute_derivatives; - - newData.J_dep = J_dep; - newData.J_indep = J_indep; - newData.J_dep_qr = J_dep_qr; - newData.J_dep_T_qr = J_dep_T_qr; - - if (compute_derivatives) { - newData.pq_dep_pq_indep = pq_dep_pq_indep; - newData.pv_dep_pq = pv_dep_pq; - newData.pv_dep_pv_indep = pv_dep_pv_indep; - newData.pa_dep_pq = pa_dep_pq; - newData.pa_dep_pv = pa_dep_pv; - newData.pa_dep_pa_indep = pa_dep_pa_indep; - } - - if (bufferDataQueue.size() > MAX_BUFFER_SIZE) { - bufferDataQueue.pop_front(); - } - - bufferDataQueue.push_back(newData); -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/KinematicsDynamics/src/ForwardKinematics.cpp b/KinematicsDynamics/src/ForwardKinematics.cpp deleted file mode 100644 index 3e373127..00000000 --- a/KinematicsDynamics/src/ForwardKinematics.cpp +++ /dev/null @@ -1,581 +0,0 @@ -#include "ForwardKinematics.h" - -namespace RAPTOR { - -// ForwardKinematicsSolver::ForwardKinematicsSolver() { -// q_copy = VecX::Zero(0); -// } - -ForwardKinematicsSolver::ForwardKinematicsSolver(const Model* model_input, - const Eigen::VectorXi& jtype_input) : - modelPtr_(model_input), - jtype(jtype_input) { - if (modelPtr_->nv != jtype.size()) { - std::cerr << "modelPtr_->nv = " << modelPtr_->nv << std::endl; - std::cerr << "jtype.size() = " << jtype.size() << std::endl; - throw std::invalid_argument("modelPtr_->nv != jtype.size()"); - } -} - -void ForwardKinematicsSolver::compute(const int start, - const int end, - const VecX& q, - const Transform* startT, - const Transform* endT, - const int order) { - if (modelPtr_ == nullptr) { - throw std::runtime_error("modelPtr_ is not initialized!"); - } - - if (jtype.size() != modelPtr_->nv) { - throw std::runtime_error("jtype is not initialized!"); - } - - if (order < 0) { - throw std::invalid_argument("order has to be non-negative!"); - } - if (order > 3) { - throw std::invalid_argument("order has to be less than or equal to 3!"); - } - - // find the kinematics chain - chain.clear(); - int search_id = end; - while (search_id != start) { - // pinocchio joint index starts from 1 - chain.push_back(search_id - 1); - - if (search_id < 0 || search_id > modelPtr_->nv) { - throw std::runtime_error("Can not find the end joint in the modelPtr_!"); - } - - search_id = modelPtr_->parents[search_id]; - } - std::reverse(chain.begin(), chain.end()); - - // allocate memory - T = Transform(); - if (order >= 1) { - dTdq.resize(modelPtr_->nv); - - if (order >= 2) { - ddTddq.resize(modelPtr_->nv); - for (int i = 0; i < modelPtr_->nv; i++) { - ddTddq[i].resize(modelPtr_->nv); - } - - if (order >= 3) { - dddTdddq.resize(modelPtr_->nv); - for (int i = 0; i < modelPtr_->nv; i++) { - dddTdddq[i].resize(modelPtr_->nv); - for (int j = 0; j < modelPtr_->nv; j++) { - dddTdddq[i][j].resize(modelPtr_->nv); - } - } - } - } - } - - // initialize memory with start transformation matrix - if (startT != nullptr) { - T = (*startT); - - if (order >= 1) { - for (auto i : chain) { - dTdq[i] = (*startT); - - if (order >= 2) { - for (auto j : chain) { - ddTddq[i][j] = (*startT); - - if (order >= 3) { - for (auto k : chain) { - dddTdddq[i][j][k] = (*startT); - } - } - } - } - } - } - } - else { - T = Transform(); - - if (order >= 1) { - for (auto i : chain) { - dTdq[i] = Transform(); - - if (order >= 2) { - for (auto j : chain) { - ddTddq[i][j] = Transform(); - - if (order >= 3) { - for (auto k : chain) { - dddTdddq[i][j][k] = Transform(); - } - } - } - } - } - } - } - - // iterative process to compute the forward kinematics - for (auto i : chain) { - // pinocchio joint index starts from 1 - const auto& jointPlacement = modelPtr_->jointPlacements[i + 1]; - - Transform Tj(jtype(i), q(i)); - Transform dTjdq(jtype(i), q(i), 1); - Transform ddTjddq(jtype(i), q(i), 2); - Transform dddTjdddq(jtype(i), q(i), 3); - - T *= (jointPlacement * Tj); - - if (order >= 1) { - for (auto j : chain) { - dTdq[j] *= jointPlacement; - if (j == i) { - dTdq[j] *= dTjdq; - } - else { - dTdq[j] *= Tj; - } - - if (order >= 2) { - for (auto k : chain) { - if (k >= j) { - ddTddq[j][k] *= jointPlacement; - if (j == i && k == i) { - ddTddq[j][k] *= ddTjddq; - } - else if (j == i || k == i) { - ddTddq[j][k] *= dTjdq; - } - else { - ddTddq[j][k] *= Tj; - } - } - else { - ddTddq[j][k] = ddTddq[k][j]; - } - - if (order >= 3) { - for (auto h : chain) { - if (h >= k && k >= j) { - dddTdddq[j][k][h] *= jointPlacement; - if (j == i && k == i && h == i) { - dddTdddq[j][k][h] *= dddTjdddq; - } - else if ((j == i && k == i) || (j == i && h == i) || (k == i && h == i)) { - dddTdddq[j][k][h] *= ddTjddq; - } - else if (j == i || k == i || h == i) { - dddTdddq[j][k][h] *= dTjdq; - } - else { - dddTdddq[j][k][h] *= Tj; - } - } - else { - if (h < k) { - dddTdddq[j][k][h] = dddTdddq[j][h][k]; - } - else if (k < j) { - dddTdddq[j][k][h] = dddTdddq[k][j][h]; - } - else { - dddTdddq[j][k][h] = dddTdddq[h][k][j]; - } - } - } - } - } - } - } - } - } - - // multiply the end transformation matrix - if (endT != nullptr) { - T *= (*endT); - - if (order >= 1) { - for (auto i : chain) { - dTdq[i] *= (*endT); - - if (order >= 2) { - for (auto j : chain) { - ddTddq[i][j] *= (*endT); - - if (order >= 3) { - for (auto k : chain) { - dddTdddq[i][j][k] *= (*endT); - } - } - } - } - } - - } - } -} - -Transform ForwardKinematicsSolver::getTransform() const { - return T; -} - -Eigen::Vector3d ForwardKinematicsSolver::getTranslation() const { - return T.p; -} - -Eigen::Matrix3d ForwardKinematicsSolver::getRotation() const { - return T.R; -} - -Eigen::Vector3d ForwardKinematicsSolver::getRPY() const { - return T.getRPY(); -} - -Eigen::MatrixXd ForwardKinematicsSolver::getTranslationJacobian() const { - if (dTdq.size() != modelPtr_->nv) { - throw std::runtime_error("dTdq is not computed yet!"); - } - - Eigen::MatrixXd J = Eigen::MatrixXd::Zero(3, modelPtr_->nv); - for (auto i : chain) { - J.col(i) = dTdq[i].p; - } - - return J; -} - -void ForwardKinematicsSolver::getRotationJacobian(Eigen::Array& result) const { - if (dTdq.size() != modelPtr_->nv) { - throw std::runtime_error("dTdq is not computed yet!"); - } - - result.resize(modelPtr_->nv); - for (int i = 0; i < modelPtr_->nv; i++) { - result(i).setZero(); - } - - for (auto i : chain) { - result(i) = dTdq[i].R; - } -} - -Eigen::MatrixXd ForwardKinematicsSolver::getRPYJacobian() const { - if (dTdq.size() != modelPtr_->nv) { - throw std::runtime_error("dTdq is not computed yet!"); - } - - const Mat3& R = T.R; // so that the code is cleaner - - const double rollDenom = R(1,2) * R(1,2) + R(2,2) * R(2,2); - const double yawDenom = R(0,0) * R(0,0) + R(0,1) * R(0,1); - - Eigen::MatrixXd J = Eigen::MatrixXd::Zero(3, modelPtr_->nv); - for (auto i : chain) { - const Mat3& dRdq = dTdq[i].R; // so that the code is cleaner - - J(0, i) = (R(1,2) * dRdq(2,2) - - R(2,2) * dRdq(1,2)) - / rollDenom; - - J(1, i) = HigherOrderDerivatives::safedasindx(R(0,2)) * dRdq(0,2); - - J(2, i) = (R(0,1) * dRdq(0,0) - - R(0,0) * dRdq(0,1)) - / yawDenom; - } - - return J; -} - -void ForwardKinematicsSolver::getTranslationHessian(Eigen::Array& result) const { - if (ddTddq.size() != modelPtr_->nv) { - throw std::runtime_error("ddTddq is not computed yet!"); - } - - for (int i = 0; i < 3; i++) { - result(i) = Eigen::MatrixXd::Zero(modelPtr_->nv, modelPtr_->nv); - } - - for (auto i : chain) { - for (auto j : chain) { - result(0)(i, j) = ddTddq[i][j].p(0); - result(1)(i, j) = ddTddq[i][j].p(1); - result(2)(i, j) = ddTddq[i][j].p(2); - } - } -} - -void ForwardKinematicsSolver::getRotationHessian(Eigen::Array& result) const { - if (ddTddq.size() != modelPtr_->nv) { - throw std::runtime_error("ddTddq is not computed yet!"); - } - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - result(i, j) = Eigen::MatrixXd::Zero(modelPtr_->nv, modelPtr_->nv); - } - } - - for (auto i : chain) { - for (auto j : chain) { - for (int k = 0; k < 3; k++) { - for (int l = 0; l < 3; l++) { - result(k, l)(i, j) = ddTddq[i][j].R(k, l); - } - } - } - } -} - -void ForwardKinematicsSolver::getRotationHessian(Eigen::Array& result) const { - if (ddTddq.size() != modelPtr_->nv) { - throw std::runtime_error("ddTddq is not computed yet!"); - } - - result.resize(modelPtr_->nv, modelPtr_->nv); - for (int i = 0; i < modelPtr_->nv; i++) { - for (int j = 0; j < modelPtr_->nv; j++) { - result(i, j).setZero(); - } - } - - for (auto i : chain) { - for (auto j : chain) { - result(i, j) = ddTddq[i][j].R; - } - } -} - -void ForwardKinematicsSolver::getRPYHessian(Eigen::Array& result) const { - if (ddTddq.size() != modelPtr_->nv) { - throw std::runtime_error("ddTddq is not computed yet!"); - } - - for (int i = 0; i < 3; i++) { - result(i) = Eigen::MatrixXd::Zero(modelPtr_->nv, modelPtr_->nv); - } - - const Mat3& R = T.R; // so that the code is cleaner - - const double R12Square = R(1,2) * R(1,2); - const double R22Square = R(2,2) * R(2,2); - const double rollDenom = R12Square + R22Square; - const double rollDenomSquare = rollDenom * rollDenom; - - const double R00Square = R(0,0) * R(0,0); - const double R01Square = R(0,1) * R(0,1); - const double yawDenom = R00Square + R01Square; - const double yawDenomSquare = yawDenom * yawDenom; - - const double dasindx = HigherOrderDerivatives::safedasindx(R(0,2)); - const double ddasinddx = HigherOrderDerivatives::safeddasinddx(R(0,2)); - - for (auto i : chain) { - const Mat3& dRdqi = dTdq[i].R; // so that the code is cleaner - for (auto j : chain) { - const Mat3& dRdqj = dTdq[j].R; // so that the code is cleaner - const Mat3& ddRdqdq = ddTddq[i][j].R; // so that the code is cleaner - - result(0)(i, j) = - dRdqj(1,2) * (dRdqi(2,2) / rollDenom + - (R(1,2) * R(2,2) * dRdqi(1,2) - dRdqi(2,2) * R12Square) - * (2.0 / rollDenomSquare)) - - dRdqj(2,2) * (dRdqi(1,2) / rollDenom + - (R(1,2) * R(2,2) * dRdqi(2,2) - dRdqi(1,2) * R22Square) - * (2.0 / rollDenomSquare)) + - (R(1,2) * ddRdqdq(2,2) - R(2,2) * ddRdqdq(1,2)) / rollDenom; - - result(1)(i, j) = - ddasinddx * dRdqi(0,2) * dRdqj(0,2) + - dasindx * ddRdqdq(0,2); - - result(2)(i, j) = - dRdqj(0,1) * (dRdqi(0,0) / yawDenom + - (R(0,1) * R(0,0) * dRdqi(0,1) - dRdqi(0,0) * R01Square) - * (2.0 / yawDenomSquare)) - - dRdqj(0,0) * (dRdqi(0,1) / yawDenom + - (R(0,1) * R(0,0) * dRdqi(0,0) - dRdqi(0,1) * R00Square) - * (2.0 / yawDenomSquare)) + - (R(0,1) * ddRdqdq(0,0) - R(0,0) * ddRdqdq(0,1)) / yawDenom; - } - } -} - -void ForwardKinematicsSolver::getTranslationThirdOrderTensor(const VecX& x, Eigen::Array& result) const { - if (dddTdddq.size() != modelPtr_->nv) { - throw std::runtime_error("dddTdddq is not computed yet!"); - } - - if (x.size() != modelPtr_->nv) { - throw std::invalid_argument("x has to be of size modelPtr_->nv"); - } - - for (int i = 0; i < 3; i++) { - result(i) = Eigen::MatrixXd::Zero(modelPtr_->nv, modelPtr_->nv); - } - - for (auto i : chain) { - for (auto j : chain) { - for (auto k : chain) { - result(0)(i, j) += dddTdddq[i][j][k].p(0) * x(k); - result(1)(i, j) += dddTdddq[i][j][k].p(1) * x(k); - result(2)(i, j) += dddTdddq[i][j][k].p(2) * x(k); - } - } - } -} - -void ForwardKinematicsSolver::getRPYThirdOrderTensor(const VecX& x, Eigen::Array& result) const { - if (dddTdddq.size() != modelPtr_->nv) { - throw std::runtime_error("dddTdddq is not computed yet!"); - } - - if (x.size() != modelPtr_->nv) { - throw std::invalid_argument("x has to be of size modelPtr_->nv"); - } - - for (int i = 0; i < 3; i++) { - result(i) = Eigen::MatrixXd::Zero(modelPtr_->nv, modelPtr_->nv); - } - - const Mat3& R = T.R; // so that the code is cleaner - - const double R12Square = R(1,2) * R(1,2); - const double R22Square = R(2,2) * R(2,2); - const double rollDenom = R12Square + R22Square; - const double rollDenomSquare = rollDenom * rollDenom; - const double rollDenomThird = rollDenomSquare * rollDenom; - - const double R00Square = R(0,0) * R(0,0); - const double R01Square = R(0,1) * R(0,1); - const double yawDenom = R00Square + R01Square; - const double yawDenomSquare = yawDenom * yawDenom; - const double yawDenomThird = yawDenomSquare * yawDenom; - - const double dasindx = HigherOrderDerivatives::safedasindx(R(0,2)); - const double ddasinddx = HigherOrderDerivatives::safeddasinddx(R(0,2)); - const double dddasindddx = HigherOrderDerivatives::safedddasindddx(R(0,2)); - - for (auto i : chain) { - const Mat3& dRdqi = dTdq[i].R; // so that the code is cleaner - - const double temp1 = 2.0 * R(1,2) * dRdqi(1,2) / rollDenomSquare; - const double temp2 = 2.0 * R(2,2) * dRdqi(2,2) / rollDenomSquare; - const double temp3 = 8.0 * R(1,2) * dRdqi(1,2) * R22Square / rollDenomThird; - const double temp4 = 8.0 * R(2,2) * dRdqi(2,2) * R12Square / rollDenomThird; - const double temp5 = temp1 - temp2 + temp4 - temp3; - - const double temp6 = 2.0 * R(0,1) * dRdqi(0,1) / yawDenomSquare; - const double temp7 = 2.0 * R(0,0) * dRdqi(0,0) / yawDenomSquare; - const double temp8 = 8.0 * R(0,1) * dRdqi(0,1) * R00Square / yawDenomThird; - const double temp9 = 8.0 * R(0,0) * dRdqi(0,0) * R01Square / yawDenomThird; - const double temp10 = temp6 - temp7 + temp9 - temp8; - - for (auto j : chain) { - const Mat3& dRdqj = dTdq[j].R; // so that the code is cleaner - const Mat3& ddRdqidqj = ddTddq[i][j].R; // so that the code is cleaner - for (auto k : chain) { - const Mat3& dRdqk = dTdq[k].R; // so that the code is cleaner - const Mat3& ddRdqidqk = ddTddq[i][k].R; // so that the code is cleaner - const Mat3& ddRdqjdqk = ddTddq[j][k].R; // so that the code is cleaner - const Mat3& dddRdqdqdq = dddTdddq[i][j][k].R; // so that the code is cleaner - - const double T0_i_j_k = - -ddRdqjdqk(2,2) * (R(1,2) * temp2 + dRdqi(1,2) / rollDenom - dRdqi(1,2) * R22Square / rollDenomSquare * 2.0) - + ddRdqjdqk(1,2) * (R(2,2) * temp1 + dRdqi(2,2) / rollDenom - dRdqi(2,2) * R12Square / rollDenomSquare * 2.0) - + dRdqk(1,2) * ( - -dRdqj(1,2) * ( - R(1,2) * dRdqi(2,2) / rollDenomSquare * 6.0 - - R(2,2) * dRdqi(1,2) / rollDenomSquare * 2.0 - - (R(1,2) * R(1,2) * R(1,2)) * dRdqi(2,2) / rollDenomThird * 8.0 - + R(2,2) * dRdqi(1,2) * R12Square / rollDenomThird * 8.0 - ) - + dRdqj(2,2) * temp5 - + ddRdqidqj(2,2) / rollDenom - - ddRdqidqj(2,2) * R12Square / rollDenomSquare * 2.0 - + R(1,2) * R(2,2) * ddRdqidqj(1,2) / rollDenomSquare * 2.0 - ) - - dRdqk(2,2) * ( - dRdqj(2,2) * ( - R(1,2) * dRdqi(2,2) / rollDenomSquare * 2.0 - - R(2,2) * dRdqi(1,2) / rollDenomSquare * 6.0 - + (R(2,2) * R(2,2) * R(2,2)) * dRdqi(1,2) / rollDenomThird * 8.0 - - R(1,2) * dRdqi(2,2) * R22Square / rollDenomThird * 8.0 - ) - - dRdqj(1,2) * temp5 - + ddRdqidqj(1,2) / rollDenom - - ddRdqidqj(1,2) * R22Square / rollDenomSquare * 2.0 - + R(1,2) * R(2,2) * ddRdqidqj(2,2) / rollDenomSquare * 2.0 - ) - - ddRdqidqk(1,2) * ( - dRdqj(2,2) * (1.0 / rollDenom - R22Square / rollDenomSquare * 2.0) - - R(1,2) * R(2,2) * dRdqj(1,2) / rollDenomSquare * 2.0 - ) - + ddRdqidqk(2,2) * ( - dRdqj(1,2) * (1.0 / rollDenom - R12Square / rollDenomSquare * 2.0) - - R(1,2) * R(2,2) * dRdqj(2,2) / rollDenomSquare * 2.0 - ) - + R(1,2) * dddRdqdqdq(2,2) / rollDenom - - R(2,2) * dddRdqdqdq(1,2) / rollDenom; - - - const double T1_i_j_k = - dddasindddx * dRdqi(0,2) * dRdqj(0,2) * dRdqk(0,2) + - ddasinddx * ddRdqidqk(0,2) * dRdqj(0,2) + - ddasinddx * ddRdqjdqk(0,2) * dRdqi(0,2) + - ddasinddx * ddRdqidqj(0,2) * dRdqk(0,2) + - dasindx * dddRdqdqdq(0,2); - - const double T2_i_j_k = - -ddRdqjdqk(0,0) * (R(0,1) * temp7 + dRdqi(0,1) / yawDenom - dRdqi(0,1) * R00Square / yawDenomSquare * 2.0) - + ddRdqjdqk(0,1) * (R(0,0) * temp6 + dRdqi(0,0) / yawDenom - dRdqi(0,0) * R01Square / yawDenomSquare * 2.0) - + dRdqk(0,1) * ( - -dRdqj(0,1) * ( - R(0,1) * dRdqi(0,0) / yawDenomSquare * 6.0 - - R(0,0) * dRdqi(0,1) / yawDenomSquare * 2.0 - - (R(0,1) * R(0,1) * R(0,1)) * dRdqi(0,0) / yawDenomThird * 8.0 - + R(0,0) * dRdqi(0,1) * R01Square / yawDenomThird * 8.0 - ) - + dRdqj(0,0) * temp10 - + ddRdqidqj(0,0) / yawDenom - - ddRdqidqj(0,0) * R01Square / yawDenomSquare * 2.0 - + R(0,1) * R(0,0) * ddRdqidqj(0,1) / yawDenomSquare * 2.0 - ) - - dRdqk(0,0) * ( - dRdqj(0,0) * ( - R(0,1) * dRdqi(0,0) / yawDenomSquare * 2.0 - - R(0,0) * dRdqi(0,1) / yawDenomSquare * 6.0 - + (R(0,0) * R(0,0) * R(0,0)) * dRdqi(0,1) / yawDenomThird * 8.0 - - R(0,1) * dRdqi(0,0) * R00Square / yawDenomThird * 8.0 - ) - - dRdqj(0,1) * temp10 - + ddRdqidqj(0,1) / yawDenom - - ddRdqidqj(0,1) * R00Square / yawDenomSquare * 2.0 - + R(0,1) * R(0,0) * ddRdqidqj(0,0) / yawDenomSquare * 2.0 - ) - - ddRdqidqk(0,1) * ( - dRdqj(0,0) * (1.0 / yawDenom - R00Square / yawDenomSquare * 2.0) - - R(0,1) * R(0,0) * dRdqj(0,1) / yawDenomSquare * 2.0 - ) - + ddRdqidqk(0,0) * ( - dRdqj(0,1) * (1.0 / yawDenom - R01Square / yawDenomSquare * 2.0) - - R(0,1) * R(0,0) * dRdqj(0,0) / yawDenomSquare * 2.0 - ) - + R(0,1) * dddRdqdqdq(0,0) / yawDenom - - R(0,0) * dddRdqdqdq(0,1) / yawDenom; - - result(0)(i, j) += T0_i_j_k * x(k); - result(1)(i, j) += T1_i_j_k * x(k); - result(2)(i, j) += T2_i_j_k * x(k); - } - } - } -} - -} // namespace RAPTOR \ No newline at end of file diff --git a/KinematicsDynamics/src/InverseDynamics.cpp b/KinematicsDynamics/src/InverseDynamics.cpp deleted file mode 100644 index 60996974..00000000 --- a/KinematicsDynamics/src/InverseDynamics.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "InverseDynamics.h" - -namespace RAPTOR { - -InverseDynamics::InverseDynamics(const Model& model_input, - const std::shared_ptr& trajPtr_input) : - trajPtr_(trajPtr_input) { - N = trajPtr_->N; - - modelPtr_ = std::make_shared(model_input); - dataPtr_ = std::make_shared(model_input); - - tau.resize(1, N); - - ptau_pz.resize(1, N); - - prnea_pq = MatX::Zero(modelPtr_->nv, modelPtr_->nv); - prnea_pv = MatX::Zero(modelPtr_->nv, modelPtr_->nv); - prnea_pa = MatX::Zero(modelPtr_->nv, modelPtr_->nv); -} - -bool InverseDynamics::is_computed(const VecX& z, bool compute_derivatives, bool compute_hessian) { - if (compute_hessian && !compute_derivatives) { - throw std::invalid_argument("compute_derivatives needs to be true when compute_hessian is true."); - return false; - } - - if (!Utils::ifTwoVectorEqual(current_z, z, 0)) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - if (compute_derivatives != if_compute_derivatives) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - if (compute_hessian != if_compute_hessian) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - // current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return true; -} - -void InverseDynamics::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (trajPtr_ == nullptr) { - throw std::runtime_error("trajPtr_ is not defined yet!"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - if (compute_hessian) { - throw std::invalid_argument("InverseDynamics: Hessian not implemented yet"); - } - - for (int i = 0; i < N; i++) { - const VecX& q = trajPtr_->q(i); - const VecX& v = trajPtr_->q_d(i); - const VecX& a = trajPtr_->q_dd(i); - - if (!compute_derivatives) { - pinocchio::rnea(*modelPtr_, *dataPtr_, q, v, a); - } - else { - pinocchio::computeRNEADerivatives(*modelPtr_, *dataPtr_, q, v, a, - prnea_pq, prnea_pv, prnea_pa); - } - - // adjust with damping force and rotor inertia force - tau(i) = dataPtr_->tau + - modelPtr_->damping.cwiseProduct(v) + - modelPtr_->rotorInertia.cwiseProduct(a); - // TODO: add friction here!!! - - if (compute_derivatives) { - // prnea_pa is just the inertia matrix. - // pinocchio only computes the upper triangle part of it. - for (int mi = 0; mi < prnea_pa.rows(); mi++) { - for (int mj = 0; mj <= mi; mj++) { - if (mi == mj) { - prnea_pa(mi, mj) += modelPtr_->rotorInertia(mi); - } - else { - prnea_pa(mi, mj) = prnea_pa(mj, mi); - } - } - } - - // pinocchio rnea does not take damping into account - for (int mi = 0; mi < prnea_pa.rows(); mi++) { - prnea_pv(mi, mi) += modelPtr_->damping(mi); - } - - ptau_pz(i) = prnea_pq * trajPtr_->pq_pz(i) + - prnea_pv * trajPtr_->pq_d_pz(i) + - prnea_pa * trajPtr_->pq_dd_pz(i); - } - } -} - -}; // namespace RAPTOR - diff --git a/KinematicsDynamics/src/RegressorInverseDynamics.cpp b/KinematicsDynamics/src/RegressorInverseDynamics.cpp deleted file mode 100644 index ecc0970c..00000000 --- a/KinematicsDynamics/src/RegressorInverseDynamics.cpp +++ /dev/null @@ -1,349 +0,0 @@ -#include "RegressorInverseDynamics.h" - -namespace RAPTOR { - -RegressorInverseDynamics::RegressorInverseDynamics(const Model& model_input, - const Eigen::VectorXi& jtype_input, - const std::shared_ptr& trajPtr_input) { - jtype = jtype_input; - trajPtr_ = trajPtr_input; - N = trajPtr_->N; - - modelPtr_ = std::make_shared(model_input); - dataPtr_ = std::make_shared(model_input); - - Xtree.resize(1, modelPtr_->nv); - I.resize(1, modelPtr_->nv); - - numInertialParams = 10 * modelPtr_->nv; - numParams = 10 * modelPtr_->nv + // inertial parameters - 3 * modelPtr_->nv; // motor dynamics parameters - // numParams = 10 * modelPtr_->nv; // inertial parameters - phi = VecX::Zero(numParams); - - for (int i = 0; i < modelPtr_->nv; i++) { - const int pinocchio_joint_id = i + 1; // the first joint in pinocchio is the root joint - - // plux in Roy Featherstone's code (transformation matrix from parent to child) - Xtree(i) = Utils::plux(modelPtr_->jointPlacements[pinocchio_joint_id].rotation().transpose(), - modelPtr_->jointPlacements[pinocchio_joint_id].translation()); - - // function mcI in Roy Featherstone's code (parallel axis theorem) - const Mat3 CC = Utils::skew(modelPtr_->inertias[pinocchio_joint_id].lever()); - const double mm = modelPtr_->inertias[pinocchio_joint_id].mass(); - const Mat3 II = modelPtr_->inertias[pinocchio_joint_id].inertia().matrix(); - - // apply parallel axis theorem - Mat3 mC = mm * CC; - I(i) << mm * CC * CC.transpose() + II, mC, - mC.transpose(), mm * MatX::Identity(3, 3); - - phi.segment<10>(10 * i) << I(i)(0, 0), // inertia (in parent joint frame) - I(i)(0, 1), - I(i)(0, 2), - I(i)(1, 1), - I(i)(1, 2), - I(i)(2, 2), - Utils::skew(mC), // center of mass (in parent joint frame) - mm; // mass - - phi.segment<3>(numInertialParams + 3 * i) << modelPtr_->friction(i), - modelPtr_->damping(i), - modelPtr_->rotorInertia(i); - } - - a_grav << modelPtr_->gravity.angular(), - modelPtr_->gravity.linear(); - - Xup.resize(1, modelPtr_->nv); - dXupdq.resize(1, modelPtr_->nv); - S.resize(1, modelPtr_->nv); - v.resize(1, modelPtr_->nv); - pv_pz.resize(1, modelPtr_->nv); - a.resize(1, modelPtr_->nv); - pa_pz.resize(1, modelPtr_->nv); - - tau.resize(1, N); - ptau_pz.resize(1, N); - - for (int i = 0; i < N; i++) { - tau(i) = VecX::Zero(modelPtr_->nv); - ptau_pz(i) = MatX::Zero(modelPtr_->nv, trajPtr_->varLength); - } - - Y = MatX::Zero(trajPtr_->N * modelPtr_->nv, numParams); - pY_pz.resize(trajPtr_->varLength); - for (int i = 0; i < trajPtr_->varLength; i++) { - pY_pz(i) = MatX::Zero(trajPtr_->N * modelPtr_->nv, numParams); - } - - // The following has nothing to do with motor dynamics parameters - Yfull = MatX::Zero(6 * modelPtr_->nv, numInertialParams); - pYfull_pz.resize(trajPtr_->varLength); - for (int i = 0; i < trajPtr_->varLength; i++) { - pYfull_pz(i) = MatX::Zero(6 * modelPtr_->nv, numInertialParams); - } - - Ycurrent = MatX::Zero(modelPtr_->nv, numInertialParams); -} - -void RegressorInverseDynamics::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (trajPtr_ == nullptr) { - throw std::runtime_error("trajPtr_ is not defined yet!"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) { - return; - } - - trajPtr_->compute(z, compute_derivatives, compute_hessian); - - if (compute_hessian) { - throw std::invalid_argument("CustomizedInverseDynamics: Hessian not implemented yet"); - } - - for (int i = 0; i < N; i++) { - const VecX& q = trajPtr_->q(i); - const VecX& q_d = trajPtr_->q_d(i); - const VecX& q_dd = trajPtr_->q_dd(i); - - const MatX& pq_pz = trajPtr_->pq_pz(i); - const MatX& pq_d_pz = trajPtr_->pq_d_pz(i); - const MatX& pq_dd_pz = trajPtr_->pq_dd_pz(i); - - if (compute_derivatives) { - ptau_pz(i) = MatX::Zero(trajPtr_->Nact, trajPtr_->varLength); - } - - // below is the original Roy Featherstone's inverse dynamics algorithm - // refer to https://royfeatherstone.org/spatial/v2/index.html#ID - - // forward pass - Vec6 vJ; - Mat6 XJ, dXJdq; - Yfull.setZero(); - Ycurrent.setZero(); - - if (compute_derivatives) { - for (int k = 0; k < trajPtr_->varLength; k++) { - pYfull_pz(k).setZero(); - } - } - - for (int j = 0; j < modelPtr_->nv; j++) { - const int pinocchio_joint_id = j + 1; // the first joint in pinocchio is the root joint - const int parent_id = modelPtr_->parents[pinocchio_joint_id] - 1; - - if (compute_derivatives) { - jcalc(XJ, dXJdq, S(j), jtype(j), q(j)); - } - else { - jcalc(XJ, S(j), jtype(j), q(j)); - } - - vJ = S(j) * q_d(j); - Xup(j) = XJ * Xtree(j); - - if (compute_derivatives) { - dXupdq(j) = dXJdq * Xtree(j); - } - - if (parent_id > -1) { - v(j) = Xup(j) * v(parent_id) + vJ; - Mat6 crm_v_j = crm(v(j)); - a(j) = Xup(j) * a(parent_id) + crm_v_j * vJ + S(j) * q_dd(j); - - if (compute_derivatives) { - // compute pv_pz - pv_pz(j) = Xup(j) * pv_pz(parent_id); - - if (j < trajPtr_->Nact) { - // deal with vJ = S(j) * q_d(j); - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pv_pz(j).row(k) += S(j)(k) * pq_d_pz.row(j); - } - } - - // deal with dXupdq(j) * v(parent_id) - Vec6 dXupdq_v_parent_id = dXupdq(j) * v(parent_id); - for (int k = 0; k < 6; k++) { - pv_pz(j).row(k) += dXupdq_v_parent_id(k) * pq_pz.row(j); - } - } - - // compute pa_pz - pa_pz(j) = Xup(j) * pa_pz(parent_id); - - // deal with crm_v_j * vJ; - pa_pz(j).row(0) += vJ(2) * pv_pz(j).row(1) - vJ(1) * pv_pz(j).row(2); - pa_pz(j).row(1) += vJ(0) * pv_pz(j).row(2) - vJ(2) * pv_pz(j).row(0); - pa_pz(j).row(2) += vJ(1) * pv_pz(j).row(0) - vJ(0) * pv_pz(j).row(1); - pa_pz(j).row(3) += vJ(2) * pv_pz(j).row(4) - vJ(1) * pv_pz(j).row(5) - vJ(4) * pv_pz(j).row(2) + vJ(5) * pv_pz(j).row(1); - pa_pz(j).row(4) += vJ(0) * pv_pz(j).row(5) - vJ(2) * pv_pz(j).row(3) + vJ(3) * pv_pz(j).row(2) - vJ(5) * pv_pz(j).row(0); - pa_pz(j).row(5) += vJ(1) * pv_pz(j).row(3) - vJ(0) * pv_pz(j).row(4) - vJ(3) * pv_pz(j).row(1) + vJ(4) * pv_pz(j).row(0); - - if (j < trajPtr_->Nact) { - Vec6 crm_v_j_S_j = crm_v_j * S(j); - for (int k = 0; k < 6; k++) { - pa_pz(j).row(k) += crm_v_j_S_j(k) * pq_d_pz.row(j); - } - - // deal with S(j) * q_dd(j); - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pa_pz(j).row(k) += S(j)(k) * pq_dd_pz.row(j); - } - } - - // deal with dXupdq(j) * a(parent_id) - Vec6 dXupdq_a_parent_id = dXupdq(j) * a(parent_id); - for (int k = 0; k < 6; k++) { - pa_pz(j).row(k) += dXupdq_a_parent_id(k) * pq_pz.row(j); - } - } - } - } - else { - v(j) = vJ; - a(j) = Xup(j) * (-a_grav) + S(j) * q_dd(j); - - if (compute_derivatives) { - // compute pv_pz - pv_pz(j) = MatX::Zero(6, trajPtr_->varLength); - - if (j < trajPtr_->Nact) { - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pv_pz(j).row(k) = S(j)(k) * pq_d_pz.row(j); - } - } - } - - // compute pa_pz - pa_pz(j) = MatX::Zero(6, trajPtr_->varLength); - - if (j < trajPtr_->Nact) { - for (int k = 0; k < S(j).size(); k++) { - if (S(j)(k) != 0) { - pa_pz(j).row(k) = S(j)(k) * pq_dd_pz.row(j); - } - } - - Vec6 dXupdq_a_grav = dXupdq(j) * (-a_grav); - for (int k = 0; k < 6; k++) { - pa_pz(j).row(k) += dXupdq_a_grav(k) * pq_pz.row(j); - } - } - } - } - - const double& v1 = v(j)(0); - const double& v2 = v(j)(1); - const double& v3 = v(j)(2); - const double& v4 = v(j)(3); - const double& v5 = v(j)(4); - const double& v6 = v(j)(5); - - const double& a1 = a(j)(0); - const double& a2 = a(j)(1); - const double& a3 = a(j)(2); - const double& a4 = a(j)(3); - const double& a5 = a(j)(4); - const double& a6 = a(j)(5); - - Yfull.block(6 * j, 10 * j, 6, 10) << - a1, a2 - v1*v3, a3 + v1*v2, -v2*v3, v2*v2 - v3*v3, v2*v3, 0, a6 + v1*v5 - v2*v4, v1*v6 - a5 - v3*v4, 0, - v1*v3, a1 + v2*v3, v3*v3 - v1*v1, a2, a3 - v1*v2, -v1*v3, v2*v4 - v1*v5 - a6, 0, a4 + v2*v6 - v3*v5, 0, - -v1*v2, v1*v1 - v2*v2, a1 - v2*v3, v1*v2, a2 + v1*v3, a3, a5 - v1*v6 + v3*v4, v3*v5 - v2*v6 - a4, 0, 0, - 0, 0, 0, 0, 0, 0, -v2*v2 - v3*v3, v1*v2 - a3, a2 + v1*v3, a4 + v2*v6 - v3*v5, - 0, 0, 0, 0, 0, 0, a3 + v1*v2, -v1*v1 - v3*v3, v2*v3 - a1, a5 - v1*v6 + v3*v4, - 0, 0, 0, 0, 0, 0, v1*v3 - a2, a1 + v2*v3, -v1*v1 - v2*v2, a6 + v1*v5 - v2*v4; - - if (compute_derivatives) { - for (int k = 0; k < trajPtr_->varLength; k++) { - const double& pv1 = pv_pz(j)(0, k); - const double& pv2 = pv_pz(j)(1, k); - const double& pv3 = pv_pz(j)(2, k); - const double& pv4 = pv_pz(j)(3, k); - const double& pv5 = pv_pz(j)(4, k); - const double& pv6 = pv_pz(j)(5, k); - - const double& pa1 = pa_pz(j)(0, k); - const double& pa2 = pa_pz(j)(1, k); - const double& pa3 = pa_pz(j)(2, k); - const double& pa4 = pa_pz(j)(3, k); - const double& pa5 = pa_pz(j)(4, k); - const double& pa6 = pa_pz(j)(5, k); - - pYfull_pz(k).block(6 * j, 10 * j, 6, 10) << - pa1, pa2 - v1*pv3 - pv1*v3, pa3 + v1*pv2 + pv1*v2, -pv2*v3 - v2*pv3, 2*v2*pv2 - 2*v3*pv3, pv2*v3 + v2*pv3, 0, pa6 + v1*pv5 + pv1*v5 - v2*pv4 - pv2*v4, v1*pv6 + pv1*v6 - pa5 - v3*pv4 - pv3*v4, 0, - v1*pv3 + pv1*v3, pa1 + v2*pv3 + pv2*v3, 2*v3*pv3 - 2*v1*pv1, pa2, pa3 - v1*pv2 - pv1*v2, -v1*pv3 - pv1*v3, v2*pv4 + pv2*v4 - v1*pv5 - pv1*v5 - pa6, 0, pa4 + v2*pv6 + pv2*v6 - v3*pv5 - pv3*v5, 0, - -v1*pv2 - pv1*v2, 2*v1*pv1 - 2*v2*pv2, pa1 - v2*pv3 - pv2*v3, v1*pv2 + pv1*v2, pa2 + v1*pv3 + pv1*v3, pa3, v3*pv4 + pv3*v4 - v1*pv6 - pv1*v6 + pa5, v3*pv5 + pv3*v5 - v2*pv6 - pv2*v6 - pa4, 0, 0, - 0, 0, 0, 0, 0, 0, -2*v2*pv2 - 2*v3*pv3, v1*pv2 + pv1*v2 - pa3, pa2 + v1*pv3 + pv1*v3, pa4 + v2*pv6 + pv2*v6 - v3*pv5 - pv3*v5, - 0, 0, 0, 0, 0, 0, pa3 + v1*pv2 + pv1*v2, -2*v1*pv1 - 2*v3*pv3, v2*pv3 + pv2*v3 - pa1, v3*pv4 + pv3*v4 - v1*pv6 - pv1*v6 + pa5, - 0, 0, 0, 0, 0, 0, v1*pv3 + pv1*v3 - pa2, pa1 + v2*pv3 + pv2*v3, -2*v1*pv1 - 2*v2*pv2, pa6 + v1*pv5 + pv1*v5 - v2*pv4 - pv2*v4; - } - } - } - - // backward pass - for (int j = modelPtr_->nv - 1; j >= 0; j--) { - const int pinocchio_joint_id = j + 1; // the first joint in pinocchio is the root joint - const int parent_id = modelPtr_->parents[pinocchio_joint_id] - 1; - - if (parent_id > -1) { - if (compute_derivatives) { - MatX dXupdq_Yfull = dXupdq(j).transpose() * Yfull.middleRows(6 * j, 6); - for (int k = 0; k < trajPtr_->varLength; k++) { - MatX temp1 = Xup(j).transpose() * pYfull_pz(k).middleRows(6 * j, 6); - pYfull_pz(k).middleRows(6 * parent_id, 6) += - temp1 + dXupdq_Yfull * pq_pz(j, k); - } - } - - MatX temp2 = Xup(j).transpose() * Yfull.middleRows(6 * j, 6); - Yfull.middleRows(6 * parent_id, 6) += temp2; - } - - Ycurrent.row(j) = S(j).transpose() * Yfull.middleRows(6 * j, 6); - - if (compute_derivatives) { - for (int k = 0; k < trajPtr_->varLength; k++) { - pY_pz(k).block(i * modelPtr_->nv + j, 0, 1, numInertialParams) = S(j).transpose() * pYfull_pz(k).middleRows(6 * j, 6); - } - } - } - - Y.block(i * modelPtr_->nv, 0, modelPtr_->nv, numInertialParams) = Ycurrent; - - // motor dynamics - for (int j = 0; j < modelPtr_->nv; j++) { - Y(i * modelPtr_->nv + j, numInertialParams + 3 * j ) = Utils::sign(q_d(j)); - Y(i * modelPtr_->nv + j, numInertialParams + 3 * j + 1) = q_d(j); - Y(i * modelPtr_->nv + j, numInertialParams + 3 * j + 2) = q_dd(j); - - if (compute_derivatives) { - for (int k = 0; k < trajPtr_->varLength; k++) { - pY_pz(k)(i * modelPtr_->nv + j, numInertialParams + 3 * j ) = 0; - pY_pz(k)(i * modelPtr_->nv + j, numInertialParams + 3 * j + 1) = pq_d_pz(j, k); - pY_pz(k)(i * modelPtr_->nv + j, numInertialParams + 3 * j + 2) = pq_dd_pz(j, k); - - } - } - } - - // compute torque - tau(i) = Y.middleRows(i * modelPtr_->nv, modelPtr_->nv) * phi; - - if (compute_derivatives) { - for (int k = 0; k < trajPtr_->varLength; k++) { - ptau_pz(i).col(k) = pY_pz(k).middleRows(i * modelPtr_->nv, modelPtr_->nv) * phi; - } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/KinematicsDynamics/src/Spatial.cpp b/KinematicsDynamics/src/Spatial.cpp deleted file mode 100644 index f7d74d07..00000000 --- a/KinematicsDynamics/src/Spatial.cpp +++ /dev/null @@ -1,436 +0,0 @@ -#ifndef SPATIAL_CPP -#define SPATIAL_CPP - -#include "Spatial.h" - -namespace RAPTOR { - -// [Xj,S] = jcalc( jtyp, q ) -void jcalc(Matrix6d& Xj, - Vector6d& S, - const int jtyp, - const double q) { - Xj.setIdentity(); - S.setZero(); - - double c = cos(q); - double s = sin(q); - - if (jtyp < 0) { // reversed direction - // c = cos(-q) = cos(q) - s = -s; // s = sin(-q) = -sin(q) - } - - switch (jtyp) - { - case 0: // fixed joint - // do nothing, results already set - break; - case 1: // revolute X axis 'Rx' - Xj(1,1) = c; - Xj(1,2) = s; - Xj(2,1) = -s; - Xj(2,2) = c; - S(0) = 1; - break; - case 2: // revolute Y axis 'Ry' - Xj(0,0) = c; - Xj(0,2) = -s; - Xj(2,0) = s; - Xj(2,2) = c; - S(1) = 1; - break; - case 3: // revolute Z axis 'Rz' - Xj(0,0) = c; - Xj(0,1) = s; - Xj(1,0) = -s; - Xj(1,1) = c; - S(2) = 1; - break; - case -1: // reversed revolute X axis '-Rx' - Xj(1,1) = c; - Xj(1,2) = s; - Xj(2,1) = -s; - Xj(2,2) = c; - S(0) = -1; - break; - case -2: // reversed revolute Y axis '-Ry' - Xj(0,0) = c; - Xj(0,2) = -s; - Xj(2,0) = s; - Xj(2,2) = c; - S(1) = -1; - break; - case -3: // reversed revolute Z axis '-Rz' - Xj(0,0) = c; - Xj(0,1) = s; - Xj(1,0) = -s; - Xj(1,1) = c; - S(2) = -1; - break; - case 4: // prismatic X axis 'Px' - Xj(4,2) = q; - Xj(5,1) = -q; - S(3) = 1; - break; - case 5: // prismatic Y axis 'Py' - Xj(3,2) = -q; - Xj(5,0) = q; - S(4) = 1; - break; - case 6: // prismatic Z axis 'Pz' - Xj(3,1) = q; - Xj(4,0) = -q; - S(5) = 1; - break; - case -4: // reversed prismatic X axis '-Px' - Xj(4,2) = -q; - Xj(5,1) = q; - S(3) = -1; - break; - case -5: // reversed prismatic Y axis '-Py' - Xj(3,2) = q; - Xj(5,0) = -q; - S(4) = -1; - break; - case -6: // reversed prismatic Z axis '-Pz' - Xj(4,2) = q; - Xj(5,1) = -q; - S(5) = -1; - break; - default: - throw std::invalid_argument("spatial.hpp: jcalc(): unknown joint type!"); - break; - } - - if (fabs(jtyp) <= 3) { - Xj.block(3,3,3,3) = Xj.block(0,0,3,3); - } -} - -void jcalc(Matrix6d& Xj, - Matrix6d& dXjdq, - Vector6d& S, - const int jtyp, - const double q) { - Xj.setIdentity(); - dXjdq.setZero(); - S.setZero(); - - double c = cos(q); - double s = sin(q); - double dcdq = -s; - double dsdq = c; - - if (jtyp < 0) { // reversed direction - // c = cos(-q) = cos(q) - s = -s; // s = sin(-q) = -sin(q) - dsdq = -dsdq; - } - - switch (jtyp) - { - case 0: // fixed joint - // do nothing, results already set - break; - case 1: // revolute X axis 'Rx' - Xj(1,1) = c; - Xj(1,2) = s; - Xj(2,1) = -s; - Xj(2,2) = c; - dXjdq(1,1) = dcdq; - dXjdq(1,2) = dsdq; - dXjdq(2,1) = -dsdq; - dXjdq(2,2) = dcdq; - S(0) = 1; - break; - case 2: // revolute Y axis 'Ry' - Xj(0,0) = c; - Xj(0,2) = -s; - Xj(2,0) = s; - Xj(2,2) = c; - dXjdq(0,0) = dcdq; - dXjdq(0,2) = -dsdq; - dXjdq(2,0) = dsdq; - dXjdq(2,2) = dcdq; - S(1) = 1; - break; - case 3: // revolute Z axis 'Rz' - Xj(0,0) = c; - Xj(0,1) = s; - Xj(1,0) = -s; - Xj(1,1) = c; - dXjdq(0,0) = dcdq; - dXjdq(0,1) = dsdq; - dXjdq(1,0) = -dsdq; - dXjdq(1,1) = dcdq; - S(2) = 1; - break; - case -1: // reversed revolute X axis '-Rx' - Xj(1,1) = c; - Xj(1,2) = s; - Xj(2,1) = -s; - Xj(2,2) = c; - dXjdq(1,1) = dcdq; - dXjdq(1,2) = dsdq; - dXjdq(2,1) = -dsdq; - dXjdq(2,2) = dcdq; - S(0) = -1; - break; - case -2: // reversed revolute Y axis '-Ry' - Xj(0,0) = c; - Xj(0,2) = -s; - Xj(2,0) = s; - Xj(2,2) = c; - dXjdq(0,0) = dcdq; - dXjdq(0,2) = -dsdq; - dXjdq(2,0) = dsdq; - dXjdq(2,2) = dcdq; - S(1) = -1; - break; - case -3: // reversed revolute Z axis '-Rz' - Xj(0,0) = c; - Xj(0,1) = s; - Xj(1,0) = -s; - Xj(1,1) = c; - dXjdq(0,0) = dcdq; - dXjdq(0,1) = dsdq; - dXjdq(1,0) = -dsdq; - dXjdq(1,1) = dcdq; - S(2) = -1; - break; - case 4: // prismatic X axis 'Px' - Xj(4,2) = q; - Xj(5,1) = -q; - dXjdq(4,2) = 1; - dXjdq(5,1) = -1; - S(3) = 1; - break; - case 5: // prismatic Y axis 'Py' - Xj(3,2) = -q; - Xj(5,0) = q; - dXjdq(3,2) = -1; - dXjdq(5,0) = 1; - S(4) = 1; - break; - case 6: // prismatic Z axis 'Pz' - Xj(3,1) = q; - Xj(4,0) = -q; - dXjdq(3,1) = 1; - dXjdq(4,0) = -1; - S(5) = 1; - break; - case -4: // reversed prismatic X axis '-Px' - Xj(4,2) = -q; - Xj(5,1) = q; - dXjdq(4,2) = -1; - dXjdq(5,1) = 1; - S(3) = -1; - break; - case -5: // reversed prismatic Y axis '-Py' - Xj(3,2) = q; - Xj(5,0) = -q; - dXjdq(3,2) = 1; - dXjdq(5,0) = -1; - S(4) = -1; - break; - case -6: // reversed prismatic Z axis '-Pz' - Xj(4,2) = q; - Xj(5,1) = -q; - dXjdq(4,2) = 1; - dXjdq(5,1) = -1; - S(5) = -1; - break; - default: - throw std::invalid_argument("spatial.hpp: jcalc(): unknown joint type!"); - break; - } - - if (fabs(jtyp) <= 3) { - Xj.block(3,3,3,3) = Xj.block(0,0,3,3); - dXjdq.block(3,3,3,3) = dXjdq.block(0,0,3,3); - } -} - -void jcalc(Matrix6d& Xj, - Matrix6d& dXjdt, - Vector6d& S, - const int jtyp, - const double q, - const double q_d) { - Xj.setIdentity(); - dXjdt.setZero(); - S.setZero(); - - double c = cos(q); - double s = sin(q); - double dcdt = -s * q_d; - double dsdt = c * q_d; - - if (jtyp < 0) { // reversed direction - // c = cos(-q) = cos(q) - s = -s; // s = sin(-q) = -sin(q) - dsdt = -dsdt; - } - - switch (jtyp) - { - case 0: // fixed joint - // do nothing, results already set - break; - case 1: // revolute X axis 'Rx' - Xj(1,1) = c; - Xj(1,2) = s; - Xj(2,1) = -s; - Xj(2,2) = c; - dXjdt(1,1) = dcdt; - dXjdt(1,2) = dsdt; - dXjdt(2,1) = -dsdt; - dXjdt(2,2) = dcdt; - S(0) = 1; - break; - case 2: // revolute Y axis 'Ry' - Xj(0,0) = c; - Xj(0,2) = -s; - Xj(2,0) = s; - Xj(2,2) = c; - dXjdt(0,0) = dcdt; - dXjdt(0,2) = -dsdt; - dXjdt(2,0) = dsdt; - dXjdt(2,2) = dcdt; - S(1) = 1; - break; - case 3: // revolute Z axis 'Rz' - Xj(0,0) = c; - Xj(0,1) = s; - Xj(1,0) = -s; - Xj(1,1) = c; - dXjdt(0,0) = dcdt; - dXjdt(0,1) = dsdt; - dXjdt(1,0) = -dsdt; - dXjdt(1,1) = dcdt; - S(2) = 1; - break; - case -1: // reversed revolute X axis '-Rx' - Xj(1,1) = c; - Xj(1,2) = s; - Xj(2,1) = -s; - Xj(2,2) = c; - dXjdt(1,1) = dcdt; - dXjdt(1,2) = dsdt; - dXjdt(2,1) = -dsdt; - dXjdt(2,2) = dcdt; - S(0) = -1; - break; - case -2: // reversed revolute Y axis '-Ry' - Xj(0,0) = c; - Xj(0,2) = -s; - Xj(2,0) = s; - Xj(2,2) = c; - dXjdt(0,0) = dcdt; - dXjdt(0,2) = -dsdt; - dXjdt(2,0) = dsdt; - dXjdt(2,2) = dcdt; - S(1) = -1; - break; - case -3: // reversed revolute Z axis '-Rz' - Xj(0,0) = c; - Xj(0,1) = s; - Xj(1,0) = -s; - Xj(1,1) = c; - dXjdt(0,0) = dcdt; - dXjdt(0,1) = dsdt; - dXjdt(1,0) = -dsdt; - dXjdt(1,1) = dcdt; - S(2) = -1; - break; - case 4: // prismatic X axis 'Px' - Xj(4,2) = q; - Xj(5,1) = -q; - dXjdt(4,2) = q_d; - dXjdt(5,1) = -q_d; - S(3) = 1; - break; - case 5: // prismatic Y axis 'Py' - Xj(3,2) = -q; - Xj(5,0) = q; - dXjdt(3,2) = -q_d; - dXjdt(5,0) = q_d; - S(4) = 1; - break; - case 6: // prismatic Z axis 'Pz' - Xj(3,1) = q; - Xj(4,0) = -q; - dXjdt(3,1) = q_d; - dXjdt(4,0) = -q_d; - S(5) = 1; - break; - case -4: // reversed prismatic X axis '-Px' - Xj(4,2) = -q; - Xj(5,1) = q; - dXjdt(4,2) = -q_d; - dXjdt(5,1) = q_d; - S(3) = -1; - break; - case -5: // reversed prismatic Y axis '-Py' - Xj(3,2) = q; - Xj(5,0) = -q; - dXjdt(3,2) = q_d; - dXjdt(5,0) = -q_d; - S(4) = -1; - break; - case -6: // reversed prismatic Z axis '-Pz' - Xj(4,2) = q; - Xj(5,1) = -q; - dXjdt(4,2) = q_d; - dXjdt(5,1) = -q_d; - S(5) = -1; - break; - default: - throw std::invalid_argument("spatial.hpp: jcalc(): unknown joint type!"); - break; - } - - if (fabs(jtyp) <= 3) { - Xj.block(3,3,3,3) = Xj.block(0,0,3,3); - dXjdt.block(3,3,3,3) = dXjdt.block(0,0,3,3); - } -} - -Matrix6d crm(const Vector6d& v) { - Matrix6d vcross; - vcross.setZero(); - - // vcross = [ 0 -v(3) v(2) 0 0 0 ; - // v(3) 0 -v(1) 0 0 0 ; - // -v(2) v(1) 0 0 0 0 ; - // 0 -v(6) v(5) 0 -v(3) v(2) ; - // v(6) 0 -v(4) v(3) 0 -v(1) ; - // -v(5) v(4) 0 -v(2) v(1) 0 ]; - - vcross(0, 1) = -v(2); - vcross(0, 2) = v(1); - vcross(1, 0) = v(2); - vcross(1, 2) = -v(0); - vcross(2, 0) = -v(1); - vcross(2, 1) = v(0); - vcross.block(3,3,3,3) = vcross.block(0,0,3,3); - - vcross(3, 1) = -v(5); - vcross(3, 2) = v(4); - vcross(4, 0) = v(5); - vcross(4, 2) = -v(3); - vcross(5, 0) = -v(4); - vcross(5, 1) = v(3); - - return vcross; -} - -Matrix6d crf(const Vector6d& v) { - Matrix6d vcross = crm(v); - return -vcross.transpose(); -} - -}; // namespace RAPTOR - -#endif // SPATIAL_CPP diff --git a/KinematicsDynamics/src/Transform.cpp b/KinematicsDynamics/src/Transform.cpp deleted file mode 100644 index 337f04d9..00000000 --- a/KinematicsDynamics/src/Transform.cpp +++ /dev/null @@ -1,346 +0,0 @@ -#include "Transform.h" - -namespace RAPTOR { - -Transform::Transform() { - R.setIdentity(); - p.setZero(); - ifDerivative = false; -} - -Transform::Transform(const Mat3& R_in, - const Vec3& p_in, - const bool i_in) : - R(R_in), - p(p_in), - ifDerivative(i_in) { -} - -Transform::Transform(const Vec3& rpy_in, - const Vec3& p_in) : - p(p_in) { - R = Eigen::AngleAxisd(rpy_in(0), Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(rpy_in(1), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy_in(2), Eigen::Vector3d::UnitZ()); - ifDerivative = false; -} - -Transform::Transform(const Vec3& p_in) : - p(p_in) { - R.setIdentity(); - ifDerivative = false; -} - -Transform::Transform(const int jtype, - const double theta, - const int order) { - if (order == 0) { // Transform of rotation matrix - R = Mat3::Identity(); - } - else if (order == 1) { // First order derivative of transform of rotation matrix - R = Mat3::Zero(); - } - else if (order == 2) { // Second order derivative of transform of rotation matrix - R = Mat3::Zero(); - } - else if (order == 3) { // Third order derivative of transform of rotation matrix - R = Mat3::Zero(); - } - else { - throw std::runtime_error("spatial.cpp: Transform(): Unsupported differentiate order!"); - } - p = Vec3::Zero(3); // translation vector - - ifDerivative = (order > 0); - - if (order == 0) { // original transformation - if (jtype == 1) { - R << 1, 0, 0, - 0, cos(theta), -sin(theta), - 0, sin(theta), cos(theta); - } - else if (jtype == -1) { - R << 1, 0, 0, - 0, cos(theta), sin(theta), - 0, -sin(theta), cos(theta); - } - else if (jtype == 2) { - R << cos(theta), 0, sin(theta), - 0, 1, 0, - -sin(theta), 0, cos(theta); - } - else if (jtype == -2) { - R << cos(theta), 0, -sin(theta), - 0, 1, 0, - sin(theta), 0, cos(theta); - } - else if (jtype == 3) { - R << cos(theta), -sin(theta), 0, - sin(theta), cos(theta), 0, - 0, 0, 1; - } - else if (jtype == -3) { - R << cos(theta), sin(theta), 0, - -sin(theta), cos(theta), 0, - 0, 0, 1; - } - else if (jtype == 4) { - p << theta, 0, 0; - } - else if (jtype == 5) { - p << 0, theta, 0; - } - else if (jtype == 6) { - p << 0, 0, theta; - } - else if (jtype == -4) { - p << -theta, 0, 0; - } - else if (jtype == -5) { - p << 0, -theta, 0; - } - else if (jtype == -6) { - p << 0, 0, -theta; - } - else { - throw std::runtime_error("spatial.cpp: Transform(): Unrecognized jtype!"); - } - } - else if (order == 1) { // first order derivative - if (jtype == 1) { - R << 0, 0, 0, - 0, -sin(theta), -cos(theta), - 0, cos(theta), -sin(theta); - } - else if (jtype == -1) { - R << 0, 0, 0, - 0, -sin(theta), cos(theta), - 0, -cos(theta), -sin(theta); - } - else if (jtype == 2) { - R << -sin(theta), 0, cos(theta), - 0, 0, 0, - -cos(theta), 0, -sin(theta); - } - else if (jtype == -2) { - R << -sin(theta), 0, -cos(theta), - 0, 0, 0, - cos(theta), 0, -sin(theta); - } - else if (jtype == 3) { - R << -sin(theta), -cos(theta), 0, - cos(theta), -sin(theta), 0, - 0, 0, 0; - } - else if (jtype == -3) { - R << -sin(theta), cos(theta), 0, - -cos(theta), -sin(theta), 0, - 0, 0, 0; - } - else if (jtype == 4) { - p << 1, 0, 0; - } - else if (jtype == 5) { - p << 0, 1, 0; - } - else if (jtype == 6) { - p << 0, 0, 1; - } - else if (jtype == -4) { - p << -1, 0, 0; - } - else if (jtype == -5) { - p << 0, -1, 0; - } - else if (jtype == -6) { - p << 0, 0, -1; - } - else { - throw std::runtime_error("spatial.cpp: Transform(): Unrecognized jtype!"); - } - } - else if (order == 2) { // Second order derivative - if (jtype == 1) { - R << 0, 0, 0, - 0, -cos(theta), sin(theta), - 0, -sin(theta), -cos(theta); - } - else if (jtype == -1) { - R << 0, 0, 0, - 0, -cos(theta), -sin(theta), - 0, sin(theta), -cos(theta); - } - else if (jtype == 2) { - R << -cos(theta), 0, -sin(theta), - 0, 0, 0, - sin(theta), 0, -cos(theta); - } - else if (jtype == -2) { - R << -cos(theta), 0, sin(theta), - 0, 0, 0, - -sin(theta), 0, -cos(theta); - } - else if (jtype == 3) { - R << -cos(theta), sin(theta), 0, - -sin(theta), -cos(theta), 0, - 0, 0, 0; - } - else if (jtype == -3) { - R << -cos(theta), -sin(theta), 0, - sin(theta), -cos(theta), 0, - 0, 0, 0; - } - else if (jtype == 4) { - p << 0, 0, 0; - } - else if (jtype == 5) { - p << 0, 0, 0; - } - else if (jtype == 6) { - p << 0, 0, 0; - } - else if (jtype == -4) { - p << 0, 0, 0; - } - else if (jtype == -5) { - p << 0, 0, 0; - } - else if (jtype == -6) { - p << 0, 0, 0; - } - else { - throw std::runtime_error("spatial.cpp: Transform(): Unrecognized jtype!"); - } - } - else if (order == 3) { // Third order derivative - if (jtype == 1) { - R << 0, 0, 0, - 0, sin(theta), cos(theta), - 0, -cos(theta), sin(theta); - } - else if (jtype == -1) { - R << 0, 0, 0, - 0, sin(theta), -cos(theta), - 0, cos(theta), sin(theta); - } - else if (jtype == 2) { - R << sin(theta), 0, -cos(theta), - 0, 0, 0, - cos(theta), 0, sin(theta); - } - else if (jtype == -2) { - R << sin(theta), 0, cos(theta), - 0, 0, 0, - -cos(theta), 0, sin(theta); - } - else if (jtype == 3) { - R << sin(theta), cos(theta), 0, - -cos(theta), sin(theta), 0, - 0, 0, 0; - } - else if (jtype == -3) { - R << sin(theta), -cos(theta), 0, - cos(theta), sin(theta), 0, - 0, 0, 0; - } - else if (jtype == 4) { - p << 0, 0, 0; - } - else if (jtype == 5) { - p << 0, 0, 0; - } - else if (jtype == 6) { - p << 0, 0, 0; - } - else if (jtype == -4) { - p << 0, 0, 0; - } - else if (jtype == -5) { - p << 0, 0, 0; - } - else if (jtype == -6) { - p << 0, 0, 0; - } - else { - throw std::runtime_error("spatial.cpp: Transform(): Unrecognized jtype!"); - } - } - else { - throw std::runtime_error("spatial.cpp: Transform(): Unsupported differentiate order!"); - } -} - -Transform Transform::operator=(const Transform& x) { - R = x.R; - p = x.p; - ifDerivative = x.ifDerivative; - return *this; -} - -Transform Transform::operator*(const Transform& x) const { - bool i = ifDerivative | x.ifDerivative; - if (x.ifDerivative) return Transform(R * x.R, R * x.p, i); - return Transform(R * x.R, R * x.p + p, i); -} - -Transform Transform::operator*=(const Transform& x) { - if (x.ifDerivative) { - p = R * x.p; - } - else { - p = R * x.p + p; - } - R = R * x.R; - ifDerivative |= x.ifDerivative; - return *this; -} - -Transform Transform::operator*(const SE3& x) const { - return Transform(R * x.rotation(), - R * x.translation() + p, - ifDerivative); -} - -Transform Transform::operator*=(const SE3& x) { - p = R * x.translation() + p; - R = R * x.rotation(); - return *this; -} - -Transform operator*(const pinocchio::SE3Tpl& x, const Transform& sRp) { - return Transform(x.rotation() * sRp.R, - x.rotation() * sRp.p + x.translation(), - sRp.ifDerivative); -} - -Transform Transform::inverse() const { - assert(!ifDerivative); - return Transform(R.transpose(), -R.transpose() * p, false); -} - -Eigen::Vector3d Transform::getRPY() const { - // roll pitch yaw conversion does not make sense - // for the derivative of a transformation matrix - assert(!ifDerivative); - - Eigen::Vector3d rpy; - rpy << atan2(-R(1,2), R(2,2)), // roll - HigherOrderDerivatives::safeasin(R(0,2)), // pitch - atan2(-R(0,1), R(0,0)); // yaw - - return rpy; -} - -Eigen::Matrix Transform::getXYZRPY() const { - Eigen::Matrix xyzrpy; - xyzrpy << p, getRPY(); - return xyzrpy; -} - -std::ostream& operator<<(std::ostream& os, const Transform& sRp) { - os << "rotation:\n" << sRp.R << std::endl - << "translation:\n" << sRp.p << std::endl; - return os; -} - -} // namespace RAPTOR diff --git a/Optimization/include/Optimizer.h b/Optimization/include/Optimizer.h deleted file mode 100644 index 02be22a1..00000000 --- a/Optimization/include/Optimizer.h +++ /dev/null @@ -1,237 +0,0 @@ -#ifndef OPTIMIZER_H -#define OPTIMIZER_H - -#include "IpTNLP.hpp" -#include "IpIpoptApplication.hpp" - -#include -#include -#include -#include -#include - -#include "Utils.h" -#include "Constraints.h" - -namespace RAPTOR { - -using namespace Ipopt; - -namespace OptimizerConstants { - enum FeasibleState { - INFEASIBLE = 0, - FEASIBLE = 1, - UNINITIALIZED = 2 - }; -}; - -class Optimizer : public Ipopt::TNLP { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - Optimizer() = default; - - /** Default destructor */ - virtual ~Optimizer() = default; - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - virtual bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) = 0; - - /** Method to return the bounds for my problem */ - virtual bool get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u - ); - - /** Method to return the starting point for the algorithm */ - virtual bool get_starting_point( - Index n, - bool init_x, - Number* x, - bool init_z, - Number* z_L, - Number* z_U, - Index m, - bool init_lambda, - Number* lambda - ); - - /** Method to warm start the entire iterate */ - virtual bool GetWarmStartIterate( - IteratesVector& /*warm_start_iterate*/ - ) - { - return false; - } - - /** Method to update the solution with the minimal cost in the current iteration */ - virtual bool update_minimal_cost_solution( - Index n, - const VecX& z, - bool new_x, - Number obj_value - ); - - /** Method to return the objective value */ - virtual bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) = 0; - - /** Method to return the gradient of the objective */ - virtual bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) = 0; - - /** Method to return the hessian of the objective */ - virtual bool eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f - ); - - /** Method to return the constraint residuals */ - virtual bool eval_g( - Index n, - const Number* x, - bool new_x, - Index m, - Number* g - ); - - /** Method to return: - * 1) The structure of the jacobian (if "values" is NULL) - * 2) The values of the jacobian (if "values" is not NULL) - */ - virtual bool eval_jac_g( - Index n, - const Number* x, - bool new_x, - Index m, - Index nele_jac, - Index* iRow, - Index* jCol, - Number* values - ); - - /** Method to return: - * 1) The structure of the hessian of the lagrangian (if "values" is NULL) - * 2) The values of the hessian of the lagrangian (if "values" is not NULL) - */ - virtual bool eval_h( - Index n, - const Number* x, - bool new_x, - Number obj_factor, - Index m, - const Number* lambda, - bool new_lambda, - Index nele_hess, - Index* iRow, - Index* jCol, - Number* values - ); - - /** This method is called when the algorithm is complete so the TNLP can store/write the solution */ - virtual void finalize_solution( - SolverReturn status, - Index n, - const Number* x, - const Number* z_L, - const Number* z_U, - Index m, - const Number* g, - const Number* lambda, - Number obj_value, - const IpoptData* ip_data, - IpoptCalculatedQuantities* ip_cq - ); - //@} - - /** This method summarizes constraint violation for each type of constraints */ - virtual void summarize_constraints( - Index m, - const Number* g, - const bool verbose = true - ); - //@} - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - Optimizer( - const Optimizer& - ); - - Optimizer& operator=( - const Optimizer& - ); - - // class members: - std::chrono::_V2::system_clock::time_point start_time; - std::chrono::_V2::system_clock::time_point end_time; - bool output_computation_time = false; - - bool enable_hessian = false; - - Index numVars = 0; // number of variables - Index numCons = 0; // number of constraints - - VecX x0; // stores the initial guess here - - std::vector> constraintsPtrVec_; - std::vector constraintsNameVec_; - - bool ifFeasibleCurrIter = false; - VecX currentIpoptSolution; - Number currentIpoptObjValue = std::numeric_limits::max(); - OptimizerConstants::FeasibleState ifCurrentIpoptFeasible = OptimizerConstants::FeasibleState::UNINITIALIZED; - - VecX optimalIpoptSolution; - Number optimalIpoptObjValue = std::numeric_limits::max(); - OptimizerConstants::FeasibleState ifOptimalIpoptFeasible = OptimizerConstants::FeasibleState::UNINITIALIZED; - - VecX solution; // stores the final solution here - Number obj_value_copy = 0; - std::vector g_copy; - - Number final_constr_violation = 0; - bool ifFeasible = true; - - // This is supposed to be consistent with the settings in IpoptApplicationFactory - // But right now it is hardcoded here - // You have to manually change this from outside - Number constr_viol_tol = 1e-4; -}; - -}; // namespace RAPTOR - -#endif // OPTIMIZER_H \ No newline at end of file diff --git a/Optimization/src/Optimizer.cpp b/Optimization/src/Optimizer.cpp deleted file mode 100644 index 0bdc2a1d..00000000 --- a/Optimization/src/Optimizer.cpp +++ /dev/null @@ -1,631 +0,0 @@ -#include "Optimizer.h" - -namespace RAPTOR { - -// [TNLP_get_bounds_info] -// returns the variable bounds -bool Optimizer::get_bounds_info( - Index n, - Number* x_l, - Number* x_u, - Index m, - Number* g_l, - Number* g_u -) -{ - // here, the n and m we gave IPOPT in get_nlp_info are passed back to us. - // If desired, we could assert to make sure they are what we think they are. - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in get_bounds_info!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in get_bounds_info!"); - } - - // lower bounds - for( Index i = 0; i < n; i++ ) { - x_l[i] = -1e19; - } - - // upper bounds - for( Index i = 0; i < n; i++ ) { - x_u[i] = 1e19; - } - - if (constraintsPtrVec_.size() != constraintsNameVec_.size()) { - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ and constraintsNameVec_ have different sizes!"); - } - - // compute bounds for all constraints - Index iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - try { - constraintsPtrVec_[c]->compute_bounds(); - } - catch (std::exception& e) { - std::cerr << "Error in " << constraintsNameVec_[c] << ": " << std::endl; - std::cerr << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in get_bounds_info! Check previous error message."); - } - - if (constraintsPtrVec_[c]->m != constraintsPtrVec_[c]->g_lb.size() || - constraintsPtrVec_[c]->m != constraintsPtrVec_[c]->g_ub.size()) { - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ have different sizes!"); - } - - for ( Index i = 0; i < constraintsPtrVec_[c]->m; i++ ) { - g_l[iter] = constraintsPtrVec_[c]->g_lb(i); - g_u[iter] = constraintsPtrVec_[c]->g_ub(i); - iter++; - } - } - - // report constraints distribution - std::cout << "Dimension of each constraints and their locations: \n"; - iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - std::cout << constraintsNameVec_[c] << ": "; - std::cout << constraintsPtrVec_[c]->m << " ["; - std::cout << iter << " "; - iter += constraintsPtrVec_[c]->m; - std::cout << iter << "]\n"; - } - - return true; -} -// [TNLP_get_bounds_info] - -// [TNLP_get_starting_point] -// returns the initial point for the problem -bool Optimizer::get_starting_point( - Index n, - bool init_x, - Number* x, - bool init_z, - Number* z_L, - Number* z_U, - Index m, - bool init_lambda, - Number* lambda -) -{ - // Here, we assume we only have starting values for x, if you code - // your own NLP, you can provide starting values for the dual variables - // if you wish - if (init_x == false || init_z == true || init_lambda == true) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of init in get_starting_point!"); - } - - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in get_starting_point!"); - } - - if (x0.size() != numVars) { - std::cerr << "x0.size() = " << x0.size() << std::endl; - std::cerr << "numVars = " << numVars << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error x0.size() != numVars in get_starting_point!"); - } - - for ( Index i = 0; i < n; i++ ) { - x[i] = x0(i); - } - - return true; -} -// [TNLP_get_starting_point] - -// [TNLP_update_minimal_cost_solution] -bool Optimizer::update_minimal_cost_solution( - Index n, - const VecX& z, - bool new_x, - Number obj_value -) -{ - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in update_minimal_cost_solution!"); - } - - // update status of the current solution - if (new_x) { // directly assign currentIpoptSolution if this x has never been evaluated before - currentIpoptSolution = z; - currentIpoptObjValue = obj_value; - ifCurrentIpoptFeasible = OptimizerConstants::FeasibleState::UNINITIALIZED; - } - else { // update currentIpoptSolution - if (Utils::ifTwoVectorEqual(currentIpoptSolution, z, 0)) { - if (ifCurrentIpoptFeasible == OptimizerConstants::FeasibleState::UNINITIALIZED) { - THROW_EXCEPTION(IpoptException, "*** Error ifCurrentIpoptFeasible is not initialized!"); - } - else { // this has been evaluated in eval_g, just need to update the cost - currentIpoptObjValue = obj_value; - } - } - else { - currentIpoptSolution = z; - currentIpoptObjValue = obj_value; - ifCurrentIpoptFeasible = OptimizerConstants::FeasibleState::UNINITIALIZED; - } - } - - // update the status of the optimal solution - if (ifCurrentIpoptFeasible == OptimizerConstants::FeasibleState::FEASIBLE && - currentIpoptObjValue < optimalIpoptObjValue) { - optimalIpoptSolution = currentIpoptSolution; - optimalIpoptObjValue = currentIpoptObjValue; - ifOptimalIpoptFeasible = ifCurrentIpoptFeasible; - } - - return true; -} -// [TNLP_update_minimal_cost_solution] - -// [eval_hess_f] -// returns the hessian of the objective -bool Optimizer::eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f -) -{ - throw std::invalid_argument("Objective function hessian is not implemented! Disable hessian please."); - return false; -} -// [eval_hess_f] - -// [TNLP_eval_g] -// return the value of the constraints: g(x) -bool Optimizer::eval_g( - Index n, - const Number* x, - bool new_x, - Index m, - Number* g -) -{ - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_g!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in eval_g!"); - } - - // fill in a Eigen Vector instance of decision variables - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - Index iter = 0; - ifFeasibleCurrIter = true; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - // compute constraints - if (output_computation_time) { - start_time = std::chrono::high_resolution_clock::now(); - } - - try { - constraintsPtrVec_[c]->compute(z, false); - } - catch (std::exception& e) { - std::cerr << "Error in " << constraintsNameVec_[c] << ": " << std::endl; - std::cerr << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in eval_g: " + constraintsNameVec_[c] + "! Check previous error message."); - } - - if (output_computation_time) { - end_time = std::chrono::high_resolution_clock::now(); - std::cout << "eval_g compute time for " << constraintsNameVec_[c] - << " is " << std::chrono::duration_cast(end_time - start_time).count() - << " microseconds.\n"; - } - - // test if constraints are feasible - if ((constraintsPtrVec_[c]->g - constraintsPtrVec_[c]->g_lb).minCoeff() < -constr_viol_tol || - (constraintsPtrVec_[c]->g_ub - constraintsPtrVec_[c]->g).minCoeff() < -constr_viol_tol) { - ifFeasibleCurrIter = false; - } - - // fill in constraints - for ( Index i = 0; i < constraintsPtrVec_[c]->m; i++ ) { - g[iter] = constraintsPtrVec_[c]->g(i); - iter++; - } - } - - // update status of the current solution - if (new_x) { // directly assign currentIpoptSolution if this x has never been evaluated before - currentIpoptSolution = z; - currentIpoptObjValue = std::numeric_limits::max(); - ifCurrentIpoptFeasible = ifFeasibleCurrIter ? - OptimizerConstants::FeasibleState::FEASIBLE : - OptimizerConstants::FeasibleState::INFEASIBLE; - } - else { // update currentIpoptSolution - if (Utils::ifTwoVectorEqual(currentIpoptSolution, z, 0)) { - if (currentIpoptObjValue == std::numeric_limits::max()) { - THROW_EXCEPTION(IpoptException, "*** Error currentIpoptObjValue is not initialized!"); - } - else { // this has been evaluated in eval_f, just need to update the feasibility - ifCurrentIpoptFeasible = ifFeasibleCurrIter ? - OptimizerConstants::FeasibleState::FEASIBLE : - OptimizerConstants::FeasibleState::INFEASIBLE; - } - } - else { - currentIpoptSolution = z; - currentIpoptObjValue = std::numeric_limits::max(); - ifCurrentIpoptFeasible = ifFeasibleCurrIter ? - OptimizerConstants::FeasibleState::FEASIBLE : - OptimizerConstants::FeasibleState::INFEASIBLE; - } - } - - // update the status of the optimal solution - if (ifCurrentIpoptFeasible == OptimizerConstants::FeasibleState::FEASIBLE && - currentIpoptObjValue < optimalIpoptObjValue) { - optimalIpoptSolution = currentIpoptSolution; - optimalIpoptObjValue = currentIpoptObjValue; - ifOptimalIpoptFeasible = ifCurrentIpoptFeasible; - } - - return true; -} -// [TNLP_eval_g] - -// [TNLP_eval_jac_g] -// return the structure or values of the Jacobian -bool Optimizer::eval_jac_g( - Index n, - const Number* x, - bool new_x, - Index m, - Index nele_jac, - Index* iRow, - Index* jCol, - Number* values -) -{ - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_jac_g!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in eval_jac_g!"); - } - - if( values == NULL ) { - // return the structure of the Jacobian - // this particular Jacobian is dense - for(Index i = 0; i < m; i++){ - for(Index j = 0; j < n; j++){ - iRow[i * n + j] = i; - jCol[i * n + j] = j; - } - } - } - else { - // fill in a Eigen Vector instance of decision variables - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - Index iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - // compute constraints - if (output_computation_time) { - start_time = std::chrono::high_resolution_clock::now(); - } - - try { - constraintsPtrVec_[c]->compute(z, true); - } - catch (std::exception& e) { - std::cerr << "Error in " << constraintsNameVec_[c] << ": " << std::endl; - std::cout << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in eval_jac_g in: " + constraintsNameVec_[c] + "! Check previous error message."); - } - - if (output_computation_time) { - end_time = std::chrono::high_resolution_clock::now(); - std::cout << "eval_jac_g compute time for " << constraintsNameVec_[c] - << " is " << std::chrono::duration_cast(end_time - start_time).count() - << " microseconds.\n"; - } - - // fill in constraints - for ( Index i = 0; i < constraintsPtrVec_[c]->pg_pz.rows(); i++ ) { - for ( Index j = 0; j < constraintsPtrVec_[c]->pg_pz.cols(); j++ ) { - values[iter] = constraintsPtrVec_[c]->pg_pz(i, j); - iter++; - } - } - } - } - - return true; -} -// [TNLP_eval_jac_g] - -// [TNLP_eval_h] -//return the structure or values of the Hessian -bool Optimizer::eval_h( - Index n, - const Number* x, - bool new_x, - Number obj_factor, - Index m, - const Number* lambda, - bool new_lambda, - Index nele_hess, - Index* iRow, - Index* jCol, - Number* values -) -{ - if (!enable_hessian) { - return false; - } - - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_h!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in eval_h!"); - } - if (nele_hess != n * (n + 1) / 2) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of nele_hess in eval_h!"); - } - - if (values == NULL) { - /* return the structure */ - /* the hessian for this problem is actually dense */ - Index iter = 0; - for(Index i = 0; i < n; i++){ - for(Index j = i; j < n; j++){ - iRow[iter] = i; - jCol[iter] = j; - iter++; - } - } - } - else { - // fill in a Eigen Vector instance of decision variables - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - MatX hessian(n, n); - hessian.setZero(); - - MatX hess_f(n, n); - hess_f.setZero(); - - if (obj_factor != 0) { - if (output_computation_time) { - start_time = std::chrono::high_resolution_clock::now(); - } - - try { - eval_hess_f(n, x, new_x, hess_f); - } - catch (std::exception& e) { - std::cerr << "Error in eval_hess_f: " << std::endl; - std::cerr << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in eval_hess_f! Check previous error message."); - } - - if (output_computation_time) { - end_time = std::chrono::high_resolution_clock::now(); - std::cout << "eval_hess_f compute time is" << std::chrono::duration_cast(end_time - start_time).count() - << " microseconds.\n"; - } - - hessian += obj_factor * hess_f; - } - - Index iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - // if lambda is zero for this constraint, skip (make CheckDerivative faster) - bool lambdaAllZeroForThisConstraint = true; - for (Index innerIter = iter; innerIter < iter + constraintsPtrVec_[c]->m; innerIter++) { - if (lambda[innerIter] != 0) { - lambdaAllZeroForThisConstraint = false; - break; - } - } - if (lambdaAllZeroForThisConstraint) { - iter += constraintsPtrVec_[c]->m; - continue; - } - - // compute constraints - if (output_computation_time) { - start_time = std::chrono::high_resolution_clock::now(); - } - - try { - constraintsPtrVec_[c]->compute(z, true, true); - } - catch (std::exception& e) { - std::cerr << "Error in " << constraintsNameVec_[c] << ": " << std::endl; - std::cout << e.what() << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error in eval_h in: " + constraintsNameVec_[c] + "! Check previous error message."); - } - - if (output_computation_time) { - end_time = std::chrono::high_resolution_clock::now(); - std::cout << "eval_hessian_g compute time for " << constraintsNameVec_[c] - << " is " << std::chrono::duration_cast(end_time - start_time).count() - << " microseconds.\n"; - } - - if (constraintsPtrVec_[c]->pg_pz_pz.size() != constraintsPtrVec_[c]->m) { - std::cerr << "Error in " - << constraintsNameVec_[c] - << ": " - << constraintsPtrVec_[c]->pg_pz_pz.size() - << " != " - << constraintsPtrVec_[c]->m << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ have wrong sizes!"); - } - - for (Index i = 0; i < constraintsPtrVec_[c]->m; i++) { - if (constraintsPtrVec_[c]->pg_pz_pz(i).rows() != n || - constraintsPtrVec_[c]->pg_pz_pz(i).cols() != n) { - std::cerr << "Error in " - << constraintsNameVec_[c] - << ": " - << constraintsPtrVec_[c]->pg_pz_pz(i).rows() - << "x" - << constraintsPtrVec_[c]->pg_pz_pz(i).cols() - << " != " - << n - << "x" - << n << std::endl; - THROW_EXCEPTION(IpoptException, "*** Error constraintsPtrVec_ have wrong sizes!"); - } - - hessian += lambda[iter] * constraintsPtrVec_[c]->pg_pz_pz(i); - iter++; - } - } - - iter = 0; - for(Index i = 0; i < n; i++){ - for(Index j = i; j < n; j++){ - values[iter] = hessian(i, j); - iter++; - } - } - - // auto end_time = std::chrono::high_resolution_clock::now(); - // std::cout << "eval_h time: " << std::chrono::duration_cast(end_time - start_time).count() << " microseconds.\n"; - } - - return true; -} -// [TNLP_eval_h] - -// [TNLP_finalize_solution] -void Optimizer::finalize_solution( - SolverReturn status, - Index n, - const Number* x, - const Number* z_L, - const Number* z_U, - Index m, - const Number* g, - const Number* lambda, - Number obj_value, - const IpoptData* ip_data, - IpoptCalculatedQuantities* ip_cq -) -{ - if (n != numVars) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in finalize_solution!"); - } - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in finalize_solution!"); - } - - // here is where we would store the solution to variables, or write to a file, etc - // so we could use the solution. - - // store the solution - solution.resize(n); - for( Index i = 0; i < n; i++ ) { - solution(i) = x[i]; - } - - // re-evaluate constraints to update values in each constraint instances - g_copy.resize(m); - eval_f(n, x, true, obj_value_copy); - eval_g(n, x, true, m, g_copy.data()); - summarize_constraints(m, g, false); - - bool recordedOptimalSolutionAvailable = - optimalIpoptSolution.size() == n && - optimalIpoptObjValue < std::numeric_limits::max() && - ifOptimalIpoptFeasible == OptimizerConstants::FeasibleState::FEASIBLE; - - if (recordedOptimalSolutionAvailable && - (!ifFeasible || optimalIpoptObjValue < obj_value)) { - ifFeasible = true; - solution = optimalIpoptSolution; - obj_value_copy = optimalIpoptObjValue; - - Number x_new[n]; - for( Index i = 0; i < n; i++ ) { - x_new[i] = solution(i); - } - - // re-evaluate constraints to update values in each constraint instances - eval_f(n, x_new, true, obj_value_copy); - eval_g(n, x_new, true, m, g_copy.data()); - summarize_constraints(m, g_copy.data()); - - std::cout << "Solution is not feasible or optimal but we have found one optimal feasible solution before" - << " with cost: " << optimalIpoptObjValue - << " and constraint violation: " << final_constr_violation << std::endl; - } - else { - summarize_constraints(m, g); - } - - std::cout << "Objective value: " << obj_value_copy << std::endl; -} -// [TNLP_finalize_solution] - -// [TNLP_summarize_constraints] -void Optimizer::summarize_constraints( - Index m, - const Number* g, - const bool verbose -) -{ - if (m != numCons) { - THROW_EXCEPTION(IpoptException, "*** Error wrong value of m in summarize_constraints!"); - } - - if (verbose) std::cout << "Constraint violation report:" << std::endl; - - Index iter = 0; - ifFeasible = true; - final_constr_violation = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - // find where the maximum constraint violation is - Number max_constr_violation = 0; - Index max_constr_violation_id1 = 0; - Index max_constr_violation_id2 = 0; - for (Index i = 0; i < constraintsPtrVec_[c]->m; i++) { - Number constr_violation = std::max(constraintsPtrVec_[c]->g_lb(i) - g[iter], - g[iter] - constraintsPtrVec_[c]->g_ub(i)); - - if (constr_violation > max_constr_violation) { - max_constr_violation_id1 = i; - max_constr_violation_id2 = iter; - max_constr_violation = constr_violation; - } - - if (constr_violation > final_constr_violation) { - final_constr_violation = constr_violation; - } - - iter++; - } - - // report constraint violation - if (max_constr_violation > constr_viol_tol) { - if (verbose) { - std::cout << constraintsNameVec_[c] << ": " << max_constr_violation << std::endl; - std::cout << " range: [" << constraintsPtrVec_[c]->g_lb[max_constr_violation_id1] - << ", " - << constraintsPtrVec_[c]->g_ub[max_constr_violation_id1] - << "], value: " << g[max_constr_violation_id2] << std::endl; - } - - constraintsPtrVec_[c]->print_violation_info(); - - ifFeasible = false; - } - } - - if (verbose) std::cout << "Total constraint violation: " << final_constr_violation << std::endl; -} -// [TNLP_summarize_constraints] - -}; // namespace RAPTOR \ No newline at end of file diff --git a/README.md b/README.md index 88cdd257..2c7e9d68 100644 --- a/README.md +++ b/README.md @@ -2,13 +2,13 @@ We aim to solve a trajectory optimization problem formulated as below -![ProblemFormulation](Assets/pic-ProblemFormulation.svg) +![ProblemFormulation](assets/pic-ProblemFormulation.svg) where we assume that the trajectories are already parameterized and only treat the trajectory parameters as the decision variables. For example, you can define your trajectory to be a polynomial (on a fixed interval [0,T]): -![TrajectoryFormulation](Assets/pic-TrajectoryFormulation.svg) +![TrajectoryFormulation](assets/pic-TrajectoryFormulation.svg) And based on that, you can add joint limit constraints, torque limit constraints, end effector constraints, or any other customized constraints to your optimization problem. diff --git a/Robots/digit-v3-modified/digit-v3-modified.urdf b/Robots/digit-v3-modified/digit-v3-modified.urdf deleted file mode 100644 index 8de13e8f..00000000 --- a/Robots/digit-v3-modified/digit-v3-modified.urdf +++ /dev/null @@ -1,1454 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Robots/digit-v3-modified/meshes/arm-v3/link-0.stl b/Robots/digit-v3-modified/meshes/arm-v3/link-0.stl deleted file mode 100644 index e08a17c6..00000000 Binary files a/Robots/digit-v3-modified/meshes/arm-v3/link-0.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/arm-v3/link-1.stl b/Robots/digit-v3-modified/meshes/arm-v3/link-1.stl deleted file mode 100644 index c9fedc7a..00000000 Binary files a/Robots/digit-v3-modified/meshes/arm-v3/link-1.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/arm-v3/link-2.stl b/Robots/digit-v3-modified/meshes/arm-v3/link-2.stl deleted file mode 100644 index b5198554..00000000 Binary files a/Robots/digit-v3-modified/meshes/arm-v3/link-2.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/arm-v3/link-3.stl b/Robots/digit-v3-modified/meshes/arm-v3/link-3.stl deleted file mode 100644 index 56a970fd..00000000 Binary files a/Robots/digit-v3-modified/meshes/arm-v3/link-3.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/arm-v3/link-4.stl b/Robots/digit-v3-modified/meshes/arm-v3/link-4.stl deleted file mode 100644 index 04b7d706..00000000 Binary files a/Robots/digit-v3-modified/meshes/arm-v3/link-4.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/achilles-rod.stl b/Robots/digit-v3-modified/meshes/leg-v3/achilles-rod.stl deleted file mode 100644 index 29454b9f..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/achilles-rod.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/heel-spring.stl b/Robots/digit-v3-modified/meshes/leg-v3/heel-spring.stl deleted file mode 100644 index 9336b7a7..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/heel-spring.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/hip-pitch-housing.stl b/Robots/digit-v3-modified/meshes/leg-v3/hip-pitch-housing.stl deleted file mode 100644 index 66cf206b..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/hip-pitch-housing.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/hip-pitch.stl b/Robots/digit-v3-modified/meshes/leg-v3/hip-pitch.stl deleted file mode 100644 index 836a7490..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/hip-pitch.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/hip-roll-housing.stl b/Robots/digit-v3-modified/meshes/leg-v3/hip-roll-housing.stl deleted file mode 100644 index 9368093b..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/hip-roll-housing.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/hip-yaw-housing.stl b/Robots/digit-v3-modified/meshes/leg-v3/hip-yaw-housing.stl deleted file mode 100644 index cdb742e9..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/hip-yaw-housing.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/knee.stl b/Robots/digit-v3-modified/meshes/leg-v3/knee.stl deleted file mode 100644 index f5deca25..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/knee.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/shin.stl b/Robots/digit-v3-modified/meshes/leg-v3/shin.stl deleted file mode 100644 index 72937abd..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/shin.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/tarsus.stl b/Robots/digit-v3-modified/meshes/leg-v3/tarsus.stl deleted file mode 100644 index 0fdb3253..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/tarsus.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/toe-a-rod.stl b/Robots/digit-v3-modified/meshes/leg-v3/toe-a-rod.stl deleted file mode 100644 index 0b43690b..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/toe-a-rod.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/toe-b-rod.stl b/Robots/digit-v3-modified/meshes/leg-v3/toe-b-rod.stl deleted file mode 100644 index 9b7133b1..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/toe-b-rod.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/toe-output.stl b/Robots/digit-v3-modified/meshes/leg-v3/toe-output.stl deleted file mode 100644 index fe539d3f..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/toe-output.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/toe-pitch.stl b/Robots/digit-v3-modified/meshes/leg-v3/toe-pitch.stl deleted file mode 100644 index 57cf028a..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/toe-pitch.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/leg-v3/toe-roll.stl b/Robots/digit-v3-modified/meshes/leg-v3/toe-roll.stl deleted file mode 100644 index 45cb62bb..00000000 Binary files a/Robots/digit-v3-modified/meshes/leg-v3/toe-roll.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/objects/tote.stl b/Robots/digit-v3-modified/meshes/objects/tote.stl deleted file mode 100644 index eee39561..00000000 Binary files a/Robots/digit-v3-modified/meshes/objects/tote.stl and /dev/null differ diff --git a/Robots/digit-v3-modified/meshes/torso-v3/torso.stl b/Robots/digit-v3-modified/meshes/torso-v3/torso.stl deleted file mode 100644 index 93eff68f..00000000 Binary files a/Robots/digit-v3-modified/meshes/torso-v3/torso.stl and /dev/null differ diff --git a/Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf b/Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf deleted file mode 100644 index 68f9115e..00000000 --- a/Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf +++ /dev/null @@ -1,1454 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Tests/Constraints/TestKinematicsConstraints.cpp b/Tests/Constraints/TestKinematicsConstraints.cpp deleted file mode 100644 index 72edc2a7..00000000 --- a/Tests/Constraints/TestKinematicsConstraints.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include "KinematicsConstraints.h" -// #include "Plain.h" -#include "Polynomials.h" -#include - -using namespace RAPTOR; - -int main() { - // Define robot model - // const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; - const std::string urdf_filename = "../Examples/Kinova/ArmourUnsafe/kinova.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - // jtype << 4, 5, 6, 1, 2, 3, - // 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3, - // 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3; - jtype << 3, 3, 3, 3, 3, 3, 3; - - // std::shared_ptr trajPtr_ = std::make_shared(model.nv); - std::shared_ptr trajPtr_ = std::make_shared(2.0, 10, model.nv, TimeDiscretization::Chebyshev, 3); - ForwardKinematicsSolver fkSolver(&model, jtype); - - // compute a valid transform using forward kinematics - std::srand(std::time(nullptr)); - Eigen::VectorXd z = 2 * M_PI * Eigen::VectorXd::Random(trajPtr_->varLength).array() - M_PI; - int start = 0; - int end = model.getJointId("joint_7"); - fkSolver.compute(start, end, z); - - KinematicsConstraints kc(trajPtr_, &model, jtype, end, 6, fkSolver.getTransform()); - - // simple test when difference is small - Eigen::VectorXd z_test = z.array() + 1e-6; - kc.compute(z_test, false); - std::cout << kc.g << std::endl << std::endl; - - // simple test when difference is large - z_test = z.array() + 1.0; - auto start_clock = std::chrono::high_resolution_clock::now(); - kc.compute(z_test, true, true); - auto end_clock = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_clock - start_clock); - std::cout << "Time taken (including gradient and hessian): " << duration.count() << " microseconds" << std::endl; - - std::cout << kc.g << std::endl << std::endl; - - // test gradient - const Eigen::MatrixXd J_analytical = kc.pg_pz; - Eigen::MatrixXd J_numerical = Eigen::MatrixXd::Zero(J_analytical.rows(), J_analytical.cols()); - for (int i = 0; i < z_test.size(); i++) { - Eigen::VectorXd q_plus = z_test; - q_plus(i) += 1e-7; - kc.compute(q_plus, false); - const Eigen::VectorXd g_plus = kc.g; - Eigen::VectorXd q_minus = z_test; - q_minus(i) -= 1e-7; - kc.compute(q_minus, false); - const Eigen::VectorXd g_minus = kc.g; - J_numerical.col(i) = (g_plus - g_minus) / 2e-7; - } - - // std::cout << "Analytical gradient: " << std::endl << J_analytical << std::endl << std::endl; - // std::cout << "Numerical gradient: " << std::endl << J_numerical << std::endl << std::endl; - std::cout << J_analytical - J_numerical << std::endl << std::endl; - - // test hessian - Eigen::Array H_analytical = kc.pg_pz_pz; - for (int i = 0; i < z_test.size(); i++) { - Eigen::VectorXd q_plus = z_test; - q_plus(i) += 1e-7; - kc.compute(q_plus, true, false); - const Eigen::MatrixXd J_plus = kc.pg_pz; - Eigen::VectorXd q_minus = z_test; - q_minus(i) -= 1e-7; - kc.compute(q_minus, true, false); - const Eigen::MatrixXd J_minus = kc.pg_pz; - const Eigen::MatrixXd H_numerical_row = (J_plus - J_minus) / 2e-7; - - for (int j = 0; j < 3; j++) { - std::cout << H_analytical(j).row(i) - H_numerical_row.row(j) << std::endl; - } - } - - return 0; -} diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp deleted file mode 100644 index 0a119c83..00000000 --- a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp +++ /dev/null @@ -1,234 +0,0 @@ -#include "Optimizer.h" -#include "ForwardKinematics.h" - -using namespace RAPTOR; -using namespace Ipopt; - -class FKGradientChecker : public Optimizer { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using Mat3 = Eigen::Matrix3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - FKGradientChecker() = default; - - /** Default destructor */ - ~FKGradientChecker() = default; - - bool set_parameters( - const VecX& x0_input, - const Model& model_input, - const Eigen::VectorXi& jtype_input - ) { - enable_hessian = true; - x0 = x0_input; - model = model_input; - jtype = jtype_input; - - fkPtr_ = std::make_unique(&model, jtype); - - return true; - } - - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override - { - // number of decision variables - numVars = model.nq; - n = numVars; - - m = 0; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; - } - - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override - { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - // fkPtr_->compute(0, model.nq, z); - // Vec3 rpy = fkPtr_->getRPY(); - - // obj_value = roll_weight * rpy(0) + - // pitch_weight * rpy(1) + - // yaw_weight * rpy(2); - - fkPtr_->compute(0, model.nq, z, nullptr, nullptr, 1); - MatX Jrpy = fkPtr_->getRPYJacobian(); - Vec3 rpy = Jrpy * VecX::Ones(n); - - obj_value = roll_weight * rpy(0) + - pitch_weight * rpy(1) + - yaw_weight * rpy(2); - - return true; - } - - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override - { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - // fkPtr_->compute(0, model.nq, z, nullptr, nullptr, 1); - // MatX Jrpy = fkPtr_->getRPYJacobian(); - - // for ( Index i = 0; i < n; i++ ) { - // grad_f[i] = roll_weight * Jrpy(0, i) + - // pitch_weight * Jrpy(1, i) + - // yaw_weight * Jrpy(2, i); - // } - - fkPtr_->compute(0, model.nq, z, nullptr, nullptr, 2); - Eigen::Array Hrpy; - fkPtr_->getRPYHessian(Hrpy); - - VecX Jr = Hrpy(0) * VecX::Ones(n); - VecX Jp = Hrpy(1) * VecX::Ones(n); - VecX Jy = Hrpy(2) * VecX::Ones(n); - - for ( Index i = 0; i < n; i++ ) { - grad_f[i] = roll_weight * Jr(i) + - pitch_weight * Jp(i) + - yaw_weight * Jy(i); - } - - return true; - } - - bool eval_hess_f( - Index n, - const Number* x, - bool new_x, - MatX& hess_f - ) final override - { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_hess_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - // fkPtr_->compute(0, model.nq, z, nullptr, nullptr, 2); - // Eigen::Array Hrpy; - // fkPtr_->getRPYHessian(Hrpy); - - // hess_f = roll_weight * Hrpy(0) + - // pitch_weight * Hrpy(1) + - // yaw_weight * Hrpy(2); - - fkPtr_->compute(0, model.nq, z, nullptr, nullptr, 3); - Eigen::Array Trpy; - fkPtr_->getRPYThirdOrderTensor(VecX::Ones(n), Trpy); - - hess_f = roll_weight * Trpy(0) + - pitch_weight * Trpy(1) + - yaw_weight * Trpy(2); - - return true; - } - - FKGradientChecker( - const FKGradientChecker& - ); - - FKGradientChecker& operator=( - const FKGradientChecker& - ); - - Model model; - Eigen::VectorXi jtype; - - std::unique_ptr fkPtr_; - - const double roll_weight = 1.0; - const double pitch_weight = 1.0; - const double yaw_weight = 1.0; -}; - -int main() { - // Define robot model - const std::string urdf_filename = "../Examples/Kinova/ArmourUnsafe/kinova.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 3, 3, 3, 3, 3, 3, 3; - - Eigen::VectorXd z0 = Eigen::VectorXd::Random(model.nq); - - // Initialize gradient checker - SmartPtr mynlp = new FKGradientChecker(); - try { - mynlp->set_parameters(z0, - model, - jtype); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("max_wall_time", 1e-5); - app->Options()->SetIntegerValue("print_level", 5); - app->Options()->SetStringValue("hessian_approximation", "exact"); - - // For gradient checking - app->Options()->SetStringValue("output_file", "ipopt.out"); - app->Options()->SetStringValue("derivative_test", "second-order"); - app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - app->Options()->SetNumericValue("derivative_test_tol", 1e-5); - - // Initialize the IpoptApplication and process the options - ApplicationReturnStatus status; - status = app->Initialize(); - if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); - } - - try { - status = app->OptimizeTNLP(mynlp); - } - catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - return 0; -} \ No newline at end of file diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp deleted file mode 100644 index 9b07b530..00000000 --- a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "ForwardKinematics.h" -#include - -using namespace RAPTOR; - -int main() { - // Define robot model - const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - pinocchio::Data data(model); - - // manually define the joint axis of rotation - // 1 for Rx, 2 for Ry, 3 for Rz - // 4 for Px, 5 for Py, 6 for Pz - // not sure how to extract this from a pinocchio model so define outside here. - Eigen::VectorXi jtype(model.nq); - jtype << 4, 5, 6, 1, 2, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3, - 3, 3, -3, 3, 2, 3, 3, 3, 3, 2, 3, 3, 2, 3, 3; - - ForwardKinematicsSolver fkSolver(&model, jtype); - - // set joint angles - std::srand(std::time(nullptr)); - Eigen::VectorXd q = 2 * M_PI * Eigen::VectorXd::Random(model.nq).array() - M_PI; - - // compute forward kinematics using pinocchio - auto start_clock = std::chrono::high_resolution_clock::now(); - pinocchio::forwardKinematics(model, data, q); - auto stop_clock = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "Pinocchio FK: " << duration.count() << " nanoseconds" << std::endl; - - // set the start and end joint - int start = 0; - int end = model.getJointId("left_toe_B"); - - // compute forward kinematics using RAPTOR - start_clock = std::chrono::high_resolution_clock::now(); - fkSolver.compute(start, end, q); - stop_clock = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "RAPTOR FK: " << duration.count() << " nanoseconds" << std::endl; - - // compare the results - std::cout << "Pinocchio: " << data.oMi[model.getJointId("left_toe_B")].translation().transpose() << std::endl; - std::cout << "RAPTOR: " << fkSolver.getTranslation().transpose() << std::endl; - - return 0; -} \ No newline at end of file diff --git a/Trajectories/README.md b/Trajectories/README.md deleted file mode 100644 index 7237a88f..00000000 --- a/Trajectories/README.md +++ /dev/null @@ -1,92 +0,0 @@ -# Trajectories - -## Overview - -### Trajectories class - -This folder contains a variety of trajectory classes that take in the decision variable `z` of the optimization problem and returns the trajectories of each joint `j` of the robot. -By trajectories, we mean the position, velocity, and the acceleration of each joint `j` over a series of time instances. -This series of time instances is predefined and fixed during the optimization process. -The most common example would be a uniform distribution over a fixed time interval `[0,T]`, where `T` is the duration of the trajectory. - -Right now, this duration and the distribution of time instances are fixed. -There are papers that incorporate the duration as one of the optimization decision variable, in the favor of solving problems, for example, reaching some target with the minimum amount of time while satisfying the torque limits or velocity limits. -Adaptive changes over the distribution of time instances are also widely discussed in the context of direct collocation methods or spectral methods. -But these are not the focus of our method for now. - -Specifically, the core function of all trajectory classes is `compute(const Eigen::VectorXd& z, bool compute_derivatives)`. -This function takes in the decision variable `z` of the optimization problem and updates the class member `q`, `q_d`, `q_dd`. - - - `q` is an Eigen array of Eigen vectors. - - `q(i)` stores all joint positions at time instance `i`. - - `q(i)(j)` stores the position of joint `j` at time instance `i`. - - If `compute_derivatives` is true, then the function will also computes the gradient of `q`, `q_d`, `q_dd` with respect to the decision variable `z`, which are stored in `pq_pz`, `pq_d_pz`, and `pq_dd_pz`. - - - `pq_pz` is an Eigen array of Eigen matrices. - - `pq_pz(i)` is a Eigen matrix of the jacobians of all joint positions with respect to the decision variable `z` at time instance `i`. - -If `compute_hessian` is true, then the function will also computes the hessian of `q`, `q_d`, `q_dd` with respect to the decision variable `z`, which are stored in `pq_pz_pz`, `pq_d_pz_pz`, and `pq_dd_pz_pz`. - - `pq_pz_pz` is a 2D Eigen array of Eigen matrices. - - `pq_pz_pz(j, i)` is a square Eigen matrix of the hessian of the position with respect to the decision variable `z` for joint `j` at time instance `i`. - -These gradients or hessians are later taken in by other classes to eventually compute the gradients of the constraints or costs in the optimization. - -### TrajectoryGroup class - -`TrajectoryGroup` is a special type of the class that contains multiple trajectories. -For example, for multiple-step humanoid gait optimization, you can assign multiple trajectories in the group for each walking step of the humanoid robot and optimize all of these steps simultaneously. -For manipulation tasks, like the robotic arm picks one object and places it at another place, you can also assign one trajectory for pick motion and another trajectory for place motion, so that they can be optimized at the same time. - -## Introduction to Each Trajectories - -### Plain class - -This class implements a very naive "trajectory", which is just the joint positions at one time instance. -The velocity and the acceleration are by default zero since there's no actual movement. - -### Polynomials class - -This class implements a polynomial representation of the trajectory, where the coefficients of the polynomial are decision variables. - -### BezierCurves class - -This class implements a [Bezier curve](https://en.wikipedia.org/wiki/B%C3%A9zier_curve) representation of the trajectory, where the Bezier coefficients are decision variables. -Note that the Bezier curve is only defined on interval [0,1] originally. -We scale the curve to the interval [0,T] where T is the duration of the trajectory and a constant positive number. - -### PiecewiseBezierCurves class - -This class implements a series of Bezier curves so that they are piecewise continuous on its second-order derivative. - -Note that the Bezier curve has the following property: - - - The value at the beginning is equal to the first Bezier coefficient. - - The first-order derivative at the beginning only depends on the first, and the second Bezier coefficient. - - The second-order derivative at the beginning only depends on the first, the second, and the thrid Bezier coefficient. - - The same property hold for the value, the first-order derivative, and the second-order derivative at the end, which only depends on the last first, the last second, and the last third Bezier coefficient. - -As a result, here we define a 5th-order Bezier curve which has 6 Bezier coefficients. -We consider the position, the velocity, and the acceleration at the beginning and at the end, which are also 6 numbers that uniquely determine the 6 Bezier coefficients. -We simplify the notation here and define the position, the velocity, and the acceleration at the beginning as `y_{i}` and at the end as `y_{i+1}`. -`y_{i}` and `y_{i+1}` are considered as decision variables. -In this class, we will transform `y_{i}` and `y_{i+1}` to corresponding Bezier coefficients so that we compute the trajectory at arbitrary time instances. - -Now let's consider a series of Bezier curves. -Let's say the `i` th Bezier curve has `y_{i}` as the position, velocity, acceleration at the beginning and `y_{i+1}` as the position, velocity, acceleration at the end. -The `i+1` th Bezier curve has `y_{i+1}` as the position, velocity, acceleration at the beginning and `y_{i+2}` as the position, velocity, acceleration at the end. -The `i` th Bezier curve and the `i+1` th Bezier curve thus are 2nd-order continuously differentiable (continuous on its 2nd order derivative) at their connection point, since they share the same positon, velocity, and acceleration here. - -As a result, we consider a series of `y_{i}` with `i=1,...N`. -We then can define `N-1` Bezier curves that are connected with each other in series. - -In this class, you can also provide initial position so that the very beginning of the entire series of Bezier curves starts from this initial position with 0 velocity and 0 acceleration (since the robot usually starts from a static position). -Note that this initial position is not part of the decision variables and is treated as constant. -The same things hold for end position, which is another optional input so that the robot stops at this position 0 velocity and 0 acceleration. -The motivation here is to directly constrain the start and the end of the entire trajectory using the property of Bezier curves, so that we have less decision variables and less constraints. - -### ArmourBezierCurves class - -This class is inherited from BezierCurves and describes a 5th-order Bezier curve that starts from specific position, velocity, acceleration, and ends at a desired position (which is the only decision variable here) with 0 velocity and 0 acceleration. -That means there's only 1 decision variable for each joint of the robot. -It is used in our previous work [ARMOUR](https://roahmlab.github.io/armour/). \ No newline at end of file diff --git a/Trajectories/include/ArmourBezierCurves.h b/Trajectories/include/ArmourBezierCurves.h deleted file mode 100644 index 1ac2ae8a..00000000 --- a/Trajectories/include/ArmourBezierCurves.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef ARMOUR_BEZIER_CURVES_H -#define ARMOUR_BEZIER_CURVES_H - -#include "BezierCurves.h" - -namespace RAPTOR { - -/* -This class implements a special type of Bezier curves used in [armour](https://arxiv.org/abs/2301.13308) -The initial position, velocity and acceleration are fixed and given by the user. -The final velocity and acceleration are fixed to be 0 (static at the end). -The final position is the trajectory parameter to optimize. -*/ - -const int ARMOUR_BEZIER_CURVE_DEGREE = 5; - -typedef struct ArmourTrajectoryParameters_ { - Eigen::VectorXd q0; - Eigen::VectorXd q_d0; - Eigen::VectorXd q_dd0; -} ArmourTrajectoryParameters; - -class ArmourBezierCurves : public BezierCurves { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - ArmourBezierCurves() = default; - - ArmourBezierCurves(const VecX& tspan_input, - int Nact_input, - const ArmourTrajectoryParameters& atp_input); - - ArmourBezierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - const ArmourTrajectoryParameters& atp_input); - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - ArmourTrajectoryParameters atp; - - MatX coefficients; -}; - -}; // namespace RAPTOR - -#endif // ARMOUR_BEZIER_CURVES_H diff --git a/Trajectories/include/BezierCurves.h b/Trajectories/include/BezierCurves.h deleted file mode 100644 index c07c49b5..00000000 --- a/Trajectories/include/BezierCurves.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef BEZIER_CURVES_H -#define BEZIER_CURVES_H - -#include "Trajectories.h" - -namespace RAPTOR { - -class BezierCurves : public Trajectories { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - BezierCurves() = default; - - BezierCurves(const VecX& tspan_input, - int Nact_input, - int degree_input); - - BezierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input); - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - int degree = 0; // degree of the Bezier curve - - VecX B; - VecX dB; - VecX ddB; - - VecX Bionomials; -}; - -}; // namespace RAPTOR - -#endif // BEZIER_CURVES_H diff --git a/Trajectories/include/FixedFrequencyFourierCurves.h b/Trajectories/include/FixedFrequencyFourierCurves.h deleted file mode 100644 index ba16a996..00000000 --- a/Trajectories/include/FixedFrequencyFourierCurves.h +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef FIXED_FOURIERCURVES_H -#define FIXED_FOURIERCURVES_H - -#include "Trajectories.h" - -namespace RAPTOR { - -class FixedFrequencyFourierCurves : public Trajectories { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - FixedFrequencyFourierCurves() = default; - - FixedFrequencyFourierCurves(const VecX& tspan_input, - int Nact_input, - int degree_input, - double base_frequency_input = 10, - VecX q0_input = VecX::Zero(0), - VecX q_d0_input = VecX::Zero(0)); - - FixedFrequencyFourierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input, - double base_frequency_input = 10, - VecX q0_input = VecX::Zero(0), - VecX q_d0_input = VecX::Zero(0)); - - // class methods: - // compute trajectories - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - // class variables: - double w = 10; // base frequency - - int degree = 0; // degree of the Fourier series - - VecX F; - VecX dF; - VecX ddF; - - VecX F0; - VecX dF0; - - VecX q0; - VecX q_d0; - bool optimize_initial_position = true; - bool optimize_initial_velocity = true; -}; - -}; // namespace RAPTOR - -#endif // FIXED_FOURIERCURVES_H diff --git a/Trajectories/include/FourierCurves.h b/Trajectories/include/FourierCurves.h deleted file mode 100644 index 04ac6c58..00000000 --- a/Trajectories/include/FourierCurves.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef FOURIERCURVES_H -#define FOURIERCURVES_H - -#include "Trajectories.h" - -namespace RAPTOR { - -class FourierCurves : public Trajectories { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - FourierCurves() = default; - - FourierCurves(const VecX& tspan_input, - int Nact_input, - int degree_input, - VecX q0_input = VecX::Zero(0), - VecX q_d0_input = VecX::Zero(0)); - - FourierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input, - VecX q0_input = VecX::Zero(0), - VecX q_d0_input = VecX::Zero(0)); - - void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false) override; - - int degree = 0; // degree of the Fourier series - - VecX F; - VecX dF; - VecX ddF; - - VecX F0; - VecX dF0; - - VecX pF_pw; - VecX pdF_pw; - VecX pddF_pw; - - VecX pF0_pw; - VecX pdF0_pw; - - VecX q0; - VecX q_d0; - bool optimize_initial_position = true; - bool optimize_initial_velocity = true; -}; - -}; // namespace RAPTOR - -#endif // FOURIERCURVES_H diff --git a/Trajectories/include/PiecewiseBezierCurves.h b/Trajectories/include/PiecewiseBezierCurves.h deleted file mode 100644 index 0f8783da..00000000 --- a/Trajectories/include/PiecewiseBezierCurves.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef PIECEWISE_BEZIER_CURVES_H -#define PIECEWISE_BEZIER_CURVES_H - -#include "BezierCurves.h" - -namespace RAPTOR { - -class PiecewiseBezierCurves : public BezierCurves { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - PiecewiseBezierCurves() = default; - - PiecewiseBezierCurves(double T_input, - int N_input, - int Nact_input, - int degree_input, - const VecX q0_input = VecX::Zero(0), - const VecX qT_input = VecX::Zero(0)); - - // Destructor - ~PiecewiseBezierCurves() = default; - - // class methods: - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - // class members: - MatX coefficients; - - // the initial position of the entire trajectory - VecX q0; - - // if q0 is not set, - // then the initial position is part of the decision variables in the optimization. - // it will be stored at the end of the decision variable vector z. - bool optimize_begin_position = false; - - // this indicates the offset of the initial position in the decision variable vector z. - size_t begin_position_offset = 0; - - // the end position of the entire trajectory - VecX qT; - - // if qT in TransferTrajectoryParameters is not set, - // then the end position is part of the decision variables in the optimization. - // it will be stored at the end of the decision variable vector z. - bool optimize_end_position = false; - - // this indicates the offset of the end position in the decision variable vector z. - size_t end_position_offset = 0; -}; - -}; // namespace RAPTOR - -#endif // BEZIER_CURVES_H diff --git a/Trajectories/include/Plain.h b/Trajectories/include/Plain.h deleted file mode 100644 index 53428377..00000000 --- a/Trajectories/include/Plain.h +++ /dev/null @@ -1,32 +0,0 @@ - -#ifndef PLAIN_H -#define PLAIN_H - -#include "Trajectories.h" - -namespace RAPTOR { - -class Plain : public Trajectories { -public: - // Constructor - Plain() = default; - - // Constructor - Plain(const int Nact_input); - - // Constructor - Plain(const int N_input, const int Nact_input); - - // Destructor - ~Plain() = default; - - // class methods: - // compute trajectories - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); -}; - -}; // namespace RAPTOR - -#endif // PLAIN_H diff --git a/Trajectories/include/Polynomials.h b/Trajectories/include/Polynomials.h deleted file mode 100644 index 8c4daecc..00000000 --- a/Trajectories/include/Polynomials.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef POLYNOMIALS_H -#define POLYNOMIALS_H - -#include "Trajectories.h" - -namespace RAPTOR { - -class Polynomials : public Trajectories { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - Polynomials() = default; - - Polynomials(const VecX& tspan_input, - int Nact_input, - int degree_input); - - Polynomials(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input); - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - int degree = 0; // degree of the polynomial - - VecX P; - VecX dP; - VecX ddP; -}; - -}; // namespace RAPTOR - -#endif // POLYNOMIALS_H diff --git a/Trajectories/include/Trajectories.h b/Trajectories/include/Trajectories.h deleted file mode 100644 index f05f2a8f..00000000 --- a/Trajectories/include/Trajectories.h +++ /dev/null @@ -1,92 +0,0 @@ -#ifndef TRAJECTORIES_H -#define TRAJECTORIES_H - -#include -#include -#include -#include -#include -#include -#include -#include "Utils.h" - -namespace RAPTOR { - -enum TimeDiscretization { - Uniform = 0, - Chebyshev -}; - -class Trajectories { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - Trajectories() = default; - - // Constructor - Trajectories(const int varLength_input, - const VecX& tspan_input, - int Nact_input); - - // Constructor - Trajectories(const int varLength_input, - double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization); - - // Destructor - ~Trajectories() = default; - - // class methods: - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - bool is_computed(const VecX& z, - bool compute_derivatives, - bool compute_hessian); - - // these methods are defined in TrajectoryGroup - virtual void add_trajectory(const std::string& name, - std::shared_ptr trajectory){ - throw std::runtime_error("add_trajectory is not implemented in Trajectories class"); - } - virtual void gather_trajectories_information(const bool print_info = false) { - throw std::runtime_error("gather_trajectories_information is not implemented in Trajectories class"); - } - - // class members: - double T = 0; // total time of the trajectory - int N = 0; // number of time instances in tspan - int Nact = 0; // number of actuated joints in the system - VecX tspan; // a column vector of discrete time instances to check constraint violation - int varLength = 0; // length of the decision variable vector - - // compute results are stored here - Eigen::Array q; - Eigen::Array q_d; - Eigen::Array q_dd; - - // compute results are stored here - Eigen::Array pq_pz; - Eigen::Array pq_d_pz; - Eigen::Array pq_dd_pz; - - // compute results are stored here - Eigen::Array pq_pz_pz; - Eigen::Array pq_d_pz_pz; - Eigen::Array pq_dd_pz_pz; - - // trajectory class is frequently used in the optimization problem - // so we store the computed results here to avoid recomputation - VecX current_z; - bool if_compute_derivatives = false; - bool if_compute_hessian = false; -}; - -}; // namespace RAPTOR - -#endif // TRAJECTORIES_H diff --git a/Trajectories/include/TrajectoryGroup.h b/Trajectories/include/TrajectoryGroup.h deleted file mode 100644 index e9957f26..00000000 --- a/Trajectories/include/TrajectoryGroup.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef TRAJECTORY_GROUP_H -#define TRAJECTORY_GROUP_H - -#include -#include -#include "Trajectories.h" - -namespace RAPTOR { - -class TrajectoryGroup : public Trajectories { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - // Constructor - TrajectoryGroup() = default; - - // Destructor - ~TrajectoryGroup() = default; - - // class methods: - virtual void add_trajectory(const std::string& name, - std::shared_ptr trajectory) final override; - - // update original information in Trajectories class - virtual void gather_trajectories_information(const bool print_info = false) final override; - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - // class members: - std::unordered_map> trajectories; - - // we will assemble the trajectories in the order they are added in q, q_d, q_dd. - // this records the start and the end of each trajectory - // in the overall time instances of the assembled trajectory. - // range is start <= ... < end - std::unordered_map> trajectory_locations; - - // we will assemble the trajectories in the order they are added in q, q_d, q_dd. - // this records the start and the end of the decision variable of each trajectory - // in the overall decision variable vector z. - // range is start <= ... < end - std::unordered_map> variable_locations; -}; - -}; // namespace RAPTOR - -#endif // TRAJECTORIES_H diff --git a/Trajectories/include/TransferBezierCurves.h b/Trajectories/include/TransferBezierCurves.h deleted file mode 100644 index b3122123..00000000 --- a/Trajectories/include/TransferBezierCurves.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef TRANSFER_BEZIER_CURVES_H -#define TRANSFER_BEZIER_CURVES_H - -#include "BezierCurves.h" - -namespace RAPTOR { - -/* -This class implements a special type of Bezier curves used in [armour](https://arxiv.org/abs/2301.13308) -But we add one more dimension to model the fixed joint between the end effector and the object being manipulated. -The position, velocity and acceleration of the fixed joint are fixed to 0 all the time. -The reason we do that is pinocchio automatically remove the fixed joints and combine two bodies. -We have to add this extra joint to access the derivatives of the force applied on the fixed joint using pinocchio's functions. - -Additionally, the initial position and the end position are pre-specified and fixed. -The initial velocity, acceleration and the end velocity, acceleration are fixed to 0. -This fixes 6 degrees of freedom in the trajectory, which are the first 3 and the last 3 Bezier coefficients of the curve. -The rest of the degree of freedom / Bezier coefficients are free to optimize. -*/ - -typedef struct TransferTrajectoryParameters_ { - Eigen::VectorXd q0; - Eigen::VectorXd qT; -} TransferTrajectoryParameters; - -class TransferBezierCurves : public BezierCurves { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - TransferBezierCurves() = default; - - TransferBezierCurves(double T_input, - int N_input, - int Nact_input, - const int degree_input, - TimeDiscretization time_discretization, - const TransferTrajectoryParameters& ttp_input); - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - TransferTrajectoryParameters ttp; - - MatX coefficients; -}; - -}; // namespace RAPTOR - -#endif // TRANSFER_BEZIER_CURVES_H diff --git a/Trajectories/include/WaitrBezierCurves.h b/Trajectories/include/WaitrBezierCurves.h deleted file mode 100644 index daa848fb..00000000 --- a/Trajectories/include/WaitrBezierCurves.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef WAITR_BEZIER_CURVES_H -#define WAITR_BEZIER_CURVES_H - -#include "BezierCurves.h" - -namespace RAPTOR { - -/* -This class implements a special type of Bezier curves used in [armour](https://arxiv.org/abs/2301.13308) -But we add one more dimension to model the fixed joint between the end effector and the object being manipulated. -The position, velocity and acceleration of the fixed joint are fixed to 0 all the time. -The reason we do that is pinocchio automatically remove the fixed joints and combine two bodies. -We have to add this extra joint to access the derivatives of the force applied on the fixed joint using pinocchio's functions. -*/ - -const int WAITR_BEZIER_CURVE_DEGREE = 5; - -typedef struct WaitrTrajectoryParameters_ { - Eigen::VectorXd q0; - Eigen::VectorXd q_d0; - Eigen::VectorXd q_dd0; -} WaitrTrajectoryParameters; - -class WaitrBezierCurves : public BezierCurves { -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - WaitrBezierCurves() = default; - - WaitrBezierCurves(const VecX& tspan_input, - int Nact_input, - const WaitrTrajectoryParameters& atp_input); - - WaitrBezierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - const WaitrTrajectoryParameters& atp_input); - - virtual void compute(const VecX& z, - bool compute_derivatives = true, - bool compute_hessian = false); - - WaitrTrajectoryParameters atp; - - MatX coefficients; -}; - -}; // namespace RAPTOR - -#endif // WAITR_BEZIER_CURVES_H diff --git a/Trajectories/src/ArmourBezierCurves.cpp b/Trajectories/src/ArmourBezierCurves.cpp deleted file mode 100644 index 1bbac82e..00000000 --- a/Trajectories/src/ArmourBezierCurves.cpp +++ /dev/null @@ -1,164 +0,0 @@ -#include "ArmourBezierCurves.h" - -namespace RAPTOR { - -ArmourBezierCurves::ArmourBezierCurves(const VecX& tspan_input, - int Nact_input, - const ArmourTrajectoryParameters& atp_input) : - BezierCurves(tspan_input, Nact_input, ARMOUR_BEZIER_CURVE_DEGREE), - atp(atp_input) { - if (atp.q0.size() != Nact) { - throw std::invalid_argument("ArmourBezierCurves: q0.size() != Nact"); - } - if (atp.q_d0.size() != Nact) { - throw std::invalid_argument("ArmourBezierCurves: q_d0.size() != Nact"); - } - if (atp.q_dd0.size() != Nact) { - throw std::invalid_argument("ArmourBezierCurves: q_dd0.size() != Nact"); - } - - varLength = Nact; - - coefficients.resize(degree + 1, Nact); - coefficients.row(0) = atp.q0.transpose(); - coefficients.row(1) = coefficients.row(0) + - (T * atp.q_d0.transpose()) / 5.0; - coefficients.row(2) = coefficients.row(0) + - (2.0 * T * atp.q_d0.transpose()) / 5.0 + - (T * T * atp.q_dd0.transpose()) / 20.0; - - // resize the memory since varLength is changed - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -ArmourBezierCurves::ArmourBezierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - const ArmourTrajectoryParameters& atp_input) : - BezierCurves(T_input, N_input, Nact_input, time_discretization, ARMOUR_BEZIER_CURVE_DEGREE), - atp(atp_input) { - if (atp.q0.size() != Nact) { - throw std::invalid_argument("ArmourBezierCurves: q0.size() != Nact"); - } - if (atp.q_d0.size() != Nact) { - throw std::invalid_argument("ArmourBezierCurves: q_d0.size() != Nact"); - } - if (atp.q_dd0.size() != Nact) { - throw std::invalid_argument("ArmourBezierCurves: q_dd0.size() != Nact"); - } - - varLength = Nact; - - coefficients.resize(degree + 1, Nact); - coefficients.row(0) = atp.q0.transpose(); - coefficients.row(1) = atp.q0.transpose() + - (T * atp.q_d0.transpose()) / 5.0; - coefficients.row(2) = atp.q0.transpose() + - (2.0 * T * atp.q_d0.transpose()) / 5.0 + - (T * T * atp.q_dd0.transpose()) / 20.0; - - // resize the memory since varLength is changed - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -void ArmourBezierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() != varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("ArmourBezierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - coefficients.row(5) = z.transpose(); - coefficients.row(4) = z.transpose(); - coefficients.row(3) = z.transpose(); - - for (int x = 0; x < N; x++) { - double t = tspan(x) / T; - - q(x) = VecX::Zero(Nact); - q_d(x) = VecX::Zero(Nact); - q_dd(x) = VecX::Zero(Nact); - - if (compute_derivatives) { - pq_pz(x) = MatX::Zero(Nact, varLength); - pq_d_pz(x) = MatX::Zero(Nact, varLength); - pq_dd_pz(x) = MatX::Zero(Nact, varLength); - } - - // Compute tA(i, j) = t(i)^j, - // tB(i, j) = (1-t(i))^(degree-j) - VecX tA = VecX::Ones(degree + 1); - VecX tB = VecX::Ones(degree + 1); - VecX dtA = VecX::Zero(degree + 1); - VecX dtB = VecX::Zero(degree + 1); - VecX ddtA = VecX::Zero(degree + 1); - VecX ddtB = VecX::Zero(degree + 1); - - // Loop to compute tA and tB - for (int j = 1; j <= degree; j++) { - tA(j) = t * tA(j - 1); - tB(degree - j) = (1 - t) * tB(degree - j + 1); - - dtA(j) = j * tA(j - 1); - dtB(degree - j) = -j * tB(degree - j + 1); - - ddtA(j) = j * dtA(j - 1); - ddtB(degree - j) = -j * dtB(degree - j + 1); - } - - B = Bionomials.array() * tA.array() * tB.array(); - dB = Bionomials.array() * (dtA.array() * tB.array() + - tA.array() * dtB.array()) / T; - ddB = Bionomials.array() * (ddtA.array() * tB.array() + - 2 * dtA.array() * dtB.array() + - tA.array() * ddtB.array()) / (T * T); - - q(x) = coefficients.transpose() * B; - q_d(x) = coefficients.transpose() * dB; - q_dd(x) = coefficients.transpose() * ddB; - - if (compute_derivatives) { - for (int i = 0; i < Nact; i++) { - pq_pz(x)(i, i) = B(3) + B(4) + B(5); - pq_d_pz(x)(i, i) = dB(3) + dB(4) + dB(5); - pq_dd_pz(x)(i, i) = ddB(3) + ddB(4) + ddB(5); - } - } - - if (compute_hessian) { - // This has been set to 0 in the constructor - // for (int i = 0; i < Nact; i++) { - // pq_pz_pz(i, x).setZero(); - // pq_d_pz_pz(i, x).setZero(); - // pq_dd_pz_pz(i, x).setZero(); - // } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Trajectories/src/BezierCurves.cpp b/Trajectories/src/BezierCurves.cpp deleted file mode 100644 index e0e3ec0d..00000000 --- a/Trajectories/src/BezierCurves.cpp +++ /dev/null @@ -1,117 +0,0 @@ -#include "BezierCurves.h" - -namespace RAPTOR { - -BezierCurves::BezierCurves(const VecX& tspan_input, - int Nact_input, - int degree_input) : - Trajectories((degree_input + 1) * Nact_input, tspan_input, Nact_input), - degree(degree_input) { - B = VecX::Zero(degree + 1); - dB = VecX::Zero(degree + 1); - ddB = VecX::Zero(degree + 1); - - Bionomials = VecX::Ones(degree + 1); - for (int j = 1; j <= degree / 2; j++) { - Bionomials(j) = Bionomials(j - 1) * (degree + 1 - j) / j; - Bionomials(degree - j) = Bionomials(j); - } -} - -BezierCurves::BezierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input) : - Trajectories((degree_input + 1) * Nact_input, T_input, N_input, Nact_input, time_discretization), - degree(degree_input) { - B = VecX::Zero(degree + 1); - dB = VecX::Zero(degree + 1); - ddB = VecX::Zero(degree + 1); - - Bionomials = VecX::Ones(degree + 1); - for (int j = 1; j <= degree / 2; j++) { - Bionomials(j) = Bionomials(j - 1) * (degree + 1 - j) / j; - Bionomials(degree - j) = Bionomials(j); - } -} - -void BezierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() < varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("BezierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - // MatX coefficients = z.head((degree + 1) * Nact).reshaped(degree + 1, Nact); - MatX coefficients = Utils::reshape(z.head((degree + 1) * Nact), degree + 1, Nact); - - for (int x = 0; x < N; x++) { - double t = tspan(x) / T; - - q(x) = VecX::Zero(Nact); - q_d(x) = VecX::Zero(Nact); - q_dd(x) = VecX::Zero(Nact); - - if (compute_derivatives) { - pq_pz(x) = MatX::Zero(Nact, varLength); - pq_d_pz(x) = MatX::Zero(Nact, varLength); - pq_dd_pz(x) = MatX::Zero(Nact, varLength); - } - - // Compute tA(i, j) = t(i)^j, - // tB(i, j) = (1-t(i))^(degree-j) - VecX tA = VecX::Ones(degree + 1); - VecX tB = VecX::Ones(degree + 1); - VecX dtA = VecX::Zero(degree + 1); - VecX dtB = VecX::Zero(degree + 1); - VecX ddtA = VecX::Zero(degree + 1); - VecX ddtB = VecX::Zero(degree + 1); - - // Loop to compute tA and tB - for (int j = 1; j <= degree; j++) { - tA(j) = t * tA(j - 1); - tB(degree - j) = (1 - t) * tB(degree - j + 1); - - dtA(j) = j * tA(j - 1); - dtB(degree - j) = -j * tB(degree - j + 1); - - ddtA(j) = j * dtA(j - 1); - ddtB(degree - j) = -j * dtB(degree - j + 1); - } - - B = Bionomials.array() * tA.array() * tB.array(); - dB = Bionomials.array() * (dtA.array() * tB.array() + - tA.array() * dtB.array()) / T; - ddB = Bionomials.array() * (ddtA.array() * tB.array() + - 2 * dtA.array() * dtB.array() + - tA.array() * ddtB.array()) / (T * T); - - q(x) = coefficients.transpose() * B; - q_d(x) = coefficients.transpose() * dB; - q_dd(x) = coefficients.transpose() * ddB; - - if (compute_derivatives) { - for (int i = 0; i < Nact; i++) { - pq_pz(x).block(i, i * (degree + 1), 1, degree + 1) = B.transpose(); - pq_d_pz(x).block(i, i * (degree + 1), 1, degree + 1) = dB.transpose(); - pq_dd_pz(x).block(i, i * (degree + 1), 1, degree + 1) = ddB.transpose(); - } - } - - if (compute_hessian) { - for (int i = 0; i < Nact; i++) { - pq_pz_pz(i, x).setZero(); - pq_d_pz_pz(i, x).setZero(); - pq_dd_pz_pz(i, x).setZero(); - } - } - } -} - -}; // namespace RAPTOR - diff --git a/Trajectories/src/FixedFrequencyFourierCurves.cpp b/Trajectories/src/FixedFrequencyFourierCurves.cpp deleted file mode 100644 index ad31e05a..00000000 --- a/Trajectories/src/FixedFrequencyFourierCurves.cpp +++ /dev/null @@ -1,242 +0,0 @@ -#include "FixedFrequencyFourierCurves.h" - -namespace RAPTOR { - -FixedFrequencyFourierCurves::FixedFrequencyFourierCurves(const VecX& tspan_input, - int Nact_input, - int degree_input, - double base_frequency_input, - VecX q0_input, - VecX q_d0_input) : - Trajectories((2 * degree_input + 1) * Nact_input, tspan_input, Nact_input), - degree(degree_input), - w(base_frequency_input), - q0(q0_input), - q_d0(q_d0_input) { - F = VecX::Zero(2 * degree + 1); - dF = VecX::Zero(2 * degree + 1); - ddF = VecX::Zero(2 * degree + 1); - - F0 = VecX::Zero(2 * degree + 1); - dF0 = VecX::Zero(2 * degree + 1); - - if (q0.size() == Nact) { - optimize_initial_position = false; - } - else { - optimize_initial_position = true; - varLength += Nact; - } - - if (q_d0.size() == Nact) { - optimize_initial_velocity = false; - } - else { - optimize_initial_velocity = true; - varLength += Nact; - } - - // varLength is changed so we have to reallocate the memory - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -FixedFrequencyFourierCurves::FixedFrequencyFourierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input, - double base_frequency_input, - VecX q0_input, - VecX q_d0_input) : - Trajectories((2 * degree_input + 1) * Nact_input, T_input, N_input, Nact_input, time_discretization), - degree(degree_input), - w(base_frequency_input), - q0(q0_input), - q_d0(q_d0_input) { - F = VecX::Zero(2 * degree + 1); - dF = VecX::Zero(2 * degree + 1); - ddF = VecX::Zero(2 * degree + 1); - - F0 = VecX::Zero(2 * degree + 1); - dF0 = VecX::Zero(2 * degree + 1); - - if (q0.size() == Nact) { - optimize_initial_position = false; - } - else { - optimize_initial_position = true; - varLength += Nact; - } - - if (q_d0.size() == Nact) { - optimize_initial_velocity = false; - } - else { - optimize_initial_velocity = true; - varLength += Nact; - } - - // varLength is changed so we have to reallocate the memory - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -void FixedFrequencyFourierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() < varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("FixedFrequencyFourierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - Eigen::MatrixXd temp = z.head((2 * degree + 1) * Nact); - MatX coefficients = Utils::reshape(temp, 2 * degree + 1, Nact); - - if (optimize_initial_position) { - q0 = z.block((2 * degree + 1) * Nact, 0, Nact, 1); - } - if (optimize_initial_velocity) { - if (optimize_initial_position) { - q_d0 = z.block((2 * degree + 1) * Nact + Nact, 0, Nact, 1); - } - else { - q_d0 = z.block((2 * degree + 1) * Nact, 0, Nact, 1); - } - } - - for (int x = 0; x < N; x++) { - double t = tspan(x); - - q(x) = VecX::Zero(Nact); - q_d(x) = VecX::Zero(Nact); - q_dd(x) = VecX::Zero(Nact); - - if (compute_derivatives) { - pq_pz(x).resize(Nact, varLength); - pq_d_pz(x).resize(Nact, varLength); - pq_dd_pz(x).resize(Nact, varLength); - pq_pz(x).setZero(); - pq_d_pz(x).setZero(); - pq_dd_pz(x).setZero(); - } - - for (int i = 0; i < Nact; i++) { - const VecX& kernel = coefficients.col(i); - - ddF(0) = 1; - dF(0) = t; - F(0) = t * t * 0.5; - - dF0(0) = 0; - F0(0) = 0; - - for (int j = 0; j < degree; j++) { - double jt = (j + 1) * t; - double sinjwt = sin(w * jt); - double cosjwt = cos(w * jt); - - ddF(2 * j + 1) = cosjwt; - ddF(2 * j + 2) = sinjwt; - - double jw = (j + 1) * w; - dF(2 * j + 1) = sinjwt / jw; - dF(2 * j + 2) = -cosjwt / jw; - dF0(2 * j + 2) = -1 / jw; - - double j2w2 = jw * jw; - F(2 * j + 1) = -cosjwt / j2w2; - F(2 * j + 2) = -sinjwt / j2w2; - F0(2 * j + 1) = -1 / j2w2; - } - - q_dd(x)(i) = ddF.dot(kernel); - - double q_d_raw = dF.dot(kernel); - double q_d_raw0 = dF0.dot(kernel); - q_d(x)(i) = q_d_raw + (q_d0(i) - q_d_raw0); - - double q_raw = F.dot(kernel) + (q_d0(i) - q_d_raw0) * t; - double q_raw0 = F0.dot(kernel); - q(x)(i) = q_raw + (q0(i) - q_raw0); - - if (compute_derivatives) { - // pq_pz - // derivative with respect to a_i - for (int j = 0; j < 2 * degree + 1; j++) { - pq_pz(x)(i, i * (2 * degree + 1) + j) = F(j) - F0(j) - dF0(j) * tspan(x); - } - - // derivative with respect to q0 - if (optimize_initial_position) { - pq_pz(x)(i, Nact * (2 * degree + 1) + i) = 1; - } - - // derivative with respect to q_d0 - if (optimize_initial_velocity) { - if (optimize_initial_position) { - pq_pz(x)(i, Nact * (2 * degree + 1) + Nact + i) = tspan(x); - } - else { - pq_pz(x)(i, Nact * (2 * degree + 1) + i) = tspan(x); - } - } - - // pq_d_pz - // derivative with respect to a_i - for (int j = 0; j < 2 * degree + 1; j++) { - pq_d_pz(x)(i, i * (2 * degree + 1) + j) = dF(j) - dF0(j); - } - - // derivative with respect to q_d0 - if (optimize_initial_velocity) { - if (optimize_initial_position) { - pq_d_pz(x)(i, Nact * (2 * degree + 1) + Nact + i) = 1; - } - else { - pq_d_pz(x)(i, Nact * (2 * degree + 1) + i) = 1; - } - } - - // pq_dd_pz - // derivative with respect to a_i - for (int j = 0; j < 2 * degree + 1; j++) { - pq_dd_pz(x)(i, i * (2 * degree + 1) + j) = ddF(j); - } - } - - if (compute_hessian) { - // have already initialized as zero in the constructor - // for (int i = 0; i < Nact; i++) { - // pq_pz_pz(i, x).setZero(); - // pq_d_pz_pz(i, x).setZero(); - // pq_dd_pz_pz(i, x).setZero(); - // } - } - } - } -} - -}; // namespace RAPTOR - diff --git a/Trajectories/src/FourierCurves.cpp b/Trajectories/src/FourierCurves.cpp deleted file mode 100644 index d4cd7145..00000000 --- a/Trajectories/src/FourierCurves.cpp +++ /dev/null @@ -1,281 +0,0 @@ -#include "FourierCurves.h" - -namespace RAPTOR { - -FourierCurves::FourierCurves(const VecX& tspan_input, - int Nact_input, - int degree_input, - VecX q0_input, - VecX q_d0_input) : - Trajectories((2 * degree_input + 2) * Nact_input, tspan_input, Nact_input), - degree(degree_input), - q0(q0_input), - q_d0(q_d0_input) { - F = VecX::Zero(2 * degree + 1); - dF = VecX::Zero(2 * degree + 1); - ddF = VecX::Zero(2 * degree + 1); - - F0 = VecX::Zero(2 * degree + 1); - dF0 = VecX::Zero(2 * degree + 1); - - pF_pw = VecX::Zero(2 * degree + 1); - pdF_pw = VecX::Zero(2 * degree + 1); - pddF_pw = VecX::Zero(2 * degree + 1); - - pF0_pw = VecX::Zero(2 * degree + 1); - pdF0_pw = VecX::Zero(2 * degree + 1); - - if (q0.size() == Nact) { - optimize_initial_position = false; - } - else { - optimize_initial_position = true; - varLength += Nact; - } - - if (q_d0.size() == Nact) { - optimize_initial_velocity = false; - } - else { - optimize_initial_velocity = true; - varLength += Nact; - } - - // varLength is changed so we have to reallocate the memory - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -FourierCurves::FourierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input, - VecX q0_input, - VecX q_d0_input) : - Trajectories((2 * degree_input + 4) * Nact_input, T_input, N_input, Nact_input, time_discretization), - degree(degree_input), - q0(q0_input), - q_d0(q_d0_input) { - F = VecX::Zero(2 * degree + 1); - dF = VecX::Zero(2 * degree + 1); - ddF = VecX::Zero(2 * degree + 1); - - F0 = VecX::Zero(2 * degree + 1); - dF0 = VecX::Zero(2 * degree + 1); - - pF_pw = VecX::Zero(2 * degree + 1); - pdF_pw = VecX::Zero(2 * degree + 1); - pddF_pw = VecX::Zero(2 * degree + 1); - - pF0_pw = VecX::Zero(2 * degree + 1); - pdF0_pw = VecX::Zero(2 * degree + 1); - - if (q0.size() == Nact) { - optimize_initial_position = false; - } - else { - optimize_initial_position = true; - varLength += Nact; - } - - if (q_d0.size() == Nact) { - optimize_initial_velocity = false; - } - else { - optimize_initial_velocity = true; - varLength += Nact; - } - - // varLength is changed so we have to reallocate the memory - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -void FourierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() < varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("FourierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - if (compute_hessian) { - throw std::invalid_argument("FourierCurves: Hessian computation not implemented"); - } - - Eigen::MatrixXd temp = z.head((2 * degree + 2) * Nact); - MatX coefficients = Utils::reshape(temp, 2 * degree + 2, Nact); - - if (optimize_initial_position) { - q0 = z.block((2 * degree + 2) * Nact, 0, Nact, 1); - } - if (optimize_initial_velocity) { - if (optimize_initial_position) { - q_d0 = z.block((2 * degree + 2) * Nact + Nact, 0, Nact, 1); - } - else { - q_d0 = z.block((2 * degree + 2) * Nact, 0, Nact, 1); - } - } - - for (int x = 0; x < N; x++) { - double t = tspan(x); - - q(x) = VecX::Zero(Nact); - q_d(x) = VecX::Zero(Nact); - q_dd(x) = VecX::Zero(Nact); - - if (compute_derivatives) { - pq_pz(x).resize(Nact, varLength); - pq_d_pz(x).resize(Nact, varLength); - pq_dd_pz(x).resize(Nact, varLength); - pq_pz(x).setZero(); - pq_d_pz(x).setZero(); - pq_dd_pz(x).setZero(); - } - - for (int i = 0; i < Nact; i++) { - const VecX& kernel = coefficients.block(0, i, 2 * degree + 1, 1); - double w = coefficients(2 * degree + 1, i); - - ddF(0) = 1; - dF(0) = t; - F(0) = t * t * 0.5; - - dF0(0) = 0; - F0(0) = 0; - - if (compute_derivatives) { - pF_pw(0) = 0; - pdF_pw(0) = 0; - pddF_pw(0) = 0; - - pF0_pw(0) = 0; - pdF0_pw(0) = 0; - } - - for (int j = 0; j < degree; j++) { - double jt = (j + 1) * t; - double sinjwt = sin(w * jt); - double cosjwt = cos(w * jt); - - ddF(2 * j + 1) = cosjwt; - ddF(2 * j + 2) = sinjwt; - - double jw = (j + 1) * w; - dF(2 * j + 1) = sinjwt / jw; - dF(2 * j + 2) = -cosjwt / jw; - dF0(2 * j + 2) = -1 / jw; - - double j2w2 = jw * jw; - F(2 * j + 1) = -cosjwt / j2w2; - F(2 * j + 2) = -sinjwt / j2w2; - F0(2 * j + 1) = -1 / j2w2; - - if (compute_derivatives) { - pddF_pw(2 * j + 1) = -jt * sinjwt; - pddF_pw(2 * j + 2) = jt * cosjwt; - - double jw2 = (j + 1) * w * w; - pdF_pw(2 * j + 1) = (t * cosjwt) / w - sinjwt / jw2; - pdF_pw(2 * j + 2) = cosjwt / jw2 + (t * sinjwt) / w; - pdF0_pw(2 * j + 2) = 1 / jw2; - - double j2w3 = jw2 * (j + 1) * w; - pF_pw(2 * j + 1) = (2 * cosjwt + (j + 1) * w * t * sinjwt) / j2w3; - pF_pw(2 * j + 2) = (2 * sinjwt - (j + 1) * w * t * cosjwt) / j2w3; - pF0_pw(2 * j + 1) = 2 / j2w3; - } - } - - q_dd(x)(i) = ddF.dot(kernel); - - double q_d_raw = dF.dot(kernel); - double q_d_raw0 = dF0.dot(kernel); - q_d(x)(i) = q_d_raw + (q_d0(i) - q_d_raw0); - - double q_raw = F.dot(kernel) + (q_d0(i) - q_d_raw0) * t; - double q_raw0 = F0.dot(kernel); - q(x)(i) = q_raw + (q0(i) - q_raw0); - - if (compute_derivatives) { - // pq_pz - // derivative with respect to a_i - for (int j = 0; j < 2 * degree + 1; j++) { - pq_pz(x)(i, i * (2 * degree + 2) + j) = F(j) - F0(j) - dF0(j) * tspan(x); - } - - // derivative with respect to w - pq_pz(x)(i, i * (2 * degree + 2) + (2 * degree + 1)) = kernel.dot(pF_pw - pF0_pw - pdF0_pw * tspan(x)); - - // derivative with respect to q0 - if (optimize_initial_position) { - pq_pz(x)(i, Nact * (2 * degree + 2) + i) = 1; - } - - // derivative with respect to q_d0 - if (optimize_initial_velocity) { - if (optimize_initial_position) { - pq_pz(x)(i, Nact * (2 * degree + 2) + Nact + i) = tspan(x); - } - else { - pq_pz(x)(i, Nact * (2 * degree + 2) + i) = tspan(x); - } - } - - // pq_d_pz - // derivative with respect to a_i - for (int j = 0; j < 2 * degree + 1; j++) { - pq_d_pz(x)(i, i * (2 * degree + 2) + j) = dF(j) - dF0(j); - } - - // derivative with respect to w - pq_d_pz(x)(i, i * (2 * degree + 2) + (2 * degree + 1)) = kernel.dot(pdF_pw - pdF0_pw); - - // derivative with respect to q_d0 - if (optimize_initial_velocity) { - if (optimize_initial_position) { - pq_d_pz(x)(i, Nact * (2 * degree + 2) + Nact + i) = 1; - } - else { - pq_d_pz(x)(i, Nact * (2 * degree + 2) + i) = 1; - } - } - - // pq_dd_pz - // derivative with respect to a_i - for (int j = 0; j < 2 * degree + 1; j++) { - pq_dd_pz(x)(i, i * (2 * degree + 2) + j) = ddF(j); - } - - // derivative with respect to w - pq_dd_pz(x)(i, i * (2 * degree + 2) + (2 * degree + 1)) = kernel.dot(pddF_pw); - } - } - } -} - -}; // namespace RAPTOR - diff --git a/Trajectories/src/PiecewiseBezierCurves.cpp b/Trajectories/src/PiecewiseBezierCurves.cpp deleted file mode 100644 index 3fc9735c..00000000 --- a/Trajectories/src/PiecewiseBezierCurves.cpp +++ /dev/null @@ -1,283 +0,0 @@ -#include "PiecewiseBezierCurves.h" - -namespace RAPTOR { - -PiecewiseBezierCurves::PiecewiseBezierCurves(double T_input, - int N_input, - int Nact_input, - int degree_input, - const VecX q0_input, - const VecX qT_input) { - T = T_input; - N = N_input; - Nact = Nact_input; - degree = degree_input; - q0 = q0_input; - qT = qT_input; - - if (N % (degree + 1) != 0) { - throw std::invalid_argument("PiecewiseBezierCurves: N must be a multiple of degree"); - } - - tspan = VecX::LinSpaced(N, 0, T); - - B = VecX::Zero(6); - dB = VecX::Zero(6); - ddB = VecX::Zero(6); - - Bionomials = VecX::Ones(6); - for (int j = 1; j <= 2; j++) { - Bionomials(j) = Bionomials(j - 1) * (6 - j) / j; - Bionomials(5 - j) = Bionomials(j); - } - - varLength = Nact * degree * 3; - - if (q0.size() != Nact) { - optimize_begin_position = true; - begin_position_offset = varLength; - varLength += Nact; - } - if (qT.size() != Nact) { - optimize_end_position = true; - end_position_offset = varLength; - varLength += Nact; - } - - coefficients.resize(6, Nact); - - q.resize(1, N); - q_d.resize(1, N); - q_dd.resize(1, N); - - pq_pz.resize(1, N); - pq_d_pz.resize(1, N); - pq_dd_pz.resize(1, N); - - pq_pz_pz.resize(Nact, N); - pq_d_pz_pz.resize(Nact, N); - pq_dd_pz_pz.resize(Nact, N); - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -void PiecewiseBezierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() != varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("PiecewiseBezierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - const int seg_size = N / (degree + 1); - const double localT = T / (degree + 1); - - for (int x = 0; x < N; x++) { - // determine which Bezier curve we are at right now - int id = x / seg_size; - - // determine the local time within the Bezier curve (scaled to [0, 1]) - double t = (tspan(x) - id * localT) / localT; - - // useful constants - double velocity_factor = localT / 5.0; - double acceleration_factor = localT * localT / 20.0; - - // determine the initial and end position, velocity and acceleration for each piece of Bezier curve - VecX begin_q, begin_q_d, begin_q_dd; - VecX end_q, end_q_d, end_q_dd; - - if (id == 0) { - const VecX& control_point = z.head(3 * Nact); - - if (optimize_begin_position) { - begin_q = z.segment(begin_position_offset, Nact).transpose(); - } - else { - begin_q = q0.transpose(); - } - - begin_q_d = VecX::Zero(Nact); - begin_q_dd = VecX::Zero(Nact); - - end_q = control_point.head(Nact).transpose(); - end_q_d = control_point.segment(Nact, Nact).transpose(); - end_q_dd = control_point.tail(Nact).transpose(); - } - else if (id == degree) { - const VecX& control_point = z.segment((id - 1) * 3 * Nact, 3 * Nact); - - begin_q = control_point.head(Nact).transpose(); - begin_q_d = control_point.segment(Nact, Nact).transpose(); - begin_q_dd = control_point.tail(Nact).transpose(); - - if (optimize_end_position) { - end_q = z.segment(end_position_offset, Nact).transpose(); - } - else { - end_q = qT.transpose(); - } - end_q_d = VecX::Zero(Nact); - end_q_dd = VecX::Zero(Nact); - } - else { - const VecX& control_point1 = z.segment((id - 1) * 3 * Nact, 3 * Nact); - const VecX& control_point2 = z.segment(id * 3 * Nact, 3 * Nact); - begin_q = control_point1.head(Nact).transpose(); - begin_q_d = control_point1.segment(Nact, Nact).transpose(); - begin_q_dd = control_point1.tail(Nact).transpose(); - end_q = control_point2.head(Nact).transpose(); - end_q_d = control_point2.segment(Nact, Nact).transpose(); - end_q_dd = control_point2.tail(Nact).transpose(); - } - - // compute the Bezier coefficients - // based on the initial and end position, velocity and acceleration - coefficients.row(0) = begin_q; - coefficients.row(1) = begin_q + - begin_q_d * velocity_factor; - coefficients.row(2) = begin_q + - begin_q_d * 2.0 * velocity_factor + - begin_q_dd * acceleration_factor; - coefficients.row(3) = end_q - - end_q_d * 2.0 * velocity_factor + - end_q_dd * acceleration_factor; - coefficients.row(4) = end_q - - end_q_d * velocity_factor; - coefficients.row(5) = end_q; - - // now compute the time-related terms (Bernstein polynomial basis) in the Bezier curve - // Compute tA(i, j) = t(i)^j, - // tB(i, j) = (1-t(i))^(degree-j) - VecX tA = VecX::Ones(6); - VecX tB = VecX::Ones(6); - VecX dtA = VecX::Zero(6); - VecX dtB = VecX::Zero(6); - VecX ddtA = VecX::Zero(6); - VecX ddtB = VecX::Zero(6); - - // Loop to compute tA and tB - for (int j = 1; j <= 5; j++) { - tA(j) = t * tA(j - 1); - tB(5 - j) = (1 - t) * tB(5 - j + 1); - - dtA(j) = j * tA(j - 1); - dtB(5 - j) = -j * tB(5 - j + 1); - - ddtA(j) = j * dtA(j - 1); - ddtB(5 - j) = -j * dtB(5 - j + 1); - } - - B = Bionomials.array() * tA.array() * tB.array(); - dB = Bionomials.array() * (dtA.array() * tB.array() + - tA.array() * dtB.array()) / localT; - ddB = Bionomials.array() * (ddtA.array() * tB.array() + - 2 * dtA.array() * dtB.array() + - tA.array() * ddtB.array()) / (localT * localT); - - // finally, compute the trajectory by multiplying the Bezier coefficients - // with the time-related terms (Bernstein polynomial basis) - q(x) = VecX::Zero(Nact); - q_d(x) = VecX::Zero(Nact); - q_dd(x) = VecX::Zero(Nact); - - q(x) = coefficients.transpose() * B; - q_d(x) = coefficients.transpose() * dB; - q_dd(x) = coefficients.transpose() * ddB; - - if (compute_derivatives) { - pq_pz(x) = MatX::Zero(Nact, varLength); - pq_d_pz(x) = MatX::Zero(Nact, varLength); - pq_dd_pz(x) = MatX::Zero(Nact, varLength); - - if (id == 0) { - for (int i = 0; i < Nact; i++) { - pq_pz(x)(i, i) = B(3) + B(4) + B(5); - pq_pz(x)(i, i + Nact) = -B(3) * 2.0 * velocity_factor - B(4) * velocity_factor; - pq_pz(x)(i, i + 2 * Nact) = B(3) * acceleration_factor; - pq_d_pz(x)(i, i) = dB(3) + dB(4) + dB(5); - pq_d_pz(x)(i, i + Nact) = -dB(3) * 2.0 * velocity_factor - dB(4) * velocity_factor; - pq_d_pz(x)(i, i + 2 * Nact) = dB(3) * acceleration_factor; - pq_dd_pz(x)(i, i) = ddB(3) + ddB(4) + ddB(5); - pq_dd_pz(x)(i, i + Nact) = -ddB(3) * 2.0 * velocity_factor - ddB(4) * velocity_factor; - pq_dd_pz(x)(i, i + 2 * Nact) = ddB(3) * acceleration_factor; - - if (optimize_begin_position) { - pq_pz(x)(i, begin_position_offset + i) = B(0) + B(1) + B(2); - pq_d_pz(x)(i, begin_position_offset + i) = dB(0) + dB(1) + dB(2); - pq_dd_pz(x)(i, begin_position_offset + i) = ddB(0) + ddB(1) + ddB(2); - } - } - } - else if (id == degree) { - for (int i = 0; i < Nact; i++) { - pq_pz(x)(i, (degree - 1) * 3 * Nact + i) = B(0) + B(1) + B(2); - pq_pz(x)(i, (degree - 1) * 3 * Nact + i + Nact) = B(1) * velocity_factor + B(2) * 2.0 * velocity_factor; - pq_pz(x)(i, (degree - 1) * 3 * Nact + i + 2 * Nact) = B(2) * acceleration_factor; - pq_d_pz(x)(i, (degree - 1) * 3 * Nact + i) = dB(0) + dB(1) + dB(2); - pq_d_pz(x)(i, (degree - 1) * 3 * Nact + i + Nact) = dB(1) * velocity_factor + dB(2) * 2.0 * velocity_factor; - pq_d_pz(x)(i, (degree - 1) * 3 * Nact + i + 2 * Nact) = dB(2) * acceleration_factor; - pq_dd_pz(x)(i, (degree - 1) * 3 * Nact + i) = ddB(0) + ddB(1) + ddB(2); - pq_dd_pz(x)(i, (degree - 1) * 3 * Nact + i + Nact) = ddB(1) * velocity_factor + ddB(2) * 2.0 * velocity_factor; - pq_dd_pz(x)(i, (degree - 1) * 3 * Nact + i + 2 * Nact) = ddB(2) * acceleration_factor; - - if (optimize_end_position) { - pq_pz(x)(i, end_position_offset + i) = B(3) + B(4) + B(5); - pq_d_pz(x)(i, end_position_offset + i) = dB(3) + dB(4) + dB(5); - pq_dd_pz(x)(i, end_position_offset + i) = ddB(3) + ddB(4) + ddB(5); - } - } - } - else { - for (int i = 0; i < Nact; i++) { - pq_pz(x)(i, (id - 1) * 3 * Nact + i) = B(0) + B(1) + B(2); - pq_pz(x)(i, (id - 1) * 3 * Nact + i + Nact) = B(1) * velocity_factor + B(2) * 2.0 * velocity_factor; - pq_pz(x)(i, (id - 1) * 3 * Nact + i + 2 * Nact) = B(2) * acceleration_factor; - pq_d_pz(x)(i, (id - 1) * 3 * Nact + i) = dB(0) + dB(1) + dB(2); - pq_d_pz(x)(i, (id - 1) * 3 * Nact + i + Nact) = dB(1) * velocity_factor + dB(2) * 2.0 * velocity_factor; - pq_d_pz(x)(i, (id - 1) * 3 * Nact + i + 2 * Nact) = dB(2) * acceleration_factor; - pq_dd_pz(x)(i, (id - 1) * 3 * Nact + i) = ddB(0) + ddB(1) + ddB(2); - pq_dd_pz(x)(i, (id - 1) * 3 * Nact + i + Nact) = ddB(1) * velocity_factor + ddB(2) * 2.0 * velocity_factor; - pq_dd_pz(x)(i, (id - 1) * 3 * Nact + i + 2 * Nact) = ddB(2) * acceleration_factor; - - pq_pz(x)(i, id * 3 * Nact + i) = B(3) + B(4) + B(5); - pq_pz(x)(i, id * 3 * Nact + i + Nact) = -B(3) * 2.0 * velocity_factor - B(4) * velocity_factor; - pq_pz(x)(i, id * 3 * Nact + i + 2 * Nact) = B(3) * acceleration_factor; - pq_d_pz(x)(i, id * 3 * Nact + i) = dB(3) + dB(4) + dB(5); - pq_d_pz(x)(i, id * 3 * Nact + i + Nact) = -dB(3) * 2.0 * velocity_factor - dB(4) * velocity_factor; - pq_d_pz(x)(i, id * 3 * Nact + i + 2 * Nact) = dB(3) * acceleration_factor; - pq_dd_pz(x)(i, id * 3 * Nact + i) = ddB(3) + ddB(4) + ddB(5); - pq_dd_pz(x)(i, id * 3 * Nact + i + Nact) = -ddB(3) * 2.0 * velocity_factor - ddB(4) * velocity_factor; - pq_dd_pz(x)(i, id * 3 * Nact + i + 2 * Nact) = ddB(3) * acceleration_factor; - } - } - } - - if (compute_hessian) { - for (int i = 0; i < Nact; i++) { - pq_pz_pz(i, x).setZero(); - pq_d_pz_pz(i, x).setZero(); - pq_dd_pz_pz(i, x).setZero(); - } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Trajectories/src/Plain.cpp b/Trajectories/src/Plain.cpp deleted file mode 100644 index 9b85f634..00000000 --- a/Trajectories/src/Plain.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "Plain.h" - -namespace RAPTOR { - -Plain::Plain(const int Nact_input) { - N = 1; - Nact = Nact_input; - varLength = Nact_input; - - q.resize(1, N); - q_d.resize(1, N); - q_dd.resize(1, N); - - pq_pz.resize(1, N); - pq_d_pz.resize(1, N); - pq_dd_pz.resize(1, N); - - pq_pz_pz.resize(Nact, N); - pq_d_pz_pz.resize(Nact, N); - pq_dd_pz_pz.resize(Nact, N); - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -Plain::Plain(const int N_input, - const int Nact_input) { - N = N_input; - Nact = Nact_input; - varLength = Nact_input * N_input; - - q.resize(1, N); - q_d.resize(1, N); - q_dd.resize(1, N); - - pq_pz.resize(1, N); - pq_d_pz.resize(1, N); - pq_dd_pz.resize(1, N); - - pq_pz_pz.resize(Nact, N); - pq_d_pz_pz.resize(Nact, N); - pq_dd_pz_pz.resize(Nact, N); - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -void Plain::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() < varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("Plain: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - for (int i = 0; i < N; i++) { - q(i) = z.block(i * Nact, 0, Nact, 1); - // q_d(i) = 0; - // q_dd(i) = 0; - - if (compute_derivatives) { - pq_pz(i).resize(Nact, varLength); - pq_pz(i).block(0, i * Nact, Nact, Nact) = Eigen::MatrixXd::Identity(Nact, Nact); - // pq_d_pz(i).resize(Nact, varLength); - // pq_dd_pz(i).resize(Nact, varLength); - } - - if (compute_hessian) { - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i).setZero(); - // pq_d_pz_pz(j, i).setZero(); - // pq_dd_pz_pz(j, i).setZero(); - } - } - } -} - -}; // namespace RAPTOR - diff --git a/Trajectories/src/Polynomials.cpp b/Trajectories/src/Polynomials.cpp deleted file mode 100644 index 06309374..00000000 --- a/Trajectories/src/Polynomials.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include "Polynomials.h" - -namespace RAPTOR { - -Polynomials::Polynomials(const VecX& tspan_input, - int Nact_input, - int degree_input) : - Trajectories((degree_input + 1) * Nact_input, tspan_input, Nact_input), - degree(degree_input) { - P = VecX::Zero(degree + 1); - dP = VecX::Zero(degree + 1); - ddP = VecX::Zero(degree + 1); -} - -Polynomials::Polynomials(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - int degree_input) : - Trajectories((degree_input + 1) * Nact_input, T_input, N_input, Nact_input, time_discretization), - degree(degree_input) { - P = VecX::Zero(degree + 1); - dP = VecX::Zero(degree + 1); - ddP = VecX::Zero(degree + 1); -} - -void Polynomials::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() < varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("Polynomials: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - // MatX coefficients = z.head((degree + 1) * Nact).reshaped(degree + 1, Nact); - MatX coefficients = Utils::reshape(z.head((degree + 1) * Nact), degree + 1, Nact); - - for (int x = 0; x < N; x++) { - double t = tspan(x); - - q(x) = VecX::Zero(Nact); - q_d(x) = VecX::Zero(Nact); - q_dd(x) = VecX::Zero(Nact); - - if (compute_derivatives) { - pq_pz(x) = MatX::Zero(Nact, varLength); - pq_d_pz(x) = MatX::Zero(Nact, varLength); - pq_dd_pz(x) = MatX::Zero(Nact, varLength); - } - - P(0) = 1; - dP(0) = 0; - ddP(0) = 0; - - P(1) = t; - dP(1) = 1; - ddP(1) = 0; - - double powt = 1; - for (int i = 2; i <= degree; i++) { - ddP(i) = powt; - dP(i) = t * ddP(i) / (i-1); - P(i) = t * dP(i) / i; - powt *= t; - } - - q(x) = coefficients.transpose() * P; - q_d(x) = coefficients.transpose() * dP; - q_dd(x) = coefficients.transpose() * ddP; - - if (compute_derivatives) { - for (int i = 0; i < Nact; i++) { - pq_pz(x).block(i, i * (degree + 1), 1, degree + 1) = P.transpose(); - pq_d_pz(x).block(i, i * (degree + 1), 1, degree + 1) = dP.transpose(); - pq_dd_pz(x).block(i, i * (degree + 1), 1, degree + 1) = ddP.transpose(); - } - } - - if (compute_hessian) { - // already done this in the constructor - // for (int i = 0; i < Nact; i++) { - // pq_pz_pz(i, x).setZero(); - // pq_d_pz_pz(i, x).setZero(); - // pq_dd_pz_pz(i, x).setZero(); - // } - } - } -} - -}; // namespace RAPTOR - diff --git a/Trajectories/src/Trajectories.cpp b/Trajectories/src/Trajectories.cpp deleted file mode 100644 index 49cff206..00000000 --- a/Trajectories/src/Trajectories.cpp +++ /dev/null @@ -1,163 +0,0 @@ -#include "Trajectories.h" - -namespace RAPTOR { - -Trajectories::Trajectories(const int varLength_input, - const VecX& tspan_input, - int Nact_input) : - varLength(varLength_input), - tspan(tspan_input), - Nact(Nact_input){ - T = tspan.bottomLeftCorner<1,1>().value(); - N = tspan.size(); - - q.resize(1, N); - q_d.resize(1, N); - q_dd.resize(1, N); - - pq_pz.resize(1, N); - pq_d_pz.resize(1, N); - pq_dd_pz.resize(1, N); - - pq_pz_pz.resize(Nact, N); - pq_d_pz_pz.resize(Nact, N); - pq_dd_pz_pz.resize(Nact, N); - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } - - current_z.resize(1); -} - -Trajectories::Trajectories(const int varLength_input, - double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization) : - varLength(varLength_input), - T(T_input), - N(N_input), - Nact(Nact_input) {; - if (time_discretization == Uniform) { - tspan = VecX::LinSpaced(N, 0, T); - } - else if (time_discretization == Chebyshev) { - tspan = VecX::Zero(N); - for (int i = 1; i < N - 1; i++) { - tspan(i) = 0.5 * T * (1 - cos(M_PI * (2 * i - 1) / (2 * (N - 2)))); - } - tspan(0) = 0; - tspan(N - 1) = T; - } - - q.resize(1, N); - q_d.resize(1, N); - q_dd.resize(1, N); - - pq_pz.resize(1, N); - pq_d_pz.resize(1, N); - pq_dd_pz.resize(1, N); - - pq_pz_pz.resize(Nact, N); - pq_d_pz_pz.resize(Nact, N); - pq_dd_pz_pz.resize(Nact, N); - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } - - current_z.resize(1); -} - -bool Trajectories::is_computed(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (compute_hessian && !compute_derivatives) { - throw std::invalid_argument("compute_derivatives needs to be true when compute_hessian is true."); - return false; - } - - if (!Utils::ifTwoVectorEqual(current_z, z, 0)) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - if (compute_derivatives != if_compute_derivatives) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - if (compute_hessian != if_compute_hessian) { - current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return false; - } - - // current_z = z; - if_compute_derivatives = compute_derivatives; - if_compute_hessian = compute_hessian; - return true; -} - -void Trajectories::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - } - - if (compute_derivatives) { - for (int i = 0; i < N; i++) { - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - } - } - - if (compute_hessian) { - for (int i = 0; i < N; i++) { - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } - } -} - -}; // namespace RAPTOR - diff --git a/Trajectories/src/TrajectoryGroup.cpp b/Trajectories/src/TrajectoryGroup.cpp deleted file mode 100644 index bde52663..00000000 --- a/Trajectories/src/TrajectoryGroup.cpp +++ /dev/null @@ -1,121 +0,0 @@ -#include "TrajectoryGroup.h" - -namespace RAPTOR { - -void TrajectoryGroup::add_trajectory(const std::string& name, - std::shared_ptr trajectory) { - trajectories[name] = trajectory; - gather_trajectories_information(); -} - -void TrajectoryGroup::gather_trajectories_information(const bool print_info) { - T = 0; - N = 0; - Nact = trajectories.begin()->second->Nact; - varLength = 0; - - if (print_info) std::cout << "Trajectory group information: " << std::endl; - - size_t index = 0; - for (const auto& it : trajectories) { - T += it.second->T; - - trajectory_locations[it.first] = std::make_pair(N, N + it.second->N); - N += it.second->N; - - if (Nact != it.second->Nact) { - throw std::invalid_argument("The number of actuated joints in the trajectories are not the same."); - } - - variable_locations[it.first] = std::make_pair(varLength, varLength + it.second->varLength); - varLength += it.second->varLength; - - if (print_info) { - std::cout << "Trajectory " << index << ": " << it.first << std::endl; - std::cout << " trajectory location: [" - << trajectory_locations[it.first].first << ", " - << trajectory_locations[it.first].second << "]" << std::endl; - std::cout << " variable location: [" - << variable_locations[it.first].first << ", " - << variable_locations[it.first].second << "]" << std::endl; - } - - index++; - } - - q.resize(1, N); - q_d.resize(1, N); - q_dd.resize(1, N); - - pq_pz.resize(1, N); - pq_d_pz.resize(1, N); - pq_dd_pz.resize(1, N); - - pq_pz_pz.resize(Nact, N); - pq_d_pz_pz.resize(Nact, N); - pq_dd_pz_pz.resize(Nact, N); - - for (int i = 0; i < N; i++) { - q(i) = VecX::Zero(Nact); - q_d(i) = VecX::Zero(Nact); - q_dd(i) = VecX::Zero(Nact); - - pq_pz(i) = MatX::Zero(Nact, varLength); - pq_d_pz(i) = MatX::Zero(Nact, varLength); - pq_dd_pz(i) = MatX::Zero(Nact, varLength); - - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_d_pz_pz(j, i) = MatX::Zero(varLength, varLength); - pq_dd_pz_pz(j, i) = MatX::Zero(varLength, varLength); - } - } -} - -void TrajectoryGroup::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - for (const auto& it : trajectories) { - const size_t trajectory_offset = trajectory_locations[it.first].first; - const size_t variable_offset = variable_locations[it.first].first; - - const VecX& z_segment = z.segment(variable_offset, - variable_locations[it.first].second - variable_offset); - it.second->compute(z_segment, compute_derivatives, compute_hessian); - - // copy the computed values of each trajectory to the local variables in TrajectoryGroup - for (int i = 0; i < it.second->N; i++) { - q(trajectory_offset + i) = it.second->q(i); - q_d(trajectory_offset + i) = it.second->q_d(i); - q_dd(trajectory_offset + i) = it.second->q_dd(i); - - if (compute_derivatives) { - pq_pz(trajectory_offset + i) - .block(0, variable_offset, Nact, it.second->varLength) - = it.second->pq_pz(i); - pq_d_pz(trajectory_offset + i) - .block(0, variable_offset, Nact, it.second->varLength) - = it.second->pq_d_pz(i); - pq_dd_pz(trajectory_offset + i) - .block(0, variable_offset, Nact, it.second->varLength) - = it.second->pq_dd_pz(i); - } - - if (compute_hessian) { - for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, trajectory_offset + i) - .block(variable_offset, variable_offset, it.second->varLength, it.second->varLength) - = it.second->pq_pz_pz(j, i); - pq_d_pz_pz(j, trajectory_offset + i) - .block(variable_offset, variable_offset, it.second->varLength, it.second->varLength) - = it.second->pq_d_pz_pz(j, i); - pq_dd_pz_pz(j, trajectory_offset + i) - .block(variable_offset, variable_offset, it.second->varLength, it.second->varLength) - = it.second->pq_dd_pz_pz(j, i); - } - } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Trajectories/src/TransferBezierCurves.cpp b/Trajectories/src/TransferBezierCurves.cpp deleted file mode 100644 index 6b1642d6..00000000 --- a/Trajectories/src/TransferBezierCurves.cpp +++ /dev/null @@ -1,135 +0,0 @@ -#include "TransferBezierCurves.h" - -namespace RAPTOR { - -TransferBezierCurves::TransferBezierCurves(double T_input, - int N_input, - int Nact_input, - const int degree_input, - TimeDiscretization time_discretization, - const TransferTrajectoryParameters& ttp_input) : - BezierCurves(T_input, N_input, Nact_input, time_discretization, degree_input) { - T = T_input; - N = N_input; - Nact = Nact_input; - degree = degree_input; - ttp = ttp_input; - - tspan = VecX::Zero(N); - for (int i = 0; i < N; i++) { - tspan(i) = 0.5 * T * (1 - cos(M_PI * (2 * i + 1) / (2 * N))); - } - - B = VecX::Zero(degree + 1); - dB = VecX::Zero(degree + 1); - ddB = VecX::Zero(degree + 1); - - Bionomials = VecX::Ones(degree + 1); - for (int j = 1; j <= degree / 2; j++) { - Bionomials(j) = Bionomials(j - 1) * (degree + 1 - j) / j; - Bionomials(degree - j) = Bionomials(j); - } - - if (ttp.q0.size() != Nact) { - throw std::invalid_argument("TransferBezierCurves: q0.size() != Nact"); - } - if (ttp.qT.size() != Nact) { - throw std::invalid_argument("TransferBezierCurves: qT.size() != Nact"); - } - - varLength = Nact * (degree - 5); - - coefficients.resize(degree + 1, Nact); - coefficients.row(0) = ttp.q0.transpose(); - coefficients.row(1) = coefficients.row(0); - coefficients.row(2) = coefficients.row(0); - coefficients.row(degree) = ttp.qT.transpose(); - coefficients.row(degree - 1) = coefficients.row(degree); - coefficients.row(degree - 2) = coefficients.row(degree); - - if (varLength <= 0) { - throw std::invalid_argument("TransferBezierCurves: degree must be larget than 5"); - } -} - -void TransferBezierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() != varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("TransferBezierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - for (int i = 3; i < degree - 2; i++) { - coefficients.row(i) = z.block((i - 3) * Nact, 0, Nact, 1).transpose(); - } - - for (int x = 0; x < N; x++) { - double t = tspan(x) / T; - - q(x) = VecX::Zero(Nact + 1); - q_d(x) = VecX::Zero(Nact + 1); - q_dd(x) = VecX::Zero(Nact + 1); - - if (compute_derivatives) { - pq_pz(x) = MatX::Zero(Nact, varLength); - pq_d_pz(x) = MatX::Zero(Nact, varLength); - pq_dd_pz(x) = MatX::Zero(Nact, varLength); - } - - // Compute tA(i, j) = t(i)^j, - // tB(i, j) = (1-t(i))^(degree-j) - VecX tA = VecX::Ones(degree + 1); - VecX tB = VecX::Ones(degree + 1); - VecX dtA = VecX::Zero(degree + 1); - VecX dtB = VecX::Zero(degree + 1); - VecX ddtA = VecX::Zero(degree + 1); - VecX ddtB = VecX::Zero(degree + 1); - - // Loop to compute tA and tB - for (int j = 1; j <= degree; j++) { - tA(j) = t * tA(j - 1); - tB(degree - j) = (1 - t) * tB(degree - j + 1); - - dtA(j) = j * tA(j - 1); - dtB(degree - j) = -j * tB(degree - j + 1); - - ddtA(j) = j * dtA(j - 1); - ddtB(degree - j) = -j * dtB(degree - j + 1); - } - - B = Bionomials.array() * tA.array() * tB.array(); - dB = Bionomials.array() * (dtA.array() * tB.array() + - tA.array() * dtB.array()) / T; - ddB = Bionomials.array() * (ddtA.array() * tB.array() + - 2 * dtA.array() * dtB.array() + - tA.array() * ddtB.array()) / (T * T); - - q(x).head(Nact) = coefficients.transpose() * B; - q_d(x).head(Nact) = coefficients.transpose() * dB; - q_dd(x).head(Nact) = coefficients.transpose() * ddB; - - if (compute_derivatives) { - for (int i = 0; i < Nact; i++) { - for (int j = 3; j < degree - 2; j++) { - pq_pz(x)(i, (j - 3) * Nact + i) = B(j); - pq_d_pz(x)(i, (j - 3) * Nact + i) = dB(j); - pq_dd_pz(x)(i, (j - 3) * Nact + i) = ddB(j); - } - } - } - - if (compute_hessian) { - for (int i = 0; i < Nact; i++) { - pq_pz_pz(i, x).setZero(); - pq_d_pz_pz(i, x).setZero(); - pq_dd_pz_pz(i, x).setZero(); - } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Trajectories/src/WaitrBezierCurves.cpp b/Trajectories/src/WaitrBezierCurves.cpp deleted file mode 100644 index 1d99370f..00000000 --- a/Trajectories/src/WaitrBezierCurves.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include "WaitrBezierCurves.h" - -namespace RAPTOR { - -WaitrBezierCurves::WaitrBezierCurves(const VecX& tspan_input, - int Nact_input, - const WaitrTrajectoryParameters& atp_input) : - BezierCurves(tspan_input, Nact_input, WAITR_BEZIER_CURVE_DEGREE), - atp(atp_input) { - if (atp.q0.size() != Nact) { - throw std::invalid_argument("WaitrBezierCurves: q0.size() != Nact"); - } - if (atp.q_d0.size() != Nact) { - throw std::invalid_argument("WaitrBezierCurves: q_d0.size() != Nact"); - } - if (atp.q_dd0.size() != Nact) { - throw std::invalid_argument("WaitrBezierCurves: q_dd0.size() != Nact"); - } - - varLength = Nact; - - coefficients.resize(degree + 1, Nact); - coefficients.row(0) = atp.q0.transpose(); - coefficients.row(1) = coefficients.row(0) + - (T * atp.q_d0.transpose()) / 5.0; - coefficients.row(2) = coefficients.row(0) + - (2.0 * T * atp.q_d0.transpose()) / 5.0 + - (T * T * atp.q_dd0.transpose()) / 20.0; -} - -WaitrBezierCurves::WaitrBezierCurves(double T_input, - int N_input, - int Nact_input, - TimeDiscretization time_discretization, - const WaitrTrajectoryParameters& atp_input) : - BezierCurves(T_input, N_input, Nact_input, time_discretization, WAITR_BEZIER_CURVE_DEGREE), - atp(atp_input) { - if (atp.q0.size() != Nact) { - throw std::invalid_argument("WaitrBezierCurves: q0.size() != Nact"); - } - if (atp.q_d0.size() != Nact) { - throw std::invalid_argument("WaitrBezierCurves: q_d0.size() != Nact"); - } - if (atp.q_dd0.size() != Nact) { - throw std::invalid_argument("WaitrBezierCurves: q_dd0.size() != Nact"); - } - - varLength = Nact; - - coefficients.resize(degree + 1, Nact); - coefficients.row(0) = atp.q0.transpose(); - coefficients.row(1) = atp.q0.transpose() + - (T * atp.q_d0.transpose()) / 5.0; - coefficients.row(2) = atp.q0.transpose() + - (2.0 * T * atp.q_d0.transpose()) / 5.0 + - (T * T * atp.q_dd0.transpose()) / 20.0; -} - -void WaitrBezierCurves::compute(const VecX& z, - bool compute_derivatives, - bool compute_hessian) { - if (z.size() != varLength) { - std::cerr << "function input: z.size() = " << z.size() << std::endl; - std::cerr << "desired: varLength = " << varLength << std::endl; - throw std::invalid_argument("WaitrBezierCurves: decision variable vector has wrong size"); - } - - if (is_computed(z, compute_derivatives, compute_hessian)) return; - - coefficients.row(5) = z.transpose(); - coefficients.row(4) = z.transpose(); - coefficients.row(3) = z.transpose(); - - for (int x = 0; x < N; x++) { - double t = tspan(x) / T; - - q(x) = VecX::Zero(Nact + 1); - q_d(x) = VecX::Zero(Nact + 1); - q_dd(x) = VecX::Zero(Nact + 1); - - if (compute_derivatives) { - pq_pz(x) = MatX::Zero(Nact, varLength); - pq_d_pz(x) = MatX::Zero(Nact, varLength); - pq_dd_pz(x) = MatX::Zero(Nact, varLength); - } - - // Compute tA(i, j) = t(i)^j, - // tB(i, j) = (1-t(i))^(degree-j) - VecX tA = VecX::Ones(degree + 1); - VecX tB = VecX::Ones(degree + 1); - VecX dtA = VecX::Zero(degree + 1); - VecX dtB = VecX::Zero(degree + 1); - VecX ddtA = VecX::Zero(degree + 1); - VecX ddtB = VecX::Zero(degree + 1); - - // Loop to compute tA and tB - for (int j = 1; j <= degree; j++) { - tA(j) = t * tA(j - 1); - tB(degree - j) = (1 - t) * tB(degree - j + 1); - - dtA(j) = j * tA(j - 1); - dtB(degree - j) = -j * tB(degree - j + 1); - - ddtA(j) = j * dtA(j - 1); - ddtB(degree - j) = -j * dtB(degree - j + 1); - } - - B = Bionomials.array() * tA.array() * tB.array(); - dB = Bionomials.array() * (dtA.array() * tB.array() + - tA.array() * dtB.array()) / T; - ddB = Bionomials.array() * (ddtA.array() * tB.array() + - 2 * dtA.array() * dtB.array() + - tA.array() * ddtB.array()) / (T * T); - - q(x).head(Nact) = coefficients.transpose() * B; - q_d(x).head(Nact) = coefficients.transpose() * dB; - q_dd(x).head(Nact) = coefficients.transpose() * ddB; - - if (compute_derivatives) { - for (int i = 0; i < Nact; i++) { - pq_pz(x)(i, i) = B(3) + B(4) + B(5); - pq_d_pz(x)(i, i) = dB(3) + dB(4) + dB(5); - pq_dd_pz(x)(i, i) = ddB(3) + ddB(4) + ddB(5); - } - } - - if (compute_hessian) { - for (int i = 0; i < Nact; i++) { - pq_pz_pz(i, x).setZero(); - pq_d_pz_pz(i, x).setZero(); - pq_dd_pz_pz(i, x).setZero(); - } - } - } -} - -}; // namespace RAPTOR \ No newline at end of file diff --git a/Utils/include/HigherOrderDerivatives.h b/Utils/include/HigherOrderDerivatives.h deleted file mode 100644 index 5e6ef9d7..00000000 --- a/Utils/include/HigherOrderDerivatives.h +++ /dev/null @@ -1,204 +0,0 @@ -#ifndef HIGHER_ORDER_DERIVATIVES_H -#define HIGHER_ORDER_DERIVATIVES_H - -#include -#include - -namespace RAPTOR { -namespace HigherOrderDerivatives { - -inline double safeasin(const double x, - const bool throw_exception = false) { - if (x > 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return M_PI / 2.0; - } - else if (x < -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return -M_PI / 2.0; - } - return std::asin(x); -} - -inline double safedasindx(const double x, - const bool throw_exception = false) { - if (x >= 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return 1e10; // a very large positive number - } - else if (x <= -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return -1e10; // a very large negative number - } - return 1.0 / std::sqrt(1.0 - x * x); -} - -inline double safeddasinddx(const double x, - const bool throw_exception = false) { - if (x >= 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return 1e10; // a very large positive number - } - else if (x <= -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return -1e10; // a very large negative number - } - return x / std::pow(1.0 - x * x, 1.5); -} - -inline double safedddasindddx(const double x, - const bool throw_exception = false) { - if (x >= 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return 1e10; // a very large positive number - } - else if (x <= -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return -1e10; // a very large negative number - } - const double xSquare = x * x; - return (1 + 2 * xSquare) / std::pow(1.0 - xSquare, 2.5); -} - -inline double safeacos(const double x, - const bool throw_exception = false) { - if (x > 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return 0.0; - } - else if (x < -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return M_PI; - } - return std::acos(x); -} - -inline double safedacosdx(const double x, - const bool throw_exception = false) { - if (x >= 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return -1e10; // a very large negative number - } - else if (x <= -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return 1e10; // a very large positive number - } - return -1.0 / std::sqrt(1.0 - x * x); -} - -inline double safeddacosddx(const double x, - const bool throw_exception = false) { - if (x >= 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return 1e10; // a very large positive number - } - else if (x <= -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return -1e10; // a very large negative number - } - return -x / std::pow(1.0 - x * x, 1.5); -} - -inline double safedddacosdddx(const double x, - const bool throw_exception = false) { - if (x >= 1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is greater than 1.0"); - } - return -1e10; // a very large negative number - } - else if (x <= -1.0) { - if (throw_exception) { - throw std::runtime_error("Input value is less than -1.0"); - } - return 1e10; // a very large positive number - } - const double xSquare = x * x; - return -(1 + 2 * xSquare) / std::pow(1.0 - xSquare, 2.5); -} - -inline double safexSinx(const double x, - const double nearZeroThreshold = false) { - if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate - double xSquare = x * x; - double xFourth = xSquare * xSquare; - return 1.0 + xSquare / 6.0 + 7.0 * xFourth / 360.0; // + O(x^6) - } - return x / std::sin(x); -} - -inline double safedxSinxdx(const double x, - double nearZeroThreshold = false) { - if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate - double xSquare = x * x; - double xThird = x * xSquare; - double XFifth = xThird * xSquare; - return x / 3.0 + 7.0 * xThird / 90.0 + 31.0 * XFifth / 2520.0; // + O(x^7) - } - const double sinx = std::sin(x); - return (sinx - x * std::cos(x)) / (sinx * sinx); -} - -inline double safeddxSinxddx(const double x, - const double nearZeroThreshold = false) { - if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate - double xSquare = x * x; - double xFourth = xSquare * xSquare; - double xSixth = xFourth * xSquare; - return 1.0 / 3.0 + 7.0 * xSquare / 30.0 + 31.0 * xFourth / 504.0; // + O(x^6) - } - const double sinx = std::sin(x); - const double cosx = std::cos(x); - const double sinxSquare = sinx * sinx; - return (x * sinxSquare - 2 * cosx * sinx + 2 * x * cosx * cosx) / (sinxSquare * sinx); -} - -inline double safedddxSinxdddx(const double x, - const double nearZeroThreshold = false) { - if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate - double xSquare = x * x; - double xThird = x * xSquare; - double XFifth = xThird * xSquare; - return 7.0 * x / 15.0 + 31.0 * xThird / 126.0 + 127.0 * XFifth / 1800.0; // + O(x^7) - } - const double sinx = std::sin(x); - const double cosx = std::cos(x); - const double sinxSquare = sinx * sinx; - return x * cosx / sinxSquare + - 6 * cosx * cosx / (sinx * sinxSquare) - - 6 * x * cosx / (sinxSquare * sinxSquare) + - 3 / sinx; -} - -}; // namespace HigherOrderDerivatives -}; // namespace RAPTOR - -#endif // HIGHER_ORDER_DERIVATIVES_H \ No newline at end of file diff --git a/Utils/include/Utils.h b/Utils/include/Utils.h deleted file mode 100644 index 2d0fbec5..00000000 --- a/Utils/include/Utils.h +++ /dev/null @@ -1,284 +0,0 @@ -#ifndef UTILS_H -#define UTILS_H - -#include -#include -#include -#include -#include - -namespace RAPTOR { -namespace Utils { - -inline double deg2rad(const double deg) { - return deg * M_PI / 180.0; -} - -inline double rad2deg(const double rad) { - return rad * 180.0 / M_PI; -} - -inline Eigen::Vector3d deg2rad(const Eigen::Vector3d& deg) { - return deg * M_PI / 180.0; -} - -inline Eigen::Vector3d rad2deg(const Eigen::Vector3d& rad) { - return rad * 180.0 / M_PI; -} - -inline Eigen::VectorXd deg2rad(const Eigen::VectorXd& deg) { - return deg * M_PI / 180.0; -} - -inline Eigen::VectorXd rad2deg(const Eigen::VectorXd& rad) { - return rad * 180.0 / M_PI; -} - -inline double wrapToPi(const double angle) { - double res = angle; - while (res > M_PI) { - res -= 2.0 * M_PI; - } - while (res < -M_PI) { - res += 2.0 * M_PI; - } - return res; -} - -inline Eigen::VectorXd wrapToPi(const Eigen::VectorXd& angles) { - Eigen::VectorXd res = angles; - for (int i = 0; i < res.size(); i++) { - res(i) = wrapToPi(res(i)); - } - return res; -} - -inline double sign(double val, double eps = 1e-8) { - if (val > eps) { - return 1.0; - } - else if (val < -eps) { - return -1.0; - } - else { - return 0.0; - } -} - -inline bool ifTwoVectorEqual(const Eigen::VectorXd& a, - const Eigen::VectorXd& b, - double tol = 1e-10) { - if (a.size() != b.size()) { - return false; - } - for (int i = 0; i < a.size(); i++) { - if (fabs(a(i) - b(i)) > tol) { - return false; - } - } - return true; -} - -inline bool ifTwoMatrixEqual(const Eigen::MatrixXd& a, - const Eigen::MatrixXd& b, - double tol = 1e-10) { - if (a.rows() != b.rows() || a.cols() != b.cols()) { - return false; - } - for (int i = 0; i < a.rows(); i++) { - for (int j = 0; j < a.cols(); j++) { - if (fabs(a(i, j) - b(i, j)) > tol) { - return false; - } - } - } - return true; -} - -inline Eigen::MatrixXd reshape(const Eigen::VectorXd& vec, - int rows, - int cols) { - return Eigen::Map(vec.data(), rows, cols); -} - -inline Eigen::Matrix3d skew(const Eigen::Vector3d& v) { - Eigen::Matrix3d res; - res << 0, -v(2), v(1), - v(2), 0, -v(0), - -v(1), v(0), 0; - return res; -} - -inline Eigen::Vector3d skew(const Eigen::Matrix3d& m) { - Eigen::Vector3d res; - res << m(2,1) - m(1,2), - m(0,2) - m(2,0), - m(1,0) - m(0,1); - return 0.5 * res; -} - -inline Eigen::Vector3d unskew(const Eigen::Matrix3d& m) { - Eigen::Vector3d res; - res << m(2,1), - m(0,2), - m(1,0); - return res; -} - -inline Eigen::Matrix plux(const Eigen::Matrix3d& R, - const Eigen::Vector3d& p) { - Eigen::Matrix res; - res << R, Eigen::MatrixXd::Zero(3, 3), - -R * skew(p), R; - return res; -} - -inline Eigen::VectorXd initializeEigenVectorFromArray(const double* array, - size_t size) { - Eigen::VectorXd res(size); - for (int i = 0; i < size; i++) { - res(i) = array[i]; - } - return res; -} - -inline Eigen::MatrixXd initializeEigenMatrixFromFile(const std::string filename) { - std::ifstream file(filename); - if (!file.is_open()) { - throw std::runtime_error("Cannot open file " + filename); - } - - std::vector> data; - std::string line; - - while (std::getline(file, line)) { - std::istringstream iss(line); - std::vector lineData; - double value; - while (iss >> value) { - lineData.push_back(value); - } - data.push_back(lineData); - } - - Eigen::MatrixXd res(data.size(), data[0].size()); - for (int i = 0; i < data.size(); i++) { - for (int j = 0; j < data[0].size(); j++) { - res(i, j) = data[i][j]; - } - } - - return res; -} - -inline void writeEigenMatrixToFile(const Eigen::MatrixXd& matrix, - const std::string filename) { - std::ofstream file(filename); - if (!file.is_open()) { - throw std::runtime_error("Cannot open file " + filename); - } - - for (int i = 0; i < matrix.rows(); i++) { - for (int j = 0; j < matrix.cols(); j++) { - file << matrix(i, j) << " "; - } - file << std::endl; - } - - file.close(); -} - -inline Eigen::VectorXd uniformlySampleVector(const Eigen::VectorXd& vec, - int numSamples) { - Eigen::VectorXd samples(0); - - if (numSamples <= 0 || - vec.size() == 0) { - return samples; - } - - if (numSamples >= vec.size()) { - samples = vec; - return samples; - } - - samples.resize(numSamples); - - // Calculate sampling interval - double interval = static_cast(vec.size()) / numSamples; - - for (int i = 0; i < numSamples; i++) { - int idx = static_cast(i * interval); - // Ensure the index is within the bounds of the vector - if (idx >= 0 && idx < vec.size()) { - samples(i) = vec(idx); - } - } - - return samples; -} - -inline Eigen::MatrixXd uniformlySampleMatrixInRows(const Eigen::MatrixXd& mat, - int numSamples) { - Eigen::MatrixXd samples(0, 0); - - if (numSamples <= 0 || - mat.rows() == 0 || - mat.cols() == 0) { - return samples; - } - - if (numSamples >= mat.rows()) { - samples = mat; - return samples; - } - - samples.resize(numSamples, mat.cols()); - - // Calculate sampling interval - double interval = static_cast(mat.rows()) / numSamples; - - for (int i = 0; i < numSamples; i++) { - int idx = static_cast(i * interval); - if (idx >= 0 && idx < mat.rows()) { - samples.row(i) = mat.row(idx); - } - } - - return samples; -} - -inline Eigen::MatrixXd uniformlySampleMatrixInCols(const Eigen::MatrixXd& mat, - int numSamples) { - Eigen::MatrixXd samples(0, 0); - - if (numSamples <= 0 || - mat.rows() == 0 || - mat.cols() == 0) { - return samples; - } - - if (numSamples >= mat.cols()) { - samples = mat; - return samples; - } - - samples.resize(mat.rows(), numSamples); - - // Calculate sampling interval - double interval = static_cast(mat.cols()) / numSamples; - - for (int i = 0; i < numSamples; i++) { - int idx = static_cast(i * interval); - if (idx >= 0 && idx < mat.cols()) { - samples.col(i) = mat.col(idx); - } - } - - return samples; -} - -}; // namespace Utils -}; // namespace RAPTOR - -#endif // UTILS_H diff --git a/assets/css/font-awesome.min.css b/assets/css/font-awesome.min.css new file mode 100755 index 00000000..9b27f8ea --- /dev/null +++ b/assets/css/font-awesome.min.css @@ -0,0 +1,4 @@ +/*! + * Font Awesome 4.6.3 by @davegandy - http://fontawesome.io - @fontawesome + * License - http://fontawesome.io/license (Font: SIL OFL 1.1, CSS: MIT License) + */@font-face{font-family:'FontAwesome';src:url('../fonts/fontawesome-webfont.eot?v=4.6.3');src:url('../fonts/fontawesome-webfont.eot?#iefix&v=4.6.3') format('embedded-opentype'),url('../fonts/fontawesome-webfont.woff2?v=4.6.3') format('woff2'),url('../fonts/fontawesome-webfont.woff?v=4.6.3') format('woff'),url('../fonts/fontawesome-webfont.ttf?v=4.6.3') format('truetype'),url('../fonts/fontawesome-webfont.svg?v=4.6.3#fontawesomeregular') format('svg');font-weight:normal;font-style:normal}.fa{display:inline-block;font:normal normal normal 14px/1 FontAwesome;font-size:inherit;text-rendering:auto;-webkit-font-smoothing:antialiased;-moz-osx-font-smoothing:grayscale}.fa-lg{font-size:1.33333333em;line-height:.75em;vertical-align:-15%}.fa-2x{font-size:2em}.fa-3x{font-size:3em}.fa-4x{font-size:4em}.fa-5x{font-size:5em}.fa-fw{width:1.28571429em;text-align:center}.fa-ul{padding-left:0;margin-left:2.14285714em;list-style-type:none}.fa-ul>li{position:relative}.fa-li{position:absolute;left:-2.14285714em;width:2.14285714em;top:.14285714em;text-align:center}.fa-li.fa-lg{left:-1.85714286em}.fa-border{padding:.2em .25em .15em;border:solid .08em #eee;border-radius:.1em}.fa-pull-left{float:left}.fa-pull-right{float:right}.fa.fa-pull-left{margin-right:.3em}.fa.fa-pull-right{margin-left:.3em}.pull-right{float:right}.pull-left{float:left}.fa.pull-left{margin-right:.3em}.fa.pull-right{margin-left:.3em}.fa-spin{-webkit-animation:fa-spin 2s infinite linear;animation:fa-spin 2s infinite linear}.fa-pulse{-webkit-animation:fa-spin 1s infinite steps(8);animation:fa-spin 1s infinite steps(8)}@-webkit-keyframes fa-spin{0%{-webkit-transform:rotate(0deg);transform:rotate(0deg)}100%{-webkit-transform:rotate(359deg);transform:rotate(359deg)}}@keyframes fa-spin{0%{-webkit-transform:rotate(0deg);transform:rotate(0deg)}100%{-webkit-transform:rotate(359deg);transform:rotate(359deg)}}.fa-rotate-90{-ms-filter:"progid:DXImageTransform.Microsoft.BasicImage(rotation=1)";-webkit-transform:rotate(90deg);-ms-transform:rotate(90deg);transform:rotate(90deg)}.fa-rotate-180{-ms-filter:"progid:DXImageTransform.Microsoft.BasicImage(rotation=2)";-webkit-transform:rotate(180deg);-ms-transform:rotate(180deg);transform:rotate(180deg)}.fa-rotate-270{-ms-filter:"progid:DXImageTransform.Microsoft.BasicImage(rotation=3)";-webkit-transform:rotate(270deg);-ms-transform:rotate(270deg);transform:rotate(270deg)}.fa-flip-horizontal{-ms-filter:"progid:DXImageTransform.Microsoft.BasicImage(rotation=0, mirror=1)";-webkit-transform:scale(-1, 1);-ms-transform:scale(-1, 1);transform:scale(-1, 1)}.fa-flip-vertical{-ms-filter:"progid:DXImageTransform.Microsoft.BasicImage(rotation=2, mirror=1)";-webkit-transform:scale(1, -1);-ms-transform:scale(1, -1);transform:scale(1, -1)}:root .fa-rotate-90,:root .fa-rotate-180,:root .fa-rotate-270,:root .fa-flip-horizontal,:root .fa-flip-vertical{filter:none}.fa-stack{position:relative;display:inline-block;width:2em;height:2em;line-height:2em;vertical-align:middle}.fa-stack-1x,.fa-stack-2x{position:absolute;left:0;width:100%;text-align:center}.fa-stack-1x{line-height:inherit}.fa-stack-2x{font-size:2em}.fa-inverse{color:#fff}.fa-glass:before{content:"\f000"}.fa-music:before{content:"\f001"}.fa-search:before{content:"\f002"}.fa-envelope-o:before{content:"\f003"}.fa-heart:before{content:"\f004"}.fa-star:before{content:"\f005"}.fa-star-o:before{content:"\f006"}.fa-user:before{content:"\f007"}.fa-film:before{content:"\f008"}.fa-th-large:before{content:"\f009"}.fa-th:before{content:"\f00a"}.fa-th-list:before{content:"\f00b"}.fa-check:before{content:"\f00c"}.fa-remove:before,.fa-close:before,.fa-times:before{content:"\f00d"}.fa-search-plus:before{content:"\f00e"}.fa-search-minus:before{content:"\f010"}.fa-power-off:before{content:"\f011"}.fa-signal:before{content:"\f012"}.fa-gear:before,.fa-cog:before{content:"\f013"}.fa-trash-o:before{content:"\f014"}.fa-home:before{content:"\f015"}.fa-file-o:before{content:"\f016"}.fa-clock-o:before{content:"\f017"}.fa-road:before{content:"\f018"}.fa-download:before{content:"\f019"}.fa-arrow-circle-o-down:before{content:"\f01a"}.fa-arrow-circle-o-up:before{content:"\f01b"}.fa-inbox:before{content:"\f01c"}.fa-play-circle-o:before{content:"\f01d"}.fa-rotate-right:before,.fa-repeat:before{content:"\f01e"}.fa-refresh:before{content:"\f021"}.fa-list-alt:before{content:"\f022"}.fa-lock:before{content:"\f023"}.fa-flag:before{content:"\f024"}.fa-headphones:before{content:"\f025"}.fa-volume-off:before{content:"\f026"}.fa-volume-down:before{content:"\f027"}.fa-volume-up:before{content:"\f028"}.fa-qrcode:before{content:"\f029"}.fa-barcode:before{content:"\f02a"}.fa-tag:before{content:"\f02b"}.fa-tags:before{content:"\f02c"}.fa-book:before{content:"\f02d"}.fa-bookmark:before{content:"\f02e"}.fa-print:before{content:"\f02f"}.fa-camera:before{content:"\f030"}.fa-font:before{content:"\f031"}.fa-bold:before{content:"\f032"}.fa-italic:before{content:"\f033"}.fa-text-height:before{content:"\f034"}.fa-text-width:before{content:"\f035"}.fa-align-left:before{content:"\f036"}.fa-align-center:before{content:"\f037"}.fa-align-right:before{content:"\f038"}.fa-align-justify:before{content:"\f039"}.fa-list:before{content:"\f03a"}.fa-dedent:before,.fa-outdent:before{content:"\f03b"}.fa-indent:before{content:"\f03c"}.fa-video-camera:before{content:"\f03d"}.fa-photo:before,.fa-image:before,.fa-picture-o:before{content:"\f03e"}.fa-pencil:before{content:"\f040"}.fa-map-marker:before{content:"\f041"}.fa-adjust:before{content:"\f042"}.fa-tint:before{content:"\f043"}.fa-edit:before,.fa-pencil-square-o:before{content:"\f044"}.fa-share-square-o:before{content:"\f045"}.fa-check-square-o:before{content:"\f046"}.fa-arrows:before{content:"\f047"}.fa-step-backward:before{content:"\f048"}.fa-fast-backward:before{content:"\f049"}.fa-backward:before{content:"\f04a"}.fa-play:before{content:"\f04b"}.fa-pause:before{content:"\f04c"}.fa-stop:before{content:"\f04d"}.fa-forward:before{content:"\f04e"}.fa-fast-forward:before{content:"\f050"}.fa-step-forward:before{content:"\f051"}.fa-eject:before{content:"\f052"}.fa-chevron-left:before{content:"\f053"}.fa-chevron-right:before{content:"\f054"}.fa-plus-circle:before{content:"\f055"}.fa-minus-circle:before{content:"\f056"}.fa-times-circle:before{content:"\f057"}.fa-check-circle:before{content:"\f058"}.fa-question-circle:before{content:"\f059"}.fa-info-circle:before{content:"\f05a"}.fa-crosshairs:before{content:"\f05b"}.fa-times-circle-o:before{content:"\f05c"}.fa-check-circle-o:before{content:"\f05d"}.fa-ban:before{content:"\f05e"}.fa-arrow-left:before{content:"\f060"}.fa-arrow-right:before{content:"\f061"}.fa-arrow-up:before{content:"\f062"}.fa-arrow-down:before{content:"\f063"}.fa-mail-forward:before,.fa-share:before{content:"\f064"}.fa-expand:before{content:"\f065"}.fa-compress:before{content:"\f066"}.fa-plus:before{content:"\f067"}.fa-minus:before{content:"\f068"}.fa-asterisk:before{content:"\f069"}.fa-exclamation-circle:before{content:"\f06a"}.fa-gift:before{content:"\f06b"}.fa-leaf:before{content:"\f06c"}.fa-fire:before{content:"\f06d"}.fa-eye:before{content:"\f06e"}.fa-eye-slash:before{content:"\f070"}.fa-warning:before,.fa-exclamation-triangle:before{content:"\f071"}.fa-plane:before{content:"\f072"}.fa-calendar:before{content:"\f073"}.fa-random:before{content:"\f074"}.fa-comment:before{content:"\f075"}.fa-magnet:before{content:"\f076"}.fa-chevron-up:before{content:"\f077"}.fa-chevron-down:before{content:"\f078"}.fa-retweet:before{content:"\f079"}.fa-shopping-cart:before{content:"\f07a"}.fa-folder:before{content:"\f07b"}.fa-folder-open:before{content:"\f07c"}.fa-arrows-v:before{content:"\f07d"}.fa-arrows-h:before{content:"\f07e"}.fa-bar-chart-o:before,.fa-bar-chart:before{content:"\f080"}.fa-twitter-square:before{content:"\f081"}.fa-facebook-square:before{content:"\f082"}.fa-camera-retro:before{content:"\f083"}.fa-key:before{content:"\f084"}.fa-gears:before,.fa-cogs:before{content:"\f085"}.fa-comments:before{content:"\f086"}.fa-thumbs-o-up:before{content:"\f087"}.fa-thumbs-o-down:before{content:"\f088"}.fa-star-half:before{content:"\f089"}.fa-heart-o:before{content:"\f08a"}.fa-sign-out:before{content:"\f08b"}.fa-linkedin-square:before{content:"\f08c"}.fa-thumb-tack:before{content:"\f08d"}.fa-external-link:before{content:"\f08e"}.fa-sign-in:before{content:"\f090"}.fa-trophy:before{content:"\f091"}.fa-github-square:before{content:"\f092"}.fa-upload:before{content:"\f093"}.fa-lemon-o:before{content:"\f094"}.fa-phone:before{content:"\f095"}.fa-square-o:before{content:"\f096"}.fa-bookmark-o:before{content:"\f097"}.fa-phone-square:before{content:"\f098"}.fa-twitter:before{content:"\f099"}.fa-facebook-f:before,.fa-facebook:before{content:"\f09a"}.fa-github:before{content:"\f09b"}.fa-unlock:before{content:"\f09c"}.fa-credit-card:before{content:"\f09d"}.fa-feed:before,.fa-rss:before{content:"\f09e"}.fa-hdd-o:before{content:"\f0a0"}.fa-bullhorn:before{content:"\f0a1"}.fa-bell:before{content:"\f0f3"}.fa-certificate:before{content:"\f0a3"}.fa-hand-o-right:before{content:"\f0a4"}.fa-hand-o-left:before{content:"\f0a5"}.fa-hand-o-up:before{content:"\f0a6"}.fa-hand-o-down:before{content:"\f0a7"}.fa-arrow-circle-left:before{content:"\f0a8"}.fa-arrow-circle-right:before{content:"\f0a9"}.fa-arrow-circle-up:before{content:"\f0aa"}.fa-arrow-circle-down:before{content:"\f0ab"}.fa-globe:before{content:"\f0ac"}.fa-wrench:before{content:"\f0ad"}.fa-tasks:before{content:"\f0ae"}.fa-filter:before{content:"\f0b0"}.fa-briefcase:before{content:"\f0b1"}.fa-arrows-alt:before{content:"\f0b2"}.fa-group:before,.fa-users:before{content:"\f0c0"}.fa-chain:before,.fa-link:before{content:"\f0c1"}.fa-cloud:before{content:"\f0c2"}.fa-flask:before{content:"\f0c3"}.fa-cut:before,.fa-scissors:before{content:"\f0c4"}.fa-copy:before,.fa-files-o:before{content:"\f0c5"}.fa-paperclip:before{content:"\f0c6"}.fa-save:before,.fa-floppy-o:before{content:"\f0c7"}.fa-square:before{content:"\f0c8"}.fa-navicon:before,.fa-reorder:before,.fa-bars:before{content:"\f0c9"}.fa-list-ul:before{content:"\f0ca"}.fa-list-ol:before{content:"\f0cb"}.fa-strikethrough:before{content:"\f0cc"}.fa-underline:before{content:"\f0cd"}.fa-table:before{content:"\f0ce"}.fa-magic:before{content:"\f0d0"}.fa-truck:before{content:"\f0d1"}.fa-pinterest:before{content:"\f0d2"}.fa-pinterest-square:before{content:"\f0d3"}.fa-google-plus-square:before{content:"\f0d4"}.fa-google-plus:before{content:"\f0d5"}.fa-money:before{content:"\f0d6"}.fa-caret-down:before{content:"\f0d7"}.fa-caret-up:before{content:"\f0d8"}.fa-caret-left:before{content:"\f0d9"}.fa-caret-right:before{content:"\f0da"}.fa-columns:before{content:"\f0db"}.fa-unsorted:before,.fa-sort:before{content:"\f0dc"}.fa-sort-down:before,.fa-sort-desc:before{content:"\f0dd"}.fa-sort-up:before,.fa-sort-asc:before{content:"\f0de"}.fa-envelope:before{content:"\f0e0"}.fa-linkedin:before{content:"\f0e1"}.fa-rotate-left:before,.fa-undo:before{content:"\f0e2"}.fa-legal:before,.fa-gavel:before{content:"\f0e3"}.fa-dashboard:before,.fa-tachometer:before{content:"\f0e4"}.fa-comment-o:before{content:"\f0e5"}.fa-comments-o:before{content:"\f0e6"}.fa-flash:before,.fa-bolt:before{content:"\f0e7"}.fa-sitemap:before{content:"\f0e8"}.fa-umbrella:before{content:"\f0e9"}.fa-paste:before,.fa-clipboard:before{content:"\f0ea"}.fa-lightbulb-o:before{content:"\f0eb"}.fa-exchange:before{content:"\f0ec"}.fa-cloud-download:before{content:"\f0ed"}.fa-cloud-upload:before{content:"\f0ee"}.fa-user-md:before{content:"\f0f0"}.fa-stethoscope:before{content:"\f0f1"}.fa-suitcase:before{content:"\f0f2"}.fa-bell-o:before{content:"\f0a2"}.fa-coffee:before{content:"\f0f4"}.fa-cutlery:before{content:"\f0f5"}.fa-file-text-o:before{content:"\f0f6"}.fa-building-o:before{content:"\f0f7"}.fa-hospital-o:before{content:"\f0f8"}.fa-ambulance:before{content:"\f0f9"}.fa-medkit:before{content:"\f0fa"}.fa-fighter-jet:before{content:"\f0fb"}.fa-beer:before{content:"\f0fc"}.fa-h-square:before{content:"\f0fd"}.fa-plus-square:before{content:"\f0fe"}.fa-angle-double-left:before{content:"\f100"}.fa-angle-double-right:before{content:"\f101"}.fa-angle-double-up:before{content:"\f102"}.fa-angle-double-down:before{content:"\f103"}.fa-angle-left:before{content:"\f104"}.fa-angle-right:before{content:"\f105"}.fa-angle-up:before{content:"\f106"}.fa-angle-down:before{content:"\f107"}.fa-desktop:before{content:"\f108"}.fa-laptop:before{content:"\f109"}.fa-tablet:before{content:"\f10a"}.fa-mobile-phone:before,.fa-mobile:before{content:"\f10b"}.fa-circle-o:before{content:"\f10c"}.fa-quote-left:before{content:"\f10d"}.fa-quote-right:before{content:"\f10e"}.fa-spinner:before{content:"\f110"}.fa-circle:before{content:"\f111"}.fa-mail-reply:before,.fa-reply:before{content:"\f112"}.fa-github-alt:before{content:"\f113"}.fa-folder-o:before{content:"\f114"}.fa-folder-open-o:before{content:"\f115"}.fa-smile-o:before{content:"\f118"}.fa-frown-o:before{content:"\f119"}.fa-meh-o:before{content:"\f11a"}.fa-gamepad:before{content:"\f11b"}.fa-keyboard-o:before{content:"\f11c"}.fa-flag-o:before{content:"\f11d"}.fa-flag-checkered:before{content:"\f11e"}.fa-terminal:before{content:"\f120"}.fa-code:before{content:"\f121"}.fa-mail-reply-all:before,.fa-reply-all:before{content:"\f122"}.fa-star-half-empty:before,.fa-star-half-full:before,.fa-star-half-o:before{content:"\f123"}.fa-location-arrow:before{content:"\f124"}.fa-crop:before{content:"\f125"}.fa-code-fork:before{content:"\f126"}.fa-unlink:before,.fa-chain-broken:before{content:"\f127"}.fa-question:before{content:"\f128"}.fa-info:before{content:"\f129"}.fa-exclamation:before{content:"\f12a"}.fa-superscript:before{content:"\f12b"}.fa-subscript:before{content:"\f12c"}.fa-eraser:before{content:"\f12d"}.fa-puzzle-piece:before{content:"\f12e"}.fa-microphone:before{content:"\f130"}.fa-microphone-slash:before{content:"\f131"}.fa-shield:before{content:"\f132"}.fa-calendar-o:before{content:"\f133"}.fa-fire-extinguisher:before{content:"\f134"}.fa-rocket:before{content:"\f135"}.fa-maxcdn:before{content:"\f136"}.fa-chevron-circle-left:before{content:"\f137"}.fa-chevron-circle-right:before{content:"\f138"}.fa-chevron-circle-up:before{content:"\f139"}.fa-chevron-circle-down:before{content:"\f13a"}.fa-html5:before{content:"\f13b"}.fa-css3:before{content:"\f13c"}.fa-anchor:before{content:"\f13d"}.fa-unlock-alt:before{content:"\f13e"}.fa-bullseye:before{content:"\f140"}.fa-ellipsis-h:before{content:"\f141"}.fa-ellipsis-v:before{content:"\f142"}.fa-rss-square:before{content:"\f143"}.fa-play-circle:before{content:"\f144"}.fa-ticket:before{content:"\f145"}.fa-minus-square:before{content:"\f146"}.fa-minus-square-o:before{content:"\f147"}.fa-level-up:before{content:"\f148"}.fa-level-down:before{content:"\f149"}.fa-check-square:before{content:"\f14a"}.fa-pencil-square:before{content:"\f14b"}.fa-external-link-square:before{content:"\f14c"}.fa-share-square:before{content:"\f14d"}.fa-compass:before{content:"\f14e"}.fa-toggle-down:before,.fa-caret-square-o-down:before{content:"\f150"}.fa-toggle-up:before,.fa-caret-square-o-up:before{content:"\f151"}.fa-toggle-right:before,.fa-caret-square-o-right:before{content:"\f152"}.fa-euro:before,.fa-eur:before{content:"\f153"}.fa-gbp:before{content:"\f154"}.fa-dollar:before,.fa-usd:before{content:"\f155"}.fa-rupee:before,.fa-inr:before{content:"\f156"}.fa-cny:before,.fa-rmb:before,.fa-yen:before,.fa-jpy:before{content:"\f157"}.fa-ruble:before,.fa-rouble:before,.fa-rub:before{content:"\f158"}.fa-won:before,.fa-krw:before{content:"\f159"}.fa-bitcoin:before,.fa-btc:before{content:"\f15a"}.fa-file:before{content:"\f15b"}.fa-file-text:before{content:"\f15c"}.fa-sort-alpha-asc:before{content:"\f15d"}.fa-sort-alpha-desc:before{content:"\f15e"}.fa-sort-amount-asc:before{content:"\f160"}.fa-sort-amount-desc:before{content:"\f161"}.fa-sort-numeric-asc:before{content:"\f162"}.fa-sort-numeric-desc:before{content:"\f163"}.fa-thumbs-up:before{content:"\f164"}.fa-thumbs-down:before{content:"\f165"}.fa-youtube-square:before{content:"\f166"}.fa-youtube:before{content:"\f167"}.fa-xing:before{content:"\f168"}.fa-xing-square:before{content:"\f169"}.fa-youtube-play:before{content:"\f16a"}.fa-dropbox:before{content:"\f16b"}.fa-stack-overflow:before{content:"\f16c"}.fa-instagram:before{content:"\f16d"}.fa-flickr:before{content:"\f16e"}.fa-adn:before{content:"\f170"}.fa-bitbucket:before{content:"\f171"}.fa-bitbucket-square:before{content:"\f172"}.fa-tumblr:before{content:"\f173"}.fa-tumblr-square:before{content:"\f174"}.fa-long-arrow-down:before{content:"\f175"}.fa-long-arrow-up:before{content:"\f176"}.fa-long-arrow-left:before{content:"\f177"}.fa-long-arrow-right:before{content:"\f178"}.fa-apple:before{content:"\f179"}.fa-windows:before{content:"\f17a"}.fa-android:before{content:"\f17b"}.fa-linux:before{content:"\f17c"}.fa-dribbble:before{content:"\f17d"}.fa-skype:before{content:"\f17e"}.fa-foursquare:before{content:"\f180"}.fa-trello:before{content:"\f181"}.fa-female:before{content:"\f182"}.fa-male:before{content:"\f183"}.fa-gittip:before,.fa-gratipay:before{content:"\f184"}.fa-sun-o:before{content:"\f185"}.fa-moon-o:before{content:"\f186"}.fa-archive:before{content:"\f187"}.fa-bug:before{content:"\f188"}.fa-vk:before{content:"\f189"}.fa-weibo:before{content:"\f18a"}.fa-renren:before{content:"\f18b"}.fa-pagelines:before{content:"\f18c"}.fa-stack-exchange:before{content:"\f18d"}.fa-arrow-circle-o-right:before{content:"\f18e"}.fa-arrow-circle-o-left:before{content:"\f190"}.fa-toggle-left:before,.fa-caret-square-o-left:before{content:"\f191"}.fa-dot-circle-o:before{content:"\f192"}.fa-wheelchair:before{content:"\f193"}.fa-vimeo-square:before{content:"\f194"}.fa-turkish-lira:before,.fa-try:before{content:"\f195"}.fa-plus-square-o:before{content:"\f196"}.fa-space-shuttle:before{content:"\f197"}.fa-slack:before{content:"\f198"}.fa-envelope-square:before{content:"\f199"}.fa-wordpress:before{content:"\f19a"}.fa-openid:before{content:"\f19b"}.fa-institution:before,.fa-bank:before,.fa-university:before{content:"\f19c"}.fa-mortar-board:before,.fa-graduation-cap:before{content:"\f19d"}.fa-yahoo:before{content:"\f19e"}.fa-google:before{content:"\f1a0"}.fa-reddit:before{content:"\f1a1"}.fa-reddit-square:before{content:"\f1a2"}.fa-stumbleupon-circle:before{content:"\f1a3"}.fa-stumbleupon:before{content:"\f1a4"}.fa-delicious:before{content:"\f1a5"}.fa-digg:before{content:"\f1a6"}.fa-pied-piper-pp:before{content:"\f1a7"}.fa-pied-piper-alt:before{content:"\f1a8"}.fa-drupal:before{content:"\f1a9"}.fa-joomla:before{content:"\f1aa"}.fa-language:before{content:"\f1ab"}.fa-fax:before{content:"\f1ac"}.fa-building:before{content:"\f1ad"}.fa-child:before{content:"\f1ae"}.fa-paw:before{content:"\f1b0"}.fa-spoon:before{content:"\f1b1"}.fa-cube:before{content:"\f1b2"}.fa-cubes:before{content:"\f1b3"}.fa-behance:before{content:"\f1b4"}.fa-behance-square:before{content:"\f1b5"}.fa-steam:before{content:"\f1b6"}.fa-steam-square:before{content:"\f1b7"}.fa-recycle:before{content:"\f1b8"}.fa-automobile:before,.fa-car:before{content:"\f1b9"}.fa-cab:before,.fa-taxi:before{content:"\f1ba"}.fa-tree:before{content:"\f1bb"}.fa-spotify:before{content:"\f1bc"}.fa-deviantart:before{content:"\f1bd"}.fa-soundcloud:before{content:"\f1be"}.fa-database:before{content:"\f1c0"}.fa-file-pdf-o:before{content:"\f1c1"}.fa-file-word-o:before{content:"\f1c2"}.fa-file-excel-o:before{content:"\f1c3"}.fa-file-powerpoint-o:before{content:"\f1c4"}.fa-file-photo-o:before,.fa-file-picture-o:before,.fa-file-image-o:before{content:"\f1c5"}.fa-file-zip-o:before,.fa-file-archive-o:before{content:"\f1c6"}.fa-file-sound-o:before,.fa-file-audio-o:before{content:"\f1c7"}.fa-file-movie-o:before,.fa-file-video-o:before{content:"\f1c8"}.fa-file-code-o:before{content:"\f1c9"}.fa-vine:before{content:"\f1ca"}.fa-codepen:before{content:"\f1cb"}.fa-jsfiddle:before{content:"\f1cc"}.fa-life-bouy:before,.fa-life-buoy:before,.fa-life-saver:before,.fa-support:before,.fa-life-ring:before{content:"\f1cd"}.fa-circle-o-notch:before{content:"\f1ce"}.fa-ra:before,.fa-resistance:before,.fa-rebel:before{content:"\f1d0"}.fa-ge:before,.fa-empire:before{content:"\f1d1"}.fa-git-square:before{content:"\f1d2"}.fa-git:before{content:"\f1d3"}.fa-y-combinator-square:before,.fa-yc-square:before,.fa-hacker-news:before{content:"\f1d4"}.fa-tencent-weibo:before{content:"\f1d5"}.fa-qq:before{content:"\f1d6"}.fa-wechat:before,.fa-weixin:before{content:"\f1d7"}.fa-send:before,.fa-paper-plane:before{content:"\f1d8"}.fa-send-o:before,.fa-paper-plane-o:before{content:"\f1d9"}.fa-history:before{content:"\f1da"}.fa-circle-thin:before{content:"\f1db"}.fa-header:before{content:"\f1dc"}.fa-paragraph:before{content:"\f1dd"}.fa-sliders:before{content:"\f1de"}.fa-share-alt:before{content:"\f1e0"}.fa-share-alt-square:before{content:"\f1e1"}.fa-bomb:before{content:"\f1e2"}.fa-soccer-ball-o:before,.fa-futbol-o:before{content:"\f1e3"}.fa-tty:before{content:"\f1e4"}.fa-binoculars:before{content:"\f1e5"}.fa-plug:before{content:"\f1e6"}.fa-slideshare:before{content:"\f1e7"}.fa-twitch:before{content:"\f1e8"}.fa-yelp:before{content:"\f1e9"}.fa-newspaper-o:before{content:"\f1ea"}.fa-wifi:before{content:"\f1eb"}.fa-calculator:before{content:"\f1ec"}.fa-paypal:before{content:"\f1ed"}.fa-google-wallet:before{content:"\f1ee"}.fa-cc-visa:before{content:"\f1f0"}.fa-cc-mastercard:before{content:"\f1f1"}.fa-cc-discover:before{content:"\f1f2"}.fa-cc-amex:before{content:"\f1f3"}.fa-cc-paypal:before{content:"\f1f4"}.fa-cc-stripe:before{content:"\f1f5"}.fa-bell-slash:before{content:"\f1f6"}.fa-bell-slash-o:before{content:"\f1f7"}.fa-trash:before{content:"\f1f8"}.fa-copyright:before{content:"\f1f9"}.fa-at:before{content:"\f1fa"}.fa-eyedropper:before{content:"\f1fb"}.fa-paint-brush:before{content:"\f1fc"}.fa-birthday-cake:before{content:"\f1fd"}.fa-area-chart:before{content:"\f1fe"}.fa-pie-chart:before{content:"\f200"}.fa-line-chart:before{content:"\f201"}.fa-lastfm:before{content:"\f202"}.fa-lastfm-square:before{content:"\f203"}.fa-toggle-off:before{content:"\f204"}.fa-toggle-on:before{content:"\f205"}.fa-bicycle:before{content:"\f206"}.fa-bus:before{content:"\f207"}.fa-ioxhost:before{content:"\f208"}.fa-angellist:before{content:"\f209"}.fa-cc:before{content:"\f20a"}.fa-shekel:before,.fa-sheqel:before,.fa-ils:before{content:"\f20b"}.fa-meanpath:before{content:"\f20c"}.fa-buysellads:before{content:"\f20d"}.fa-connectdevelop:before{content:"\f20e"}.fa-dashcube:before{content:"\f210"}.fa-forumbee:before{content:"\f211"}.fa-leanpub:before{content:"\f212"}.fa-sellsy:before{content:"\f213"}.fa-shirtsinbulk:before{content:"\f214"}.fa-simplybuilt:before{content:"\f215"}.fa-skyatlas:before{content:"\f216"}.fa-cart-plus:before{content:"\f217"}.fa-cart-arrow-down:before{content:"\f218"}.fa-diamond:before{content:"\f219"}.fa-ship:before{content:"\f21a"}.fa-user-secret:before{content:"\f21b"}.fa-motorcycle:before{content:"\f21c"}.fa-street-view:before{content:"\f21d"}.fa-heartbeat:before{content:"\f21e"}.fa-venus:before{content:"\f221"}.fa-mars:before{content:"\f222"}.fa-mercury:before{content:"\f223"}.fa-intersex:before,.fa-transgender:before{content:"\f224"}.fa-transgender-alt:before{content:"\f225"}.fa-venus-double:before{content:"\f226"}.fa-mars-double:before{content:"\f227"}.fa-venus-mars:before{content:"\f228"}.fa-mars-stroke:before{content:"\f229"}.fa-mars-stroke-v:before{content:"\f22a"}.fa-mars-stroke-h:before{content:"\f22b"}.fa-neuter:before{content:"\f22c"}.fa-genderless:before{content:"\f22d"}.fa-facebook-official:before{content:"\f230"}.fa-pinterest-p:before{content:"\f231"}.fa-whatsapp:before{content:"\f232"}.fa-server:before{content:"\f233"}.fa-user-plus:before{content:"\f234"}.fa-user-times:before{content:"\f235"}.fa-hotel:before,.fa-bed:before{content:"\f236"}.fa-viacoin:before{content:"\f237"}.fa-train:before{content:"\f238"}.fa-subway:before{content:"\f239"}.fa-medium:before{content:"\f23a"}.fa-yc:before,.fa-y-combinator:before{content:"\f23b"}.fa-optin-monster:before{content:"\f23c"}.fa-opencart:before{content:"\f23d"}.fa-expeditedssl:before{content:"\f23e"}.fa-battery-4:before,.fa-battery-full:before{content:"\f240"}.fa-battery-3:before,.fa-battery-three-quarters:before{content:"\f241"}.fa-battery-2:before,.fa-battery-half:before{content:"\f242"}.fa-battery-1:before,.fa-battery-quarter:before{content:"\f243"}.fa-battery-0:before,.fa-battery-empty:before{content:"\f244"}.fa-mouse-pointer:before{content:"\f245"}.fa-i-cursor:before{content:"\f246"}.fa-object-group:before{content:"\f247"}.fa-object-ungroup:before{content:"\f248"}.fa-sticky-note:before{content:"\f249"}.fa-sticky-note-o:before{content:"\f24a"}.fa-cc-jcb:before{content:"\f24b"}.fa-cc-diners-club:before{content:"\f24c"}.fa-clone:before{content:"\f24d"}.fa-balance-scale:before{content:"\f24e"}.fa-hourglass-o:before{content:"\f250"}.fa-hourglass-1:before,.fa-hourglass-start:before{content:"\f251"}.fa-hourglass-2:before,.fa-hourglass-half:before{content:"\f252"}.fa-hourglass-3:before,.fa-hourglass-end:before{content:"\f253"}.fa-hourglass:before{content:"\f254"}.fa-hand-grab-o:before,.fa-hand-rock-o:before{content:"\f255"}.fa-hand-stop-o:before,.fa-hand-paper-o:before{content:"\f256"}.fa-hand-scissors-o:before{content:"\f257"}.fa-hand-lizard-o:before{content:"\f258"}.fa-hand-spock-o:before{content:"\f259"}.fa-hand-pointer-o:before{content:"\f25a"}.fa-hand-peace-o:before{content:"\f25b"}.fa-trademark:before{content:"\f25c"}.fa-registered:before{content:"\f25d"}.fa-creative-commons:before{content:"\f25e"}.fa-gg:before{content:"\f260"}.fa-gg-circle:before{content:"\f261"}.fa-tripadvisor:before{content:"\f262"}.fa-odnoklassniki:before{content:"\f263"}.fa-odnoklassniki-square:before{content:"\f264"}.fa-get-pocket:before{content:"\f265"}.fa-wikipedia-w:before{content:"\f266"}.fa-safari:before{content:"\f267"}.fa-chrome:before{content:"\f268"}.fa-firefox:before{content:"\f269"}.fa-opera:before{content:"\f26a"}.fa-internet-explorer:before{content:"\f26b"}.fa-tv:before,.fa-television:before{content:"\f26c"}.fa-contao:before{content:"\f26d"}.fa-500px:before{content:"\f26e"}.fa-amazon:before{content:"\f270"}.fa-calendar-plus-o:before{content:"\f271"}.fa-calendar-minus-o:before{content:"\f272"}.fa-calendar-times-o:before{content:"\f273"}.fa-calendar-check-o:before{content:"\f274"}.fa-industry:before{content:"\f275"}.fa-map-pin:before{content:"\f276"}.fa-map-signs:before{content:"\f277"}.fa-map-o:before{content:"\f278"}.fa-map:before{content:"\f279"}.fa-commenting:before{content:"\f27a"}.fa-commenting-o:before{content:"\f27b"}.fa-houzz:before{content:"\f27c"}.fa-vimeo:before{content:"\f27d"}.fa-black-tie:before{content:"\f27e"}.fa-fonticons:before{content:"\f280"}.fa-reddit-alien:before{content:"\f281"}.fa-edge:before{content:"\f282"}.fa-credit-card-alt:before{content:"\f283"}.fa-codiepie:before{content:"\f284"}.fa-modx:before{content:"\f285"}.fa-fort-awesome:before{content:"\f286"}.fa-usb:before{content:"\f287"}.fa-product-hunt:before{content:"\f288"}.fa-mixcloud:before{content:"\f289"}.fa-scribd:before{content:"\f28a"}.fa-pause-circle:before{content:"\f28b"}.fa-pause-circle-o:before{content:"\f28c"}.fa-stop-circle:before{content:"\f28d"}.fa-stop-circle-o:before{content:"\f28e"}.fa-shopping-bag:before{content:"\f290"}.fa-shopping-basket:before{content:"\f291"}.fa-hashtag:before{content:"\f292"}.fa-bluetooth:before{content:"\f293"}.fa-bluetooth-b:before{content:"\f294"}.fa-percent:before{content:"\f295"}.fa-gitlab:before{content:"\f296"}.fa-wpbeginner:before{content:"\f297"}.fa-wpforms:before{content:"\f298"}.fa-envira:before{content:"\f299"}.fa-universal-access:before{content:"\f29a"}.fa-wheelchair-alt:before{content:"\f29b"}.fa-question-circle-o:before{content:"\f29c"}.fa-blind:before{content:"\f29d"}.fa-audio-description:before{content:"\f29e"}.fa-volume-control-phone:before{content:"\f2a0"}.fa-braille:before{content:"\f2a1"}.fa-assistive-listening-systems:before{content:"\f2a2"}.fa-asl-interpreting:before,.fa-american-sign-language-interpreting:before{content:"\f2a3"}.fa-deafness:before,.fa-hard-of-hearing:before,.fa-deaf:before{content:"\f2a4"}.fa-glide:before{content:"\f2a5"}.fa-glide-g:before{content:"\f2a6"}.fa-signing:before,.fa-sign-language:before{content:"\f2a7"}.fa-low-vision:before{content:"\f2a8"}.fa-viadeo:before{content:"\f2a9"}.fa-viadeo-square:before{content:"\f2aa"}.fa-snapchat:before{content:"\f2ab"}.fa-snapchat-ghost:before{content:"\f2ac"}.fa-snapchat-square:before{content:"\f2ad"}.fa-pied-piper:before{content:"\f2ae"}.fa-first-order:before{content:"\f2b0"}.fa-yoast:before{content:"\f2b1"}.fa-themeisle:before{content:"\f2b2"}.fa-google-plus-circle:before,.fa-google-plus-official:before{content:"\f2b3"}.fa-fa:before,.fa-font-awesome:before{content:"\f2b4"}.sr-only{position:absolute;width:1px;height:1px;padding:0;margin:-1px;overflow:hidden;clip:rect(0, 0, 0, 0);border:0}.sr-only-focusable:active,.sr-only-focusable:focus{position:static;width:auto;height:auto;margin:0;overflow:visible;clip:auto} diff --git a/assets/css/ie8.css b/assets/css/ie8.css new file mode 100755 index 00000000..da702866 --- /dev/null +++ b/assets/css/ie8.css @@ -0,0 +1,74 @@ +/* + Strata by HTML5 UP + html5up.net | @ajlkn + Free for personal and commercial use under the CCA 3.0 license (html5up.net/license) +*/ + +/* Button */ + + input[type="submit"], + input[type="reset"], + input[type="button"], + .button { + position: relative; + -ms-behavior: url("assets/js/ie/PIE.htc"); + } + +/* Form */ + + input[type="text"], + input[type="password"], + input[type="email"], + select, + textarea { + position: relative; + -ms-behavior: url("assets/js/ie/PIE.htc"); + } + + input[type="text"], + input[type="password"], + input[type="email"], + select { + height: 2.75em; + line-height: 2.75em; + } + + input[type="checkbox"] + label:before, + input[type="radio"] + label:before { + display: none; + } + +/* Image */ + + .image { + position: relative; + -ms-behavior: url("assets/js/ie/PIE.htc"); + } + + .image:before, .image:after { + display: none !important; + } + + .image img { + position: relative; + -ms-behavior: url("assets/js/ie/PIE.htc"); + } + +/* Header */ + + #header { + background-image: url("../../images/bg.jpg"); + background-repeat: no-repeat; + background-size: cover; + -ms-behavior: url("assets/js/ie/backgroundsize.min.htc"); + } + + #header h1 { + color: #ffffff; + } + +/* Footer */ + + #footer .icons a { + color: #ffffff; + } \ No newline at end of file diff --git a/assets/css/images/overlay.png b/assets/css/images/overlay.png new file mode 100755 index 00000000..93893016 Binary files /dev/null and b/assets/css/images/overlay.png differ diff --git a/assets/css/main.css b/assets/css/main.css new file mode 100755 index 00000000..c96134b4 --- /dev/null +++ b/assets/css/main.css @@ -0,0 +1,3132 @@ +@import url("font-awesome.min.css"); +@import url("https://fonts.googleapis.com/css?family=Source+Sans+Pro:400,400italic"); + +/* + Strata by HTML5 UP + html5up.net | @ajlkn + Free for personal and commercial use under the CCA 3.0 license (html5up.net/license) +*/ + +/* Reset */ + + html, body, div, span, applet, object, iframe, h1, h2, h3, h4, h5, h6, p, blockquote, pre, a, abbr, acronym, address, big, cite, code, del, dfn, em, img, ins, kbd, q, s, samp, small, strike, strong, sub, sup, tt, var, b, u, i, center, dl, dt, dd, ol, ul, li, fieldset, form, label, legend, table, caption, tbody, tfoot, thead, tr, th, td, article, aside, canvas, details, embed, figure, figcaption, footer, header, hgroup, menu, nav, output, ruby, section, summary, time, mark, audio, video { + margin: 0; + padding: 0; + border: 0; + font-size: 100%; + font: inherit; + vertical-align: baseline; + } + + article, aside, details, figcaption, figure, footer, header, hgroup, menu, nav, section { + display: block; + } + + body { + line-height: 1; + } + + ol, ul { + list-style: none; + } + + blockquote, q { + quotes: none; + } + + blockquote:before, blockquote:after, q:before, q:after { + content: ''; + content: none; + } + + table { + border-collapse: collapse; + border-spacing: 0; + } + + body { + -webkit-text-size-adjust: none; + } + +/* Box Model */ + + *, *:before, *:after { + -moz-box-sizing: border-box; + -webkit-box-sizing: border-box; + box-sizing: border-box; + } + +/* Containers */ + + .container { + margin-left: auto; + margin-right: auto; + } + + .container.\31 25\25 { + width: 100%; + max-width: 1200px; + min-width: 960px; + } + + .container.\37 5\25 { + width: 720px; + } + + .container.\35 0\25 { + width: 480px; + } + + .container.\32 5\25 { + width: 240px; + } + + .container { + width: 960px; + } + + @media screen and (max-width: 1800px) { + + .container.\31 25\25 { + width: 100%; + max-width: 1200px; + min-width: 960px; + } + + .container.\37 5\25 { + width: 720px; + } + + .container.\35 0\25 { + width: 480px; + } + + .container.\32 5\25 { + width: 240px; + } + + .container { + width: 960px; + } + + } + + @media screen and (max-width: 1280px) { + + .container.\31 25\25 { + width: 100%; + max-width: 1200px; + min-width: 960px; + } + + .container.\37 5\25 { + width: 720px; + } + + .container.\35 0\25 { + width: 480px; + } + + .container.\32 5\25 { + width: 240px; + } + + .container { + width: 960px; + } + + } + + @media screen and (max-width: 980px) { + + .container.\31 25\25 { + width: 100%; + max-width: 1200px; + min-width: 960px; + } + + .container.\37 5\25 { + width: 720px; + } + + .container.\35 0\25 { + width: 480px; + } + + .container.\32 5\25 { + width: 240px; + } + + .container { + width: 960px; + } + + } + + @media screen and (max-width: 736px) { + + .container.\31 25\25 { + width: 100%; + max-width: 1200px; + min-width: 960px; + } + + .container.\37 5\25 { + width: 720px; + } + + .container.\35 0\25 { + width: 480px; + } + + .container.\32 5\25 { + width: 240px; + } + + .container { + width: 960px; + } + + } + + @media screen and (max-width: 480px) { + + .container.\31 25\25 { + width: 100%; + max-width: 1200px; + min-width: 960px; + } + + .container.\37 5\25 { + width: 720px; + } + + .container.\35 0\25 { + width: 480px; + } + + .container.\32 5\25 { + width: 240px; + } + + .container { + width: 960px; + } + + } + +/* Grid */ + + .row { + border-bottom: solid 1px transparent; + -moz-box-sizing: border-box; + -webkit-box-sizing: border-box; + box-sizing: border-box; + } + + .row > * { + float: left; + -moz-box-sizing: border-box; + -webkit-box-sizing: border-box; + box-sizing: border-box; + } + + .row:after, .row:before { + content: ''; + display: block; + clear: both; + height: 0; + } + + .row.uniform > * > :first-child { + margin-top: 0; + } + + .row.uniform > * > :last-child { + margin-bottom: 0; + } + + .row.\30 \25 > * { + padding: 0 0 0 0em; + } + + .row.\30 \25 { + margin: 0 0 -1px 0em; + } + + .row.uniform.\30 \25 > * { + padding: 0em 0 0 0em; + } + + .row.uniform.\30 \25 { + margin: 0em 0 -1px 0em; + } + + .row > * { + padding: 0 0 0 2.5em; + } + + .row { + margin: 0 0 -1px -2.5em; + } + + .row.uniform > * { + padding: 2.5em 0 0 2.5em; + } + + .row.uniform { + margin: -2.5em 0 -1px -2.5em; + } + + .row.\32 00\25 > * { + padding: 0 0 0 5em; + } + + .row.\32 00\25 { + margin: 0 0 -1px -5em; + } + + .row.uniform.\32 00\25 > * { + padding: 5em 0 0 5em; + } + + .row.uniform.\32 00\25 { + margin: -5em 0 -1px -5em; + } + + .row.\31 50\25 > * { + padding: 0 0 0 3.75em; + } + + .row.\31 50\25 { + margin: 0 0 -1px -3.75em; + } + + .row.uniform.\31 50\25 > * { + padding: 3.75em 0 0 3.75em; + } + + .row.uniform.\31 50\25 { + margin: -3.75em 0 -1px -3.75em; + } + + .row.\35 0\25 > * { + padding: 0 0 0 1.25em; + } + + .row.\35 0\25 { + margin: 0 0 -1px -1.25em; + } + + .row.uniform.\35 0\25 > * { + padding: 1.25em 0 0 1.25em; + } + + .row.uniform.\35 0\25 { + margin: -1.25em 0 -1px -1.25em; + } + + .row.\32 5\25 > * { + padding: 0 0 0 0.625em; + } + + .row.\32 5\25 { + margin: 0 0 -1px -0.625em; + } + + .row.uniform.\32 5\25 > * { + padding: 0.625em 0 0 0.625em; + } + + .row.uniform.\32 5\25 { + margin: -0.625em 0 -1px -0.625em; + } + + .\31 2u, .\31 2u\24 { + width: 100%; + clear: none; + margin-left: 0; + } + + .\31 1u, .\31 1u\24 { + width: 91.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 0u, .\31 0u\24 { + width: 83.3333333333%; + clear: none; + margin-left: 0; + } + + .\39 u, .\39 u\24 { + width: 75%; + clear: none; + margin-left: 0; + } + + .\38 u, .\38 u\24 { + width: 66.6666666667%; + clear: none; + margin-left: 0; + } + + .\37 u, .\37 u\24 { + width: 58.3333333333%; + clear: none; + margin-left: 0; + } + + .\36 u, .\36 u\24 { + width: 50%; + clear: none; + margin-left: 0; + } + + .\35 u, .\35 u\24 { + width: 41.6666666667%; + clear: none; + margin-left: 0; + } + + .\34 u, .\34 u\24 { + width: 33.3333333333%; + clear: none; + margin-left: 0; + } + + .\33 u, .\33 u\24 { + width: 25%; + clear: none; + margin-left: 0; + } + + .\32 u, .\32 u\24 { + width: 16.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 u, .\31 u\24 { + width: 8.3333333333%; + clear: none; + margin-left: 0; + } + + .\31 2u\24 + *, + .\31 1u\24 + *, + .\31 0u\24 + *, + .\39 u\24 + *, + .\38 u\24 + *, + .\37 u\24 + *, + .\36 u\24 + *, + .\35 u\24 + *, + .\34 u\24 + *, + .\33 u\24 + *, + .\32 u\24 + *, + .\31 u\24 + * { + clear: left; + } + + .\-11u { + margin-left: 91.66667%; + } + + .\-10u { + margin-left: 83.33333%; + } + + .\-9u { + margin-left: 75%; + } + + .\-8u { + margin-left: 66.66667%; + } + + .\-7u { + margin-left: 58.33333%; + } + + .\-6u { + margin-left: 50%; + } + + .\-5u { + margin-left: 41.66667%; + } + + .\-4u { + margin-left: 33.33333%; + } + + .\-3u { + margin-left: 25%; + } + + .\-2u { + margin-left: 16.66667%; + } + + .\-1u { + margin-left: 8.33333%; + } + + @media screen and (max-width: 1800px) { + + .row > * { + padding: 0 0 0 2.5em; + } + + .row { + margin: 0 0 -1px -2.5em; + } + + .row.uniform > * { + padding: 2.5em 0 0 2.5em; + } + + .row.uniform { + margin: -2.5em 0 -1px -2.5em; + } + + .row.\32 00\25 > * { + padding: 0 0 0 5em; + } + + .row.\32 00\25 { + margin: 0 0 -1px -5em; + } + + .row.uniform.\32 00\25 > * { + padding: 5em 0 0 5em; + } + + .row.uniform.\32 00\25 { + margin: -5em 0 -1px -5em; + } + + .row.\31 50\25 > * { + padding: 0 0 0 3.75em; + } + + .row.\31 50\25 { + margin: 0 0 -1px -3.75em; + } + + .row.uniform.\31 50\25 > * { + padding: 3.75em 0 0 3.75em; + } + + .row.uniform.\31 50\25 { + margin: -3.75em 0 -1px -3.75em; + } + + .row.\35 0\25 > * { + padding: 0 0 0 1.25em; + } + + .row.\35 0\25 { + margin: 0 0 -1px -1.25em; + } + + .row.uniform.\35 0\25 > * { + padding: 1.25em 0 0 1.25em; + } + + .row.uniform.\35 0\25 { + margin: -1.25em 0 -1px -1.25em; + } + + .row.\32 5\25 > * { + padding: 0 0 0 0.625em; + } + + .row.\32 5\25 { + margin: 0 0 -1px -0.625em; + } + + .row.uniform.\32 5\25 > * { + padding: 0.625em 0 0 0.625em; + } + + .row.uniform.\32 5\25 { + margin: -0.625em 0 -1px -0.625em; + } + + .\31 2u\28xlarge\29, .\31 2u\24\28xlarge\29 { + width: 100%; + clear: none; + margin-left: 0; + } + + .\31 1u\28xlarge\29, .\31 1u\24\28xlarge\29 { + width: 91.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 0u\28xlarge\29, .\31 0u\24\28xlarge\29 { + width: 83.3333333333%; + clear: none; + margin-left: 0; + } + + .\39 u\28xlarge\29, .\39 u\24\28xlarge\29 { + width: 75%; + clear: none; + margin-left: 0; + } + + .\38 u\28xlarge\29, .\38 u\24\28xlarge\29 { + width: 66.6666666667%; + clear: none; + margin-left: 0; + } + + .\37 u\28xlarge\29, .\37 u\24\28xlarge\29 { + width: 58.3333333333%; + clear: none; + margin-left: 0; + } + + .\36 u\28xlarge\29, .\36 u\24\28xlarge\29 { + width: 50%; + clear: none; + margin-left: 0; + } + + .\35 u\28xlarge\29, .\35 u\24\28xlarge\29 { + width: 41.6666666667%; + clear: none; + margin-left: 0; + } + + .\34 u\28xlarge\29, .\34 u\24\28xlarge\29 { + width: 33.3333333333%; + clear: none; + margin-left: 0; + } + + .\33 u\28xlarge\29, .\33 u\24\28xlarge\29 { + width: 25%; + clear: none; + margin-left: 0; + } + + .\32 u\28xlarge\29, .\32 u\24\28xlarge\29 { + width: 16.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 u\28xlarge\29, .\31 u\24\28xlarge\29 { + width: 8.3333333333%; + clear: none; + margin-left: 0; + } + + .\31 2u\24\28xlarge\29 + *, + .\31 1u\24\28xlarge\29 + *, + .\31 0u\24\28xlarge\29 + *, + .\39 u\24\28xlarge\29 + *, + .\38 u\24\28xlarge\29 + *, + .\37 u\24\28xlarge\29 + *, + .\36 u\24\28xlarge\29 + *, + .\35 u\24\28xlarge\29 + *, + .\34 u\24\28xlarge\29 + *, + .\33 u\24\28xlarge\29 + *, + .\32 u\24\28xlarge\29 + *, + .\31 u\24\28xlarge\29 + * { + clear: left; + } + + .\-11u\28xlarge\29 { + margin-left: 91.66667%; + } + + .\-10u\28xlarge\29 { + margin-left: 83.33333%; + } + + .\-9u\28xlarge\29 { + margin-left: 75%; + } + + .\-8u\28xlarge\29 { + margin-left: 66.66667%; + } + + .\-7u\28xlarge\29 { + margin-left: 58.33333%; + } + + .\-6u\28xlarge\29 { + margin-left: 50%; + } + + .\-5u\28xlarge\29 { + margin-left: 41.66667%; + } + + .\-4u\28xlarge\29 { + margin-left: 33.33333%; + } + + .\-3u\28xlarge\29 { + margin-left: 25%; + } + + .\-2u\28xlarge\29 { + margin-left: 16.66667%; + } + + .\-1u\28xlarge\29 { + margin-left: 8.33333%; + } + + } + + @media screen and (max-width: 1280px) { + + .row > * { + padding: 0 0 0 2em; + } + + .row { + margin: 0 0 -1px -2em; + } + + .row.uniform > * { + padding: 2em 0 0 2em; + } + + .row.uniform { + margin: -2em 0 -1px -2em; + } + + .row.\32 00\25 > * { + padding: 0 0 0 4em; + } + + .row.\32 00\25 { + margin: 0 0 -1px -4em; + } + + .row.uniform.\32 00\25 > * { + padding: 4em 0 0 4em; + } + + .row.uniform.\32 00\25 { + margin: -4em 0 -1px -4em; + } + + .row.\31 50\25 > * { + padding: 0 0 0 3em; + } + + .row.\31 50\25 { + margin: 0 0 -1px -3em; + } + + .row.uniform.\31 50\25 > * { + padding: 3em 0 0 3em; + } + + .row.uniform.\31 50\25 { + margin: -3em 0 -1px -3em; + } + + .row.\35 0\25 > * { + padding: 0 0 0 1em; + } + + .row.\35 0\25 { + margin: 0 0 -1px -1em; + } + + .row.uniform.\35 0\25 > * { + padding: 1em 0 0 1em; + } + + .row.uniform.\35 0\25 { + margin: -1em 0 -1px -1em; + } + + .row.\32 5\25 > * { + padding: 0 0 0 0.5em; + } + + .row.\32 5\25 { + margin: 0 0 -1px -0.5em; + } + + .row.uniform.\32 5\25 > * { + padding: 0.5em 0 0 0.5em; + } + + .row.uniform.\32 5\25 { + margin: -0.5em 0 -1px -0.5em; + } + + .\31 2u\28large\29, .\31 2u\24\28large\29 { + width: 100%; + clear: none; + margin-left: 0; + } + + .\31 1u\28large\29, .\31 1u\24\28large\29 { + width: 91.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 0u\28large\29, .\31 0u\24\28large\29 { + width: 83.3333333333%; + clear: none; + margin-left: 0; + } + + .\39 u\28large\29, .\39 u\24\28large\29 { + width: 75%; + clear: none; + margin-left: 0; + } + + .\38 u\28large\29, .\38 u\24\28large\29 { + width: 66.6666666667%; + clear: none; + margin-left: 0; + } + + .\37 u\28large\29, .\37 u\24\28large\29 { + width: 58.3333333333%; + clear: none; + margin-left: 0; + } + + .\36 u\28large\29, .\36 u\24\28large\29 { + width: 50%; + clear: none; + margin-left: 0; + } + + .\35 u\28large\29, .\35 u\24\28large\29 { + width: 41.6666666667%; + clear: none; + margin-left: 0; + } + + .\34 u\28large\29, .\34 u\24\28large\29 { + width: 33.3333333333%; + clear: none; + margin-left: 0; + } + + .\33 u\28large\29, .\33 u\24\28large\29 { + width: 25%; + clear: none; + margin-left: 0; + } + + .\32 u\28large\29, .\32 u\24\28large\29 { + width: 16.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 u\28large\29, .\31 u\24\28large\29 { + width: 8.3333333333%; + clear: none; + margin-left: 0; + } + + .\31 2u\24\28large\29 + *, + .\31 1u\24\28large\29 + *, + .\31 0u\24\28large\29 + *, + .\39 u\24\28large\29 + *, + .\38 u\24\28large\29 + *, + .\37 u\24\28large\29 + *, + .\36 u\24\28large\29 + *, + .\35 u\24\28large\29 + *, + .\34 u\24\28large\29 + *, + .\33 u\24\28large\29 + *, + .\32 u\24\28large\29 + *, + .\31 u\24\28large\29 + * { + clear: left; + } + + .\-11u\28large\29 { + margin-left: 91.66667%; + } + + .\-10u\28large\29 { + margin-left: 83.33333%; + } + + .\-9u\28large\29 { + margin-left: 75%; + } + + .\-8u\28large\29 { + margin-left: 66.66667%; + } + + .\-7u\28large\29 { + margin-left: 58.33333%; + } + + .\-6u\28large\29 { + margin-left: 50%; + } + + .\-5u\28large\29 { + margin-left: 41.66667%; + } + + .\-4u\28large\29 { + margin-left: 33.33333%; + } + + .\-3u\28large\29 { + margin-left: 25%; + } + + .\-2u\28large\29 { + margin-left: 16.66667%; + } + + .\-1u\28large\29 { + margin-left: 8.33333%; + } + + } + + @media screen and (max-width: 980px) { + + .row > * { + padding: 0 0 0 2em; + } + + .row { + margin: 0 0 -1px -2em; + } + + .row.uniform > * { + padding: 2em 0 0 2em; + } + + .row.uniform { + margin: -2em 0 -1px -2em; + } + + .row.\32 00\25 > * { + padding: 0 0 0 4em; + } + + .row.\32 00\25 { + margin: 0 0 -1px -4em; + } + + .row.uniform.\32 00\25 > * { + padding: 4em 0 0 4em; + } + + .row.uniform.\32 00\25 { + margin: -4em 0 -1px -4em; + } + + .row.\31 50\25 > * { + padding: 0 0 0 3em; + } + + .row.\31 50\25 { + margin: 0 0 -1px -3em; + } + + .row.uniform.\31 50\25 > * { + padding: 3em 0 0 3em; + } + + .row.uniform.\31 50\25 { + margin: -3em 0 -1px -3em; + } + + .row.\35 0\25 > * { + padding: 0 0 0 1em; + } + + .row.\35 0\25 { + margin: 0 0 -1px -1em; + } + + .row.uniform.\35 0\25 > * { + padding: 1em 0 0 1em; + } + + .row.uniform.\35 0\25 { + margin: -1em 0 -1px -1em; + } + + .row.\32 5\25 > * { + padding: 0 0 0 0.5em; + } + + .row.\32 5\25 { + margin: 0 0 -1px -0.5em; + } + + .row.uniform.\32 5\25 > * { + padding: 0.5em 0 0 0.5em; + } + + .row.uniform.\32 5\25 { + margin: -0.5em 0 -1px -0.5em; + } + + .\31 2u\28medium\29, .\31 2u\24\28medium\29 { + width: 100%; + clear: none; + margin-left: 0; + } + + .\31 1u\28medium\29, .\31 1u\24\28medium\29 { + width: 91.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 0u\28medium\29, .\31 0u\24\28medium\29 { + width: 83.3333333333%; + clear: none; + margin-left: 0; + } + + .\39 u\28medium\29, .\39 u\24\28medium\29 { + width: 75%; + clear: none; + margin-left: 0; + } + + .\38 u\28medium\29, .\38 u\24\28medium\29 { + width: 66.6666666667%; + clear: none; + margin-left: 0; + } + + .\37 u\28medium\29, .\37 u\24\28medium\29 { + width: 58.3333333333%; + clear: none; + margin-left: 0; + } + + .\36 u\28medium\29, .\36 u\24\28medium\29 { + width: 50%; + clear: none; + margin-left: 0; + } + + .\35 u\28medium\29, .\35 u\24\28medium\29 { + width: 41.6666666667%; + clear: none; + margin-left: 0; + } + + .\34 u\28medium\29, .\34 u\24\28medium\29 { + width: 33.3333333333%; + clear: none; + margin-left: 0; + } + + .\33 u\28medium\29, .\33 u\24\28medium\29 { + width: 25%; + clear: none; + margin-left: 0; + } + + .\32 u\28medium\29, .\32 u\24\28medium\29 { + width: 16.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 u\28medium\29, .\31 u\24\28medium\29 { + width: 8.3333333333%; + clear: none; + margin-left: 0; + } + + .\31 2u\24\28medium\29 + *, + .\31 1u\24\28medium\29 + *, + .\31 0u\24\28medium\29 + *, + .\39 u\24\28medium\29 + *, + .\38 u\24\28medium\29 + *, + .\37 u\24\28medium\29 + *, + .\36 u\24\28medium\29 + *, + .\35 u\24\28medium\29 + *, + .\34 u\24\28medium\29 + *, + .\33 u\24\28medium\29 + *, + .\32 u\24\28medium\29 + *, + .\31 u\24\28medium\29 + * { + clear: left; + } + + .\-11u\28medium\29 { + margin-left: 91.66667%; + } + + .\-10u\28medium\29 { + margin-left: 83.33333%; + } + + .\-9u\28medium\29 { + margin-left: 75%; + } + + .\-8u\28medium\29 { + margin-left: 66.66667%; + } + + .\-7u\28medium\29 { + margin-left: 58.33333%; + } + + .\-6u\28medium\29 { + margin-left: 50%; + } + + .\-5u\28medium\29 { + margin-left: 41.66667%; + } + + .\-4u\28medium\29 { + margin-left: 33.33333%; + } + + .\-3u\28medium\29 { + margin-left: 25%; + } + + .\-2u\28medium\29 { + margin-left: 16.66667%; + } + + .\-1u\28medium\29 { + margin-left: 8.33333%; + } + + } + + @media screen and (max-width: 736px) { + + .row > * { + padding: 0 0 0 1.5em; + } + + .row { + margin: 0 0 -1px -1.5em; + } + + .row.uniform > * { + padding: 1.5em 0 0 1.5em; + } + + .row.uniform { + margin: -1.5em 0 -1px -1.5em; + } + + .row.\32 00\25 > * { + padding: 0 0 0 3em; + } + + .row.\32 00\25 { + margin: 0 0 -1px -3em; + } + + .row.uniform.\32 00\25 > * { + padding: 3em 0 0 3em; + } + + .row.uniform.\32 00\25 { + margin: -3em 0 -1px -3em; + } + + .row.\31 50\25 > * { + padding: 0 0 0 2.25em; + } + + .row.\31 50\25 { + margin: 0 0 -1px -2.25em; + } + + .row.uniform.\31 50\25 > * { + padding: 2.25em 0 0 2.25em; + } + + .row.uniform.\31 50\25 { + margin: -2.25em 0 -1px -2.25em; + } + + .row.\35 0\25 > * { + padding: 0 0 0 0.75em; + } + + .row.\35 0\25 { + margin: 0 0 -1px -0.75em; + } + + .row.uniform.\35 0\25 > * { + padding: 0.75em 0 0 0.75em; + } + + .row.uniform.\35 0\25 { + margin: -0.75em 0 -1px -0.75em; + } + + .row.\32 5\25 > * { + padding: 0 0 0 0.375em; + } + + .row.\32 5\25 { + margin: 0 0 -1px -0.375em; + } + + .row.uniform.\32 5\25 > * { + padding: 0.375em 0 0 0.375em; + } + + .row.uniform.\32 5\25 { + margin: -0.375em 0 -1px -0.375em; + } + + .\31 2u\28small\29, .\31 2u\24\28small\29 { + width: 100%; + clear: none; + margin-left: 0; + } + + .\31 1u\28small\29, .\31 1u\24\28small\29 { + width: 91.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 0u\28small\29, .\31 0u\24\28small\29 { + width: 83.3333333333%; + clear: none; + margin-left: 0; + } + + .\39 u\28small\29, .\39 u\24\28small\29 { + width: 75%; + clear: none; + margin-left: 0; + } + + .\38 u\28small\29, .\38 u\24\28small\29 { + width: 66.6666666667%; + clear: none; + margin-left: 0; + } + + .\37 u\28small\29, .\37 u\24\28small\29 { + width: 58.3333333333%; + clear: none; + margin-left: 0; + } + + .\36 u\28small\29, .\36 u\24\28small\29 { + width: 50%; + clear: none; + margin-left: 0; + } + + .\35 u\28small\29, .\35 u\24\28small\29 { + width: 41.6666666667%; + clear: none; + margin-left: 0; + } + + .\34 u\28small\29, .\34 u\24\28small\29 { + width: 33.3333333333%; + clear: none; + margin-left: 0; + } + + .\33 u\28small\29, .\33 u\24\28small\29 { + width: 25%; + clear: none; + margin-left: 0; + } + + .\32 u\28small\29, .\32 u\24\28small\29 { + width: 16.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 u\28small\29, .\31 u\24\28small\29 { + width: 8.3333333333%; + clear: none; + margin-left: 0; + } + + .\31 2u\24\28small\29 + *, + .\31 1u\24\28small\29 + *, + .\31 0u\24\28small\29 + *, + .\39 u\24\28small\29 + *, + .\38 u\24\28small\29 + *, + .\37 u\24\28small\29 + *, + .\36 u\24\28small\29 + *, + .\35 u\24\28small\29 + *, + .\34 u\24\28small\29 + *, + .\33 u\24\28small\29 + *, + .\32 u\24\28small\29 + *, + .\31 u\24\28small\29 + * { + clear: left; + } + + .\-11u\28small\29 { + margin-left: 91.66667%; + } + + .\-10u\28small\29 { + margin-left: 83.33333%; + } + + .\-9u\28small\29 { + margin-left: 75%; + } + + .\-8u\28small\29 { + margin-left: 66.66667%; + } + + .\-7u\28small\29 { + margin-left: 58.33333%; + } + + .\-6u\28small\29 { + margin-left: 50%; + } + + .\-5u\28small\29 { + margin-left: 41.66667%; + } + + .\-4u\28small\29 { + margin-left: 33.33333%; + } + + .\-3u\28small\29 { + margin-left: 25%; + } + + .\-2u\28small\29 { + margin-left: 16.66667%; + } + + .\-1u\28small\29 { + margin-left: 8.33333%; + } + + } + + @media screen and (max-width: 480px) { + + .row > * { + padding: 0 0 0 1.5em; + } + + .row { + margin: 0 0 -1px -1.5em; + } + + .row.uniform > * { + padding: 1.5em 0 0 1.5em; + } + + .row.uniform { + margin: -1.5em 0 -1px -1.5em; + } + + .row.\32 00\25 > * { + padding: 0 0 0 3em; + } + + .row.\32 00\25 { + margin: 0 0 -1px -3em; + } + + .row.uniform.\32 00\25 > * { + padding: 3em 0 0 3em; + } + + .row.uniform.\32 00\25 { + margin: -3em 0 -1px -3em; + } + + .row.\31 50\25 > * { + padding: 0 0 0 2.25em; + } + + .row.\31 50\25 { + margin: 0 0 -1px -2.25em; + } + + .row.uniform.\31 50\25 > * { + padding: 2.25em 0 0 2.25em; + } + + .row.uniform.\31 50\25 { + margin: -2.25em 0 -1px -2.25em; + } + + .row.\35 0\25 > * { + padding: 0 0 0 0.75em; + } + + .row.\35 0\25 { + margin: 0 0 -1px -0.75em; + } + + .row.uniform.\35 0\25 > * { + padding: 0.75em 0 0 0.75em; + } + + .row.uniform.\35 0\25 { + margin: -0.75em 0 -1px -0.75em; + } + + .row.\32 5\25 > * { + padding: 0 0 0 0.375em; + } + + .row.\32 5\25 { + margin: 0 0 -1px -0.375em; + } + + .row.uniform.\32 5\25 > * { + padding: 0.375em 0 0 0.375em; + } + + .row.uniform.\32 5\25 { + margin: -0.375em 0 -1px -0.375em; + } + + .\31 2u\28xsmall\29, .\31 2u\24\28xsmall\29 { + width: 100%; + clear: none; + margin-left: 0; + } + + .\31 1u\28xsmall\29, .\31 1u\24\28xsmall\29 { + width: 91.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 0u\28xsmall\29, .\31 0u\24\28xsmall\29 { + width: 83.3333333333%; + clear: none; + margin-left: 0; + } + + .\39 u\28xsmall\29, .\39 u\24\28xsmall\29 { + width: 75%; + clear: none; + margin-left: 0; + } + + .\38 u\28xsmall\29, .\38 u\24\28xsmall\29 { + width: 66.6666666667%; + clear: none; + margin-left: 0; + } + + .\37 u\28xsmall\29, .\37 u\24\28xsmall\29 { + width: 58.3333333333%; + clear: none; + margin-left: 0; + } + + .\36 u\28xsmall\29, .\36 u\24\28xsmall\29 { + width: 50%; + clear: none; + margin-left: 0; + } + + .\35 u\28xsmall\29, .\35 u\24\28xsmall\29 { + width: 41.6666666667%; + clear: none; + margin-left: 0; + } + + .\34 u\28xsmall\29, .\34 u\24\28xsmall\29 { + width: 33.3333333333%; + clear: none; + margin-left: 0; + } + + .\33 u\28xsmall\29, .\33 u\24\28xsmall\29 { + width: 25%; + clear: none; + margin-left: 0; + } + + .\32 u\28xsmall\29, .\32 u\24\28xsmall\29 { + width: 16.6666666667%; + clear: none; + margin-left: 0; + } + + .\31 u\28xsmall\29, .\31 u\24\28xsmall\29 { + width: 8.3333333333%; + clear: none; + margin-left: 0; + } + + .\31 2u\24\28xsmall\29 + *, + .\31 1u\24\28xsmall\29 + *, + .\31 0u\24\28xsmall\29 + *, + .\39 u\24\28xsmall\29 + *, + .\38 u\24\28xsmall\29 + *, + .\37 u\24\28xsmall\29 + *, + .\36 u\24\28xsmall\29 + *, + .\35 u\24\28xsmall\29 + *, + .\34 u\24\28xsmall\29 + *, + .\33 u\24\28xsmall\29 + *, + .\32 u\24\28xsmall\29 + *, + .\31 u\24\28xsmall\29 + * { + clear: left; + } + + .\-11u\28xsmall\29 { + margin-left: 91.66667%; + } + + .\-10u\28xsmall\29 { + margin-left: 83.33333%; + } + + .\-9u\28xsmall\29 { + margin-left: 75%; + } + + .\-8u\28xsmall\29 { + margin-left: 66.66667%; + } + + .\-7u\28xsmall\29 { + margin-left: 58.33333%; + } + + .\-6u\28xsmall\29 { + margin-left: 50%; + } + + .\-5u\28xsmall\29 { + margin-left: 41.66667%; + } + + .\-4u\28xsmall\29 { + margin-left: 33.33333%; + } + + .\-3u\28xsmall\29 { + margin-left: 25%; + } + + .\-2u\28xsmall\29 { + margin-left: 16.66667%; + } + + .\-1u\28xsmall\29 { + margin-left: 8.33333%; + } + + } + +/* Basic */ + + body { + background: #fff; + } + + body.is-loading *, body.is-loading *:before, body.is-loading *:after { + -moz-animation: none !important; + -webkit-animation: none !important; + -ms-animation: none !important; + animation: none !important; + -moz-transition: none !important; + -webkit-transition: none !important; + -ms-transition: none !important; + transition: none !important; + } + + body, input, select, textarea { + color: #a2a2a2; + font-family: "Source Sans Pro", Helvetica, sans-serif; + font-size: 16pt; + font-weight: 400; + line-height: 1.75em; + } + + a { + -moz-transition: color 0.2s ease-in-out, border-color 0.2s ease-in-out; + -webkit-transition: color 0.2s ease-in-out, border-color 0.2s ease-in-out; + -ms-transition: color 0.2s ease-in-out, border-color 0.2s ease-in-out; + transition: color 0.2s ease-in-out, border-color 0.2s ease-in-out; + border-bottom: dotted 1px; + color: #49bf9d; + text-decoration: none; + } + + a:hover { + border-bottom-color: transparent; + color: #49bf9d !important; + text-decoration: none; + } + + strong, b { + color: #787878; + font-weight: 400; + } + + em, i { + font-style: italic; + } + + p { + margin: 0 0 2em 0; + } + + h1, h2, h3, h4, h5, h6 { + color: #787878; + font-weight: 400; + line-height: 1em; + margin: 0 0 1em 0; + } + + h1 a, h2 a, h3 a, h4 a, h5 a, h6 a { + color: inherit; + text-decoration: none; + } + + h1 { + font-size: 2em; + line-height: 1.5em; + } + + h2 { + font-size: 1.5em; + line-height: 1.5em; + } + + h3 { + font-size: 1.25em; + line-height: 1.5em; + } + + h4 { + font-size: 1.1em; + line-height: 1.5em; + } + + h5 { + font-size: 0.9em; + line-height: 1.5em; + } + + h6 { + font-size: 0.7em; + line-height: 1.5em; + } + + sub { + font-size: 0.8em; + position: relative; + top: 0.5em; + } + + sup { + font-size: 0.8em; + position: relative; + top: -0.5em; + } + + hr { + border: 0; + border-bottom: solid 2px #efefef; + margin: 2em 0; + } + + hr.major { + margin: 3em 0; + } + + blockquote { + border-left: solid 6px #efefef; + font-style: italic; + margin: 0 0 2em 0; + padding: 0.5em 0 0.5em 1.5em; + } + + code { + background: #f7f7f7; + border-radius: 0.35em; + border: solid 2px #efefef; + font-family: "Courier New", monospace; + font-size: 0.9em; + margin: 0 0.25em; + padding: 0.25em 0.65em; + } + + pre { + -webkit-overflow-scrolling: touch; + font-family: "Courier New", monospace; + font-size: 0.9em; + margin: 0 0 2em 0; + } + + pre code { + display: block; + line-height: 1.75em; + padding: 1em 1.5em; + overflow-x: auto; + } + + .align-left { + text-align: left; + } + + .align-center { + text-align: center; + } + + .align-right { + text-align: right; + } + +/* Section/Article */ + + section.special, article.special { + text-align: center; + } + + header p { + color: #b2b2b2; + position: relative; + margin: 0 0 1.5em 0; + } + + header h2 + p { + font-size: 1.25em; + margin-top: -1em; + line-height: 1.5em; + } + + header h3 + p { + font-size: 1.1em; + margin-top: -0.8em; + line-height: 1.5em; + } + + header h4 + p, + header h5 + p, + header h6 + p { + font-size: 0.9em; + margin-top: -0.6em; + line-height: 1.5em; + } + + header.major h2 { + font-size: 2em; + } + +/* Form */ + + form { + margin: 0 0 2em 0; + } + + label { + color: #787878; + display: block; + font-size: 0.9em; + font-weight: 400; + margin: 0 0 1em 0; + } + + input[type="text"], + input[type="password"], + input[type="email"], + select, + textarea { + -moz-appearance: none; + -webkit-appearance: none; + -ms-appearance: none; + appearance: none; + background: #f7f7f7; + border-radius: 0.35em; + border: solid 2px transparent; + color: inherit; + display: block; + outline: 0; + padding: 0 0.75em; + text-decoration: none; + width: 100%; + } + + input[type="text"]:invalid, + input[type="password"]:invalid, + input[type="email"]:invalid, + select:invalid, + textarea:invalid { + box-shadow: none; + } + + input[type="text"]:focus, + input[type="password"]:focus, + input[type="email"]:focus, + select:focus, + textarea:focus { + border-color: #49bf9d; + } + + .select-wrapper { + text-decoration: none; + display: block; + position: relative; + } + + .select-wrapper:before { + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + } + + .select-wrapper:before { + color: #dfdfdf; + content: '\f078'; + display: block; + height: 2.75em; + line-height: 2.75em; + pointer-events: none; + position: absolute; + right: 0; + text-align: center; + top: 0; + width: 2.75em; + } + + .select-wrapper select::-ms-expand { + display: none; + } + + input[type="text"], + input[type="password"], + input[type="email"], + select { + height: 2.75em; + } + + textarea { + padding: 0.75em; + } + + input[type="checkbox"], + input[type="radio"] { + -moz-appearance: none; + -webkit-appearance: none; + -ms-appearance: none; + appearance: none; + display: block; + float: left; + margin-right: -2em; + opacity: 0; + width: 1em; + z-index: -1; + } + + input[type="checkbox"] + label, + input[type="radio"] + label { + text-decoration: none; + color: #a2a2a2; + cursor: pointer; + display: inline-block; + font-size: 1em; + font-weight: 400; + padding-left: 2.4em; + padding-right: 0.75em; + position: relative; + } + + input[type="checkbox"] + label:before, + input[type="radio"] + label:before { + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + } + + input[type="checkbox"] + label:before, + input[type="radio"] + label:before { + background: #f7f7f7; + border-radius: 0.35em; + border: solid 2px transparent; + content: ''; + display: inline-block; + height: 1.65em; + left: 0; + line-height: 1.58125em; + position: absolute; + text-align: center; + top: 0; + width: 1.65em; + } + + input[type="checkbox"]:checked + label:before, + input[type="radio"]:checked + label:before { + background: #787878; + border-color: #787878; + color: #fff; + content: '\f00c'; + } + + input[type="checkbox"]:focus + label:before, + input[type="radio"]:focus + label:before { + border-color: #49bf9d; + } + + input[type="checkbox"] + label:before { + border-radius: 0.35em; + } + + input[type="radio"] + label:before { + border-radius: 100%; + } + + ::-webkit-input-placeholder { + color: #b2b2b2 !important; + opacity: 1.0; + } + + :-moz-placeholder { + color: #b2b2b2 !important; + opacity: 1.0; + } + + ::-moz-placeholder { + color: #b2b2b2 !important; + opacity: 1.0; + } + + :-ms-input-placeholder { + color: #b2b2b2 !important; + opacity: 1.0; + } + + .formerize-placeholder { + color: #b2b2b2 !important; + opacity: 1.0; + } + +/* Box */ + + .box { + border-radius: 0.35em; + border: solid 2px #efefef; + margin-bottom: 2em; + padding: 1.5em; + } + + .box > :last-child, + .box > :last-child > :last-child, + .box > :last-child > :last-child > :last-child { + margin-bottom: 0; + } + + .box.alt { + border: 0; + border-radius: 0; + padding: 0; + } + +/* Icon */ + + .icon { + text-decoration: none; + border-bottom: none; + position: relative; + } + + .icon:before { + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + } + + .icon > .label { + display: none; + } + +/* Image */ + + .image { + /*border-radius: 0.35em;*/ + border: 0; + display: inline-block; + position: relative; + } + + .image:before { + -moz-transition: opacity 0.2s ease-in-out; + -webkit-transition: opacity 0.2s ease-in-out; + -ms-transition: opacity 0.2s ease-in-out; + transition: opacity 0.2s ease-in-out; + /*background: url("images/overlay.png");*/ + /*border-radius: 0.35em;*/ + content: ''; + display: block; + height: 100%; + left: 0; + /*opacity: 0.5;*/ + position: absolute; + top: 0; + width: 100%; + } + + .image.thumb { + text-align: center; + } + + .image.thumb:after { + -moz-transition: opacity 0.2s ease-in-out; + -webkit-transition: opacity 0.2s ease-in-out; + -ms-transition: opacity 0.2s ease-in-out; + transition: opacity 0.2s ease-in-out; + border-radius: 0.35em; + border: solid 3px rgba(255, 255, 255, 0.5); + color: #fff; + content: 'View'; + display: inline-block; + font-size: 0.8em; + font-weight: 400; + left: 50%; + line-height: 2.25em; + margin: -1.25em 0 0 -3em; + opacity: 0; + padding: 0 1.5em; + position: absolute; + text-align: center; + text-decoration: none; + top: 50%; + white-space: nowrap; + } + + .image.thumb:hover:after { + opacity: 1.0; + } + + .image.thumb:hover:before { + background: url("images/overlay.png"), url("images/overlay.png"); + opacity: 1.0; + } + + .image img { + /*border-radius: 0.35em;*/ + display: block; + } + + .image.left { + float: left; + margin: 0 1.5em 1em 0; + top: 0.25em; + } + + .image.right { + float: right; + margin: 0 0 1em 1.5em; + top: 0.25em; + } + + .image.left, .image.right { + max-width: 40%; + } + + .image.left img, .image.right img { + width: 100%; + } + + .image.fit { + display: block; + margin: 0 0 2em 0; + width: 100%; + } + + .image.fit img { + width: 100%; + } + + .image.avatar { + border-radius: 100%; + } + + .image.avatar:before { + display: none; + } + + .image.avatar img { + border-radius: 100%; + width: 100%; + } + +/* List */ + + ol { + list-style: decimal; + margin: 0 0 2em 0; + padding-left: 1.25em; + } + + ol li { + padding-left: 0.25em; + } + + ul { + list-style: disc; + margin: 0 0 2em 0; + padding-left: 1em; + } + + ul li { + padding-left: 0.5em; + } + + ul.alt { + list-style: none; + padding-left: 0; + } + + ul.alt li { + border-top: solid 2px #efefef; + padding: 0.5em 0; + } + + ul.alt li:first-child { + border-top: 0; + padding-top: 0; + } + + ul.icons { + cursor: default; + list-style: none; + padding-left: 0; + } + + ul.icons li { + display: inline-block; + padding: 0 1em 0 0; + } + + ul.icons li:last-child { + padding-right: 0; + } + + ul.icons li .icon:before { + font-size: 1.5em; + } + + ul.actions { + cursor: default; + list-style: none; + padding-left: 0; + } + + ul.actions li { + display: inline-block; + padding: 0 1em 0 0; + vertical-align: middle; + } + + ul.actions li:last-child { + padding-right: 0; + } + + ul.actions.small li { + padding: 0 0.5em 0 0; + } + + ul.actions.vertical li { + display: block; + padding: 1em 0 0 0; + } + + ul.actions.vertical li:first-child { + padding-top: 0; + } + + ul.actions.vertical li > * { + margin-bottom: 0; + } + + ul.actions.vertical.small li { + padding: 0.5em 0 0 0; + } + + ul.actions.vertical.small li:first-child { + padding-top: 0; + } + + ul.actions.fit { + display: table; + margin-left: -1em; + padding: 0; + table-layout: fixed; + width: calc(100% + 1em); + } + + ul.actions.fit li { + display: table-cell; + padding: 0 0 0 1em; + } + + ul.actions.fit li > * { + margin-bottom: 0; + } + + ul.actions.fit.small { + margin-left: -0.5em; + width: calc(100% + 0.5em); + } + + ul.actions.fit.small li { + padding: 0 0 0 0.5em; + } + + ul.labeled-icons { + list-style: none; + padding: 0; + } + + ul.labeled-icons li { + line-height: 1.75em; + margin: 1.5em 0 0 0; + padding-left: 2.25em; + position: relative; + } + + ul.labeled-icons li:first-child { + margin-top: 0; + } + + ul.labeled-icons li a { + color: inherit; + } + + ul.labeled-icons li h3 { + color: #b2b2b2; + left: 0; + position: absolute; + text-align: center; + top: 0; + width: 1em; + } + + dl { + margin: 0 0 2em 0; + } + +/* Table */ + + .table-wrapper { + -webkit-overflow-scrolling: touch; + overflow-x: auto; + } + + table { + margin: 0 0 2em 0; + width: 100%; + } + + table tbody tr { + border: solid 1px #efefef; + border-left: 0; + border-right: 0; + } + + table tbody tr:nth-child(2n + 1) { + background-color: #f7f7f7; + } + + table td { + padding: 0.75em 0.75em; + } + + table th { + color: #787878; + font-size: 0.9em; + font-weight: 400; + padding: 0 0.75em 0.75em 0.75em; + text-align: left; + } + + table thead { + border-bottom: solid 2px #efefef; + } + + table tfoot { + border-top: solid 2px #efefef; + } + + table.alt { + border-collapse: separate; + } + + table.alt tbody tr td { + border: solid 2px #efefef; + border-left-width: 0; + border-top-width: 0; + } + + table.alt tbody tr td:first-child { + border-left-width: 2px; + } + + table.alt tbody tr:first-child td { + border-top-width: 2px; + } + + table.alt thead { + border-bottom: 0; + } + + table.alt tfoot { + border-top: 0; + } + +/* Button */ + + input[type="submit"], + input[type="reset"], + input[type="button"], + .button { + -moz-appearance: none; + -webkit-appearance: none; + -ms-appearance: none; + appearance: none; + -moz-transition: background-color 0.2s ease-in-out, color 0.2s ease-in-out, border-color 0.2s ease-in-out; + -webkit-transition: background-color 0.2s ease-in-out, color 0.2s ease-in-out, border-color 0.2s ease-in-out; + -ms-transition: background-color 0.2s ease-in-out, color 0.2s ease-in-out, border-color 0.2s ease-in-out; + transition: background-color 0.2s ease-in-out, color 0.2s ease-in-out, border-color 0.2s ease-in-out; + background-color: transparent; + border-radius: 0.35em; + border: solid 3px #efefef; + color: #787878 !important; + cursor: pointer; + display: inline-block; + font-weight: 400; + height: 3.15em; + height: calc(2.75em + 6px); + line-height: 2.75em; + min-width: 10em; + padding: 0 1.5em; + text-align: center; + text-decoration: none; + white-space: nowrap; + } + + input[type="submit"]:hover, + input[type="reset"]:hover, + input[type="button"]:hover, + .button:hover { + border-color: #49bf9d; + color: #49bf9d !important; + } + + input[type="submit"]:active, + input[type="reset"]:active, + input[type="button"]:active, + .button:active { + background-color: rgba(73, 191, 157, 0.1); + border-color: #49bf9d; + color: #49bf9d !important; + } + + input[type="submit"].icon, + input[type="reset"].icon, + input[type="button"].icon, + .button.icon { + padding-left: 1.35em; + } + + input[type="submit"].icon:before, + input[type="reset"].icon:before, + input[type="button"].icon:before, + .button.icon:before { + margin-right: 0.5em; + } + + input[type="submit"].fit, + input[type="reset"].fit, + input[type="button"].fit, + .button.fit { + display: block; + margin: 0 0 1em 0; + min-width: 0; + width: 100%; + } + + input[type="submit"].small, + input[type="reset"].small, + input[type="button"].small, + .button.small { + font-size: 0.8em; + } + + input[type="submit"].big, + input[type="reset"].big, + input[type="button"].big, + .button.big { + font-size: 1.35em; + } + + input[type="submit"].special, + input[type="reset"].special, + input[type="button"].special, + .button.special { + background-color: #49bf9d; + border-color: #49bf9d; + color: #ffffff !important; + } + + input[type="submit"].special:hover, + input[type="reset"].special:hover, + input[type="button"].special:hover, + .button.special:hover { + background-color: #5cc6a7; + border-color: #5cc6a7; + } + + input[type="submit"].special:active, + input[type="reset"].special:active, + input[type="button"].special:active, + .button.special:active { + background-color: #3eb08f; + border-color: #3eb08f; + } + + input[type="submit"].disabled, input[type="submit"]:disabled, + input[type="reset"].disabled, + input[type="reset"]:disabled, + input[type="button"].disabled, + input[type="button"]:disabled, + .button.disabled, + .button:disabled { + background-color: #e7e7e7 !important; + border-color: #e7e7e7 !important; + color: #b2b2b2 !important; + cursor: default; + } + +/* Work Item */ + + .work-item { + margin: 0 0 2em 0; + } + + .work-item .image { + margin: 0 0 1.5em 0; + } + + .work-item h3 { + font-size: 1em; + margin: 0 0 0.5em 0; + } + + .work-item p { + font-size: 0.8em; + line-height: 1.5em; + margin: 0; + } + +/* Header */ + + #header { + display: -moz-flex; + display: -webkit-flex; + display: -ms-flex; + display: flex; + -moz-flex-direction: column; + -webkit-flex-direction: column; + -ms-flex-direction: column; + flex-direction: column; + -moz-align-items: -moz-flex-end; + -webkit-align-items: -webkit-flex-end; + -ms-align-items: -ms-flex-end; + align-items: flex-end; + -moz-justify-content: space-between; + -webkit-justify-content: space-between; + -ms-justify-content: space-between; + justify-content: space-between; + background-color: #1f1815; + background-attachment: scroll, fixed; + background-image: url("images/overlay.png"), url("../../images/bg.jpg"); + background-position: top left, top left; + background-repeat: repeat, no-repeat; + background-size: auto, auto 100%; + color: rgba(255, 255, 255, 0.5); + height: 100%; + left: 0; + padding: 8em 4em; + position: fixed; + text-align: right; + top: 0; + width: 60em; + } + + #header > * { + -moz-flex-shrink: 0; + -webkit-flex-shrink: 0; + -ms-flex-shrink: 0; + flex-shrink: 0; + width: 100%; + } + + #header > .inner { + -moz-flex-grow: 1; + -webkit-flex-grow: 1; + -ms-flex-grow: 1; + flex-grow: 1; + margin: 0 0 2em 0; + } + + + #header .icons a { + color: rgba(255, 255, 255, 0.4); + } + + #header strong, #header b { + color: #ffffff; + } + + #header h2, #header h3, #header h4, #header h5, #header h6 { + color: #ffffff; + } + + #header h1 { + color: rgba(255, 255, 255, 0.5); + font-size: 1.35em; + line-height: 1.5em; + margin: 0; + } + + #header .image.avatar { + margin: 0 0 0.5em 0; + width: 10em; + } + +/* Footer */ + + #footer .icons { + margin: 1em 0 0 0; + } + + #footer .icons a { + color: rgba(255, 255, 255, 0.4); + } + + #footer .copyright { + color: rgba(255, 255, 255, 0.4); + font-size: 0.8em; + list-style: none; + margin: 1em 0 0 0; + padding: 0; + } + + #footer .copyright li { + border-left: solid 1px rgba(255, 255, 255, 0.25); + display: inline-block; + line-height: 1em; + margin-left: 0.75em; + padding-left: 0.75em; + } + + #footer .copyright li:first-child { + border-left: 0; + margin-left: 0; + padding-left: 0; + } + + #footer .copyright li a { + color: inherit; + } + +/* Main */ + + #main { + margin-left: 15%; + max-width: 54em; + padding: 8em 4em 4em 4em; + width: 70%; + } + + #main > section { + border-top: solid 2px #efefef; + margin: 4em 0 0 0; + padding: 4em 0 0 0; + } + + #main > section:first-child { + border-top: 0; + margin-top: 0; + padding-top: 0; + } + +/* Poptrox */ + + @-moz-keyframes spin { + 0% { + -moz-transform: rotate(0deg); + -webkit-transform: rotate(0deg); + -ms-transform: rotate(0deg); + transform: rotate(0deg); + } + + 100% { + -moz-transform: rotate(360deg); + -webkit-transform: rotate(360deg); + -ms-transform: rotate(360deg); + transform: rotate(360deg); + } + } + + @-webkit-keyframes spin { + 0% { + -moz-transform: rotate(0deg); + -webkit-transform: rotate(0deg); + -ms-transform: rotate(0deg); + transform: rotate(0deg); + } + + 100% { + -moz-transform: rotate(360deg); + -webkit-transform: rotate(360deg); + -ms-transform: rotate(360deg); + transform: rotate(360deg); + } + } + + @-ms-keyframes spin { + 0% { + -moz-transform: rotate(0deg); + -webkit-transform: rotate(0deg); + -ms-transform: rotate(0deg); + transform: rotate(0deg); + } + + 100% { + -moz-transform: rotate(360deg); + -webkit-transform: rotate(360deg); + -ms-transform: rotate(360deg); + transform: rotate(360deg); + } + } + + @keyframes spin { + 0% { + -moz-transform: rotate(0deg); + -webkit-transform: rotate(0deg); + -ms-transform: rotate(0deg); + transform: rotate(0deg); + } + + 100% { + -moz-transform: rotate(360deg); + -webkit-transform: rotate(360deg); + -ms-transform: rotate(360deg); + transform: rotate(360deg); + } + } + + .poptrox-popup { + -moz-box-sizing: content-box; + -webkit-box-sizing: content-box; + -ms-box-sizing: content-box; + box-sizing: content-box; + -webkit-tap-highlight-color: rgba(255, 255, 255, 0); + background: #fff; + border-radius: 0.35em; + box-shadow: 0 0.1em 0.15em 0 rgba(0, 0, 0, 0.15); + overflow: hidden; + padding-bottom: 3em; + } + + .poptrox-popup .loader { + text-decoration: none; + -moz-animation: spin 1s linear infinite; + -webkit-animation: spin 1s linear infinite; + -ms-animation: spin 1s linear infinite; + animation: spin 1s linear infinite; + font-size: 1.5em; + height: 1em; + left: 50%; + line-height: 1em; + margin: -0.5em 0 0 -0.5em; + position: absolute; + top: 50%; + width: 1em; + } + + .poptrox-popup .loader:before { + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + } + + .poptrox-popup .loader:before { + content: '\f1ce'; + } + + .poptrox-popup .caption { + background: #fff; + bottom: 0; + cursor: default; + font-size: 0.9em; + height: 3em; + left: 0; + line-height: 2.8em; + position: absolute; + text-align: center; + width: 100%; + z-index: 1; + } + + .poptrox-popup .nav-next, + .poptrox-popup .nav-previous { + text-decoration: none; + -moz-transition: opacity 0.2s ease-in-out; + -webkit-transition: opacity 0.2s ease-in-out; + -ms-transition: opacity 0.2s ease-in-out; + transition: opacity 0.2s ease-in-out; + -webkit-tap-highlight-color: rgba(255, 255, 255, 0); + background: rgba(0, 0, 0, 0.01); + cursor: pointer; + height: 100%; + opacity: 0; + position: absolute; + top: 0; + width: 50%; + } + + .poptrox-popup .nav-next:before, + .poptrox-popup .nav-previous:before { + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + } + + .poptrox-popup .nav-next:before, + .poptrox-popup .nav-previous:before { + color: #fff; + font-size: 2.5em; + height: 1em; + line-height: 1em; + margin-top: -0.75em; + position: absolute; + text-align: center; + top: 50%; + width: 1.5em; + } + + .poptrox-popup .nav-next { + right: 0; + } + + .poptrox-popup .nav-next:before { + content: '\f105'; + right: 0; + } + + .poptrox-popup .nav-previous { + left: 0; + } + + .poptrox-popup .nav-previous:before { + content: '\f104'; + left: 0; + } + + .poptrox-popup .closer { + text-decoration: none; + -moz-transition: opacity 0.2s ease-in-out; + -webkit-transition: opacity 0.2s ease-in-out; + -ms-transition: opacity 0.2s ease-in-out; + transition: opacity 0.2s ease-in-out; + -webkit-tap-highlight-color: rgba(255, 255, 255, 0); + color: #fff; + height: 4em; + line-height: 4em; + opacity: 0; + position: absolute; + right: 0; + text-align: center; + top: 0; + width: 4em; + z-index: 2; + } + + .poptrox-popup .closer:before { + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + } + + .poptrox-popup .closer:before { + -moz-box-sizing: content-box; + -webkit-box-sizing: content-box; + -ms-box-sizing: content-box; + box-sizing: content-box; + border-radius: 100%; + border: solid 3px rgba(255, 255, 255, 0.5); + content: '\f00d'; + display: block; + font-size: 1em; + height: 1.75em; + left: 50%; + line-height: 1.75em; + margin: -0.875em 0 0 -0.875em; + position: absolute; + top: 50%; + width: 1.75em; + } + + .poptrox-popup:hover .nav-next, + .poptrox-popup:hover .nav-previous { + opacity: 0.5; + } + + .poptrox-popup:hover .nav-next:hover, + .poptrox-popup:hover .nav-previous:hover { + opacity: 1.0; + } + + .poptrox-popup:hover .closer { + opacity: 0.5; + } + + .poptrox-popup:hover .closer:hover { + opacity: 1.0; + } + +/* Touch */ + + body.is-touch .image.thumb:before { + opacity: 0.5 !important; + } + + body.is-touch .image.thumb:after { + display: none !important; + } + + body.is-touch #header { + background-attachment: scroll; + background-size: auto, cover; + } + + body.is-touch .poptrox-popup .nav-next, + body.is-touch .poptrox-popup .nav-previous, + body.is-touch .poptrox-popup .closer { + opacity: 1.0 !important; + } + +/* XLarge */ + + @media screen and (max-width: 1800px) { + + /* Basic */ + + body, input, select, textarea { + font-size: 12pt; + } + + } + +/* Large */ + + @media screen and (max-width: 1280px) { + + /* Header */ + + #header { + padding: 6em 3em 3em 3em; + width: 30%; + } + + #header h1 { + font-size: 1.25em; + } + + #header h1 br { + display: none; + } + + /* Footer */ + + #footer .copyright li { + border-left-width: 0; + display: block; + line-height: 2.25em; + margin-left: 0; + padding-left: 0; + } + + /* Main */ + + #main { + margin-left: 30%; + max-width: none; + padding: 6em 3em 3em 3em; + width: calc(100% - 30%); + } + + } + +/* Medium */ + + @media screen and (max-width: 980px) { + + /* Basic */ + + h1 br, h2 br, h3 br, h4 br, h5 br, h6 br { + display: none; + } + + /* List */ + + ul.icons li .icon { + font-size: 1.25em; + } + + /* Header */ + + #header { + background-attachment: scroll; + background-position: top left, center center; + background-size: auto, cover; + left: auto; + padding: 6em 4em; + position: relative; + text-align: center; + top: auto; + width: 100%; + display: block; + } + + #header h1 { + font-size: 1.75em; + } + + #header h1 br { + display: inline; + } + + /* Footer */ + + #footer { + background-attachment: scroll; + background-color: #1f1815; + background-image: url("images/overlay.png"), url("../../images/bg.jpg"); + background-position: top left, bottom center; + background-repeat: repeat, no-repeat; + background-size: auto, cover; + bottom: auto; + left: auto; + padding: 4em 4em 6em 4em; + position: relative; + text-align: center; + width: 100%; + } + + #footer .icons { + margin: 0 0 1em 0; + } + + #footer .copyright { + margin: 0 0 1em 0; + } + + #footer .copyright li { + border-left-width: 1px; + display: inline-block; + line-height: 1em; + margin-left: 0.75em; + padding-left: 0.75em; + } + + /* Main */ + + #main { + margin: 0; + padding: 6em 4em; + width: 100%; + } + + } + +/* Small */ + + @media screen and (max-width: 736px) { + + /* Basic */ + + h1 { + font-size: 1.5em; + } + + h2 { + font-size: 1.2em; + } + + h3 { + font-size: 1em; + } + + /* Section/Article */ + + section.special, article.special { + text-align: center; + } + + header.major h2 { + font-size: 1.35em; + } + + /* List */ + + ul.labeled-icons li { + padding-left: 2em; + } + + ul.labeled-icons li h3 { + line-height: 1.75em; + } + + /* Header */ + + #header { + padding: 2.25em 1.5em; + } + + #header h1 { + font-size: 1.35em; + } + + /* Footer */ + + #footer { + padding: 2.25em 1.5em; + } + + /* Main */ + + #main { + padding: 2.25em 1.5em 0.25em 1.5em; + } + + #main > section { + margin: 2.25em 0 0 0; + padding: 2.25em 0 0 0; + } + + /* Poptrox */ + + .poptrox-popup { + border-radius: 0; + } + + .poptrox-popup .nav-next:before, + .poptrox-popup .nav-previous:before { + margin-top: -1em; + } + + } + +/* XSmall */ + + @media screen and (max-width: 480px) { + + /* List */ + + ul.actions { + margin: 0 0 2em 0; + } + + ul.actions li { + display: block; + padding: 1em 0 0 0; + text-align: center; + width: 100%; + } + + ul.actions li:first-child { + padding-top: 0; + } + + ul.actions li > * { + margin: 0 !important; + width: 100%; + } + + ul.actions li > *.icon:before { + margin-left: -2em; + } + + ul.actions.small li { + padding: 0.5em 0 0 0; + } + + ul.actions.small li:first-child { + padding-top: 0; + } + + /* Header */ + + #header { + padding: 4.5em 1.5em; + } + + #header h1 br { + display: none; + } + + /* Footer */ + + #footer .copyright li { + border-left-width: 0; + display: block; + line-height: 2.25em; + margin-left: 0; + padding-left: 0; + } + + } \ No newline at end of file diff --git a/assets/fonts/FontAwesome.otf b/assets/fonts/FontAwesome.otf new file mode 100755 index 00000000..d4de13e8 Binary files /dev/null and b/assets/fonts/FontAwesome.otf differ diff --git a/assets/fonts/fontawesome-webfont.eot b/assets/fonts/fontawesome-webfont.eot new file mode 100755 index 00000000..c7b00d2b Binary files /dev/null and b/assets/fonts/fontawesome-webfont.eot differ diff --git a/assets/fonts/fontawesome-webfont.svg b/assets/fonts/fontawesome-webfont.svg new file mode 100755 index 00000000..8b66187f --- /dev/null +++ b/assets/fonts/fontawesome-webfont.svg @@ -0,0 +1,685 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/assets/fonts/fontawesome-webfont.ttf b/assets/fonts/fontawesome-webfont.ttf new file mode 100755 index 00000000..f221e50a Binary files /dev/null and b/assets/fonts/fontawesome-webfont.ttf differ diff --git a/assets/fonts/fontawesome-webfont.woff b/assets/fonts/fontawesome-webfont.woff new file mode 100755 index 00000000..6e7483cf Binary files /dev/null and b/assets/fonts/fontawesome-webfont.woff differ diff --git a/assets/fonts/fontawesome-webfont.woff2 b/assets/fonts/fontawesome-webfont.woff2 new file mode 100755 index 00000000..7eb74fd1 Binary files /dev/null and b/assets/fonts/fontawesome-webfont.woff2 differ diff --git a/assets/js/ie/PIE.htc b/assets/js/ie/PIE.htc new file mode 100755 index 00000000..ca3b5470 --- /dev/null +++ b/assets/js/ie/PIE.htc @@ -0,0 +1,96 @@ + + + + + + + + + diff --git a/assets/js/ie/backgroundsize.min.htc b/assets/js/ie/backgroundsize.min.htc new file mode 100755 index 00000000..3d9960de --- /dev/null +++ b/assets/js/ie/backgroundsize.min.htc @@ -0,0 +1,7 @@ + + + + + \ No newline at end of file diff --git a/assets/js/ie/html5shiv.js b/assets/js/ie/html5shiv.js new file mode 100755 index 00000000..dcf351c8 --- /dev/null +++ b/assets/js/ie/html5shiv.js @@ -0,0 +1,8 @@ +/* + HTML5 Shiv v3.6.2 | @afarkas @jdalton @jon_neal @rem | MIT/GPL2 Licensed +*/ +(function(l,f){function m(){var a=e.elements;return"string"==typeof a?a.split(" "):a}function i(a){var b=n[a[o]];b||(b={},h++,a[o]=h,n[h]=b);return b}function p(a,b,c){b||(b=f);if(g)return b.createElement(a);c||(c=i(b));b=c.cache[a]?c.cache[a].cloneNode():r.test(a)?(c.cache[a]=c.createElem(a)).cloneNode():c.createElem(a);return b.canHaveChildren&&!s.test(a)?c.frag.appendChild(b):b}function t(a,b){if(!b.cache)b.cache={},b.createElem=a.createElement,b.createFrag=a.createDocumentFragment,b.frag=b.createFrag(); +a.createElement=function(c){return!e.shivMethods?b.createElem(c):p(c,a,b)};a.createDocumentFragment=Function("h,f","return function(){var n=f.cloneNode(),c=n.createElement;h.shivMethods&&("+m().join().replace(/\w+/g,function(a){b.createElem(a);b.frag.createElement(a);return'c("'+a+'")'})+");return n}")(e,b.frag)}function q(a){a||(a=f);var b=i(a);if(e.shivCSS&&!j&&!b.hasCSS){var c,d=a;c=d.createElement("p");d=d.getElementsByTagName("head")[0]||d.documentElement;c.innerHTML="x"; +c=d.insertBefore(c.lastChild,d.firstChild);b.hasCSS=!!c}g||t(a,b);return a}var k=l.html5||{},s=/^<|^(?:button|map|select|textarea|object|iframe|option|optgroup)$/i,r=/^(?:a|b|code|div|fieldset|h1|h2|h3|h4|h5|h6|i|label|li|ol|p|q|span|strong|style|table|tbody|td|th|tr|ul)$/i,j,o="_html5shiv",h=0,n={},g;(function(){try{var a=f.createElement("a");a.innerHTML="";j="hidden"in a;var b;if(!(b=1==a.childNodes.length)){f.createElement("a");var c=f.createDocumentFragment();b="undefined"==typeof c.cloneNode|| +"undefined"==typeof c.createDocumentFragment||"undefined"==typeof c.createElement}g=b}catch(d){g=j=!0}})();var e={elements:k.elements||"abbr article aside audio bdi canvas data datalist details figcaption figure footer header hgroup main mark meter nav output progress section summary time video",version:"3.6.2",shivCSS:!1!==k.shivCSS,supportsUnknownElements:g,shivMethods:!1!==k.shivMethods,type:"default",shivDocument:q,createElement:p,createDocumentFragment:function(a,b){a||(a=f);if(g)return a.createDocumentFragment(); +for(var b=b||i(a),c=b.frag.cloneNode(),d=0,e=m(),h=e.length;d #mq-test-1 { width: 42px; }',c.insertBefore(e,d),b=42===f.offsetWidth,c.removeChild(e),{matches:b,media:a}}}(a.document)}(this),function(a){"use strict";function b(){v(!0)}var c={};a.respond=c,c.update=function(){};var d=[],e=function(){var b=!1;try{b=new a.XMLHttpRequest}catch(c){b=new a.ActiveXObject("Microsoft.XMLHTTP")}return function(){return b}}(),f=function(a,b){var c=e();c&&(c.open("GET",a,!0),c.onreadystatechange=function(){4!==c.readyState||200!==c.status&&304!==c.status||b(c.responseText)},4!==c.readyState&&c.send(null))},g=function(a){return a.replace(c.regex.minmaxwh,"").match(c.regex.other)};if(c.ajax=f,c.queue=d,c.unsupportedmq=g,c.regex={media:/@media[^\{]+\{([^\{\}]*\{[^\}\{]*\})+/gi,keyframes:/@(?:\-(?:o|moz|webkit)\-)?keyframes[^\{]+\{(?:[^\{\}]*\{[^\}\{]*\})+[^\}]*\}/gi,comments:/\/\*[^*]*\*+([^/][^*]*\*+)*\//gi,urls:/(url\()['"]?([^\/\)'"][^:\)'"]+)['"]?(\))/g,findStyles:/@media *([^\{]+)\{([\S\s]+?)$/,only:/(only\s+)?([a-zA-Z]+)\s?/,minw:/\(\s*min\-width\s*:\s*(\s*[0-9\.]+)(px|em)\s*\)/,maxw:/\(\s*max\-width\s*:\s*(\s*[0-9\.]+)(px|em)\s*\)/,minmaxwh:/\(\s*m(in|ax)\-(height|width)\s*:\s*(\s*[0-9\.]+)(px|em)\s*\)/gi,other:/\([^\)]*\)/g},c.mediaQueriesSupported=a.matchMedia&&null!==a.matchMedia("only all")&&a.matchMedia("only all").matches,!c.mediaQueriesSupported){var h,i,j,k=a.document,l=k.documentElement,m=[],n=[],o=[],p={},q=30,r=k.getElementsByTagName("head")[0]||l,s=k.getElementsByTagName("base")[0],t=r.getElementsByTagName("link"),u=function(){var a,b=k.createElement("div"),c=k.body,d=l.style.fontSize,e=c&&c.style.fontSize,f=!1;return b.style.cssText="position:absolute;font-size:1em;width:1em",c||(c=f=k.createElement("body"),c.style.background="none"),l.style.fontSize="100%",c.style.fontSize="100%",c.appendChild(b),f&&l.insertBefore(c,l.firstChild),a=b.offsetWidth,f?l.removeChild(c):c.removeChild(b),l.style.fontSize=d,e&&(c.style.fontSize=e),a=j=parseFloat(a)},v=function(b){var c="clientWidth",d=l[c],e="CSS1Compat"===k.compatMode&&d||k.body[c]||d,f={},g=t[t.length-1],p=(new Date).getTime();if(b&&h&&q>p-h)return a.clearTimeout(i),i=a.setTimeout(v,q),void 0;h=p;for(var s in m)if(m.hasOwnProperty(s)){var w=m[s],x=w.minw,y=w.maxw,z=null===x,A=null===y,B="em";x&&(x=parseFloat(x)*(x.indexOf(B)>-1?j||u():1)),y&&(y=parseFloat(y)*(y.indexOf(B)>-1?j||u():1)),w.hasquery&&(z&&A||!(z||e>=x)||!(A||y>=e))||(f[w.media]||(f[w.media]=[]),f[w.media].push(n[w.rules]))}for(var C in o)o.hasOwnProperty(C)&&o[C]&&o[C].parentNode===r&&r.removeChild(o[C]);o.length=0;for(var D in f)if(f.hasOwnProperty(D)){var E=k.createElement("style"),F=f[D].join("\n");E.type="text/css",E.media=D,r.insertBefore(E,g.nextSibling),E.styleSheet?E.styleSheet.cssText=F:E.appendChild(k.createTextNode(F)),o.push(E)}},w=function(a,b,d){var e=a.replace(c.regex.comments,"").replace(c.regex.keyframes,"").match(c.regex.media),f=e&&e.length||0;b=b.substring(0,b.lastIndexOf("/"));var h=function(a){return a.replace(c.regex.urls,"$1"+b+"$2$3")},i=!f&&d;b.length&&(b+="/"),i&&(f=1);for(var j=0;f>j;j++){var k,l,o,p;i?(k=d,n.push(h(a))):(k=e[j].match(c.regex.findStyles)&&RegExp.$1,n.push(RegExp.$2&&h(RegExp.$2))),o=k.split(","),p=o.length;for(var q=0;p>q;q++)l=o[q],g(l)||m.push({media:l.split("(")[0].match(c.regex.only)&&RegExp.$2||"all",rules:n.length-1,hasquery:l.indexOf("(")>-1,minw:l.match(c.regex.minw)&&parseFloat(RegExp.$1)+(RegExp.$2||""),maxw:l.match(c.regex.maxw)&&parseFloat(RegExp.$1)+(RegExp.$2||"")})}v()},x=function(){if(d.length){var b=d.shift();f(b.href,function(c){w(c,b.href,b.media),p[b.href]=!0,a.setTimeout(function(){x()},0)})}},y=function(){for(var b=0;ba?this[a+this.length]:this[a]:d.call(this)},pushStack:function(a){var b=m.merge(this.constructor(),a);return b.prevObject=this,b.context=this.context,b},each:function(a,b){return m.each(this,a,b)},map:function(a){return this.pushStack(m.map(this,function(b,c){return a.call(b,c,b)}))},slice:function(){return this.pushStack(d.apply(this,arguments))},first:function(){return this.eq(0)},last:function(){return this.eq(-1)},eq:function(a){var b=this.length,c=+a+(0>a?b:0);return this.pushStack(c>=0&&b>c?[this[c]]:[])},end:function(){return this.prevObject||this.constructor(null)},push:f,sort:c.sort,splice:c.splice},m.extend=m.fn.extend=function(){var a,b,c,d,e,f,g=arguments[0]||{},h=1,i=arguments.length,j=!1;for("boolean"==typeof g&&(j=g,g=arguments[h]||{},h++),"object"==typeof g||m.isFunction(g)||(g={}),h===i&&(g=this,h--);i>h;h++)if(null!=(e=arguments[h]))for(d in e)a=g[d],c=e[d],g!==c&&(j&&c&&(m.isPlainObject(c)||(b=m.isArray(c)))?(b?(b=!1,f=a&&m.isArray(a)?a:[]):f=a&&m.isPlainObject(a)?a:{},g[d]=m.extend(j,f,c)):void 0!==c&&(g[d]=c));return g},m.extend({expando:"jQuery"+(l+Math.random()).replace(/\D/g,""),isReady:!0,error:function(a){throw new Error(a)},noop:function(){},isFunction:function(a){return"function"===m.type(a)},isArray:Array.isArray||function(a){return"array"===m.type(a)},isWindow:function(a){return null!=a&&a==a.window},isNumeric:function(a){return!m.isArray(a)&&a-parseFloat(a)+1>=0},isEmptyObject:function(a){var b;for(b in a)return!1;return!0},isPlainObject:function(a){var b;if(!a||"object"!==m.type(a)||a.nodeType||m.isWindow(a))return!1;try{if(a.constructor&&!j.call(a,"constructor")&&!j.call(a.constructor.prototype,"isPrototypeOf"))return!1}catch(c){return!1}if(k.ownLast)for(b in a)return j.call(a,b);for(b in a);return void 0===b||j.call(a,b)},type:function(a){return null==a?a+"":"object"==typeof a||"function"==typeof a?h[i.call(a)]||"object":typeof a},globalEval:function(b){b&&m.trim(b)&&(a.execScript||function(b){a.eval.call(a,b)})(b)},camelCase:function(a){return a.replace(o,"ms-").replace(p,q)},nodeName:function(a,b){return a.nodeName&&a.nodeName.toLowerCase()===b.toLowerCase()},each:function(a,b,c){var d,e=0,f=a.length,g=r(a);if(c){if(g){for(;f>e;e++)if(d=b.apply(a[e],c),d===!1)break}else for(e in a)if(d=b.apply(a[e],c),d===!1)break}else if(g){for(;f>e;e++)if(d=b.call(a[e],e,a[e]),d===!1)break}else for(e in a)if(d=b.call(a[e],e,a[e]),d===!1)break;return a},trim:function(a){return null==a?"":(a+"").replace(n,"")},makeArray:function(a,b){var c=b||[];return null!=a&&(r(Object(a))?m.merge(c,"string"==typeof a?[a]:a):f.call(c,a)),c},inArray:function(a,b,c){var d;if(b){if(g)return g.call(b,a,c);for(d=b.length,c=c?0>c?Math.max(0,d+c):c:0;d>c;c++)if(c in b&&b[c]===a)return c}return-1},merge:function(a,b){var c=+b.length,d=0,e=a.length;while(c>d)a[e++]=b[d++];if(c!==c)while(void 0!==b[d])a[e++]=b[d++];return a.length=e,a},grep:function(a,b,c){for(var d,e=[],f=0,g=a.length,h=!c;g>f;f++)d=!b(a[f],f),d!==h&&e.push(a[f]);return e},map:function(a,b,c){var d,f=0,g=a.length,h=r(a),i=[];if(h)for(;g>f;f++)d=b(a[f],f,c),null!=d&&i.push(d);else for(f in a)d=b(a[f],f,c),null!=d&&i.push(d);return e.apply([],i)},guid:1,proxy:function(a,b){var c,e,f;return"string"==typeof b&&(f=a[b],b=a,a=f),m.isFunction(a)?(c=d.call(arguments,2),e=function(){return a.apply(b||this,c.concat(d.call(arguments)))},e.guid=a.guid=a.guid||m.guid++,e):void 0},now:function(){return+new Date},support:k}),m.each("Boolean Number String Function Array Date RegExp Object Error".split(" "),function(a,b){h["[object "+b+"]"]=b.toLowerCase()});function r(a){var b="length"in a&&a.length,c=m.type(a);return"function"===c||m.isWindow(a)?!1:1===a.nodeType&&b?!0:"array"===c||0===b||"number"==typeof b&&b>0&&b-1 in a}var s=function(a){var b,c,d,e,f,g,h,i,j,k,l,m,n,o,p,q,r,s,t,u="sizzle"+1*new Date,v=a.document,w=0,x=0,y=ha(),z=ha(),A=ha(),B=function(a,b){return a===b&&(l=!0),0},C=1<<31,D={}.hasOwnProperty,E=[],F=E.pop,G=E.push,H=E.push,I=E.slice,J=function(a,b){for(var c=0,d=a.length;d>c;c++)if(a[c]===b)return c;return-1},K="checked|selected|async|autofocus|autoplay|controls|defer|disabled|hidden|ismap|loop|multiple|open|readonly|required|scoped",L="[\\x20\\t\\r\\n\\f]",M="(?:\\\\.|[\\w-]|[^\\x00-\\xa0])+",N=M.replace("w","w#"),O="\\["+L+"*("+M+")(?:"+L+"*([*^$|!~]?=)"+L+"*(?:'((?:\\\\.|[^\\\\'])*)'|\"((?:\\\\.|[^\\\\\"])*)\"|("+N+"))|)"+L+"*\\]",P=":("+M+")(?:\\((('((?:\\\\.|[^\\\\'])*)'|\"((?:\\\\.|[^\\\\\"])*)\")|((?:\\\\.|[^\\\\()[\\]]|"+O+")*)|.*)\\)|)",Q=new RegExp(L+"+","g"),R=new RegExp("^"+L+"+|((?:^|[^\\\\])(?:\\\\.)*)"+L+"+$","g"),S=new RegExp("^"+L+"*,"+L+"*"),T=new RegExp("^"+L+"*([>+~]|"+L+")"+L+"*"),U=new RegExp("="+L+"*([^\\]'\"]*?)"+L+"*\\]","g"),V=new RegExp(P),W=new RegExp("^"+N+"$"),X={ID:new RegExp("^#("+M+")"),CLASS:new RegExp("^\\.("+M+")"),TAG:new RegExp("^("+M.replace("w","w*")+")"),ATTR:new RegExp("^"+O),PSEUDO:new RegExp("^"+P),CHILD:new RegExp("^:(only|first|last|nth|nth-last)-(child|of-type)(?:\\("+L+"*(even|odd|(([+-]|)(\\d*)n|)"+L+"*(?:([+-]|)"+L+"*(\\d+)|))"+L+"*\\)|)","i"),bool:new RegExp("^(?:"+K+")$","i"),needsContext:new RegExp("^"+L+"*[>+~]|:(even|odd|eq|gt|lt|nth|first|last)(?:\\("+L+"*((?:-\\d)?\\d*)"+L+"*\\)|)(?=[^-]|$)","i")},Y=/^(?:input|select|textarea|button)$/i,Z=/^h\d$/i,$=/^[^{]+\{\s*\[native \w/,_=/^(?:#([\w-]+)|(\w+)|\.([\w-]+))$/,aa=/[+~]/,ba=/'|\\/g,ca=new RegExp("\\\\([\\da-f]{1,6}"+L+"?|("+L+")|.)","ig"),da=function(a,b,c){var d="0x"+b-65536;return d!==d||c?b:0>d?String.fromCharCode(d+65536):String.fromCharCode(d>>10|55296,1023&d|56320)},ea=function(){m()};try{H.apply(E=I.call(v.childNodes),v.childNodes),E[v.childNodes.length].nodeType}catch(fa){H={apply:E.length?function(a,b){G.apply(a,I.call(b))}:function(a,b){var c=a.length,d=0;while(a[c++]=b[d++]);a.length=c-1}}}function ga(a,b,d,e){var f,h,j,k,l,o,r,s,w,x;if((b?b.ownerDocument||b:v)!==n&&m(b),b=b||n,d=d||[],k=b.nodeType,"string"!=typeof a||!a||1!==k&&9!==k&&11!==k)return d;if(!e&&p){if(11!==k&&(f=_.exec(a)))if(j=f[1]){if(9===k){if(h=b.getElementById(j),!h||!h.parentNode)return d;if(h.id===j)return d.push(h),d}else if(b.ownerDocument&&(h=b.ownerDocument.getElementById(j))&&t(b,h)&&h.id===j)return d.push(h),d}else{if(f[2])return H.apply(d,b.getElementsByTagName(a)),d;if((j=f[3])&&c.getElementsByClassName)return H.apply(d,b.getElementsByClassName(j)),d}if(c.qsa&&(!q||!q.test(a))){if(s=r=u,w=b,x=1!==k&&a,1===k&&"object"!==b.nodeName.toLowerCase()){o=g(a),(r=b.getAttribute("id"))?s=r.replace(ba,"\\$&"):b.setAttribute("id",s),s="[id='"+s+"'] ",l=o.length;while(l--)o[l]=s+ra(o[l]);w=aa.test(a)&&pa(b.parentNode)||b,x=o.join(",")}if(x)try{return H.apply(d,w.querySelectorAll(x)),d}catch(y){}finally{r||b.removeAttribute("id")}}}return i(a.replace(R,"$1"),b,d,e)}function ha(){var a=[];function b(c,e){return a.push(c+" ")>d.cacheLength&&delete b[a.shift()],b[c+" "]=e}return b}function ia(a){return a[u]=!0,a}function ja(a){var b=n.createElement("div");try{return!!a(b)}catch(c){return!1}finally{b.parentNode&&b.parentNode.removeChild(b),b=null}}function ka(a,b){var c=a.split("|"),e=a.length;while(e--)d.attrHandle[c[e]]=b}function la(a,b){var c=b&&a,d=c&&1===a.nodeType&&1===b.nodeType&&(~b.sourceIndex||C)-(~a.sourceIndex||C);if(d)return d;if(c)while(c=c.nextSibling)if(c===b)return-1;return a?1:-1}function ma(a){return function(b){var c=b.nodeName.toLowerCase();return"input"===c&&b.type===a}}function na(a){return function(b){var c=b.nodeName.toLowerCase();return("input"===c||"button"===c)&&b.type===a}}function oa(a){return ia(function(b){return b=+b,ia(function(c,d){var e,f=a([],c.length,b),g=f.length;while(g--)c[e=f[g]]&&(c[e]=!(d[e]=c[e]))})})}function pa(a){return a&&"undefined"!=typeof a.getElementsByTagName&&a}c=ga.support={},f=ga.isXML=function(a){var b=a&&(a.ownerDocument||a).documentElement;return b?"HTML"!==b.nodeName:!1},m=ga.setDocument=function(a){var b,e,g=a?a.ownerDocument||a:v;return g!==n&&9===g.nodeType&&g.documentElement?(n=g,o=g.documentElement,e=g.defaultView,e&&e!==e.top&&(e.addEventListener?e.addEventListener("unload",ea,!1):e.attachEvent&&e.attachEvent("onunload",ea)),p=!f(g),c.attributes=ja(function(a){return a.className="i",!a.getAttribute("className")}),c.getElementsByTagName=ja(function(a){return a.appendChild(g.createComment("")),!a.getElementsByTagName("*").length}),c.getElementsByClassName=$.test(g.getElementsByClassName),c.getById=ja(function(a){return o.appendChild(a).id=u,!g.getElementsByName||!g.getElementsByName(u).length}),c.getById?(d.find.ID=function(a,b){if("undefined"!=typeof b.getElementById&&p){var c=b.getElementById(a);return c&&c.parentNode?[c]:[]}},d.filter.ID=function(a){var b=a.replace(ca,da);return function(a){return a.getAttribute("id")===b}}):(delete d.find.ID,d.filter.ID=function(a){var b=a.replace(ca,da);return function(a){var c="undefined"!=typeof a.getAttributeNode&&a.getAttributeNode("id");return c&&c.value===b}}),d.find.TAG=c.getElementsByTagName?function(a,b){return"undefined"!=typeof b.getElementsByTagName?b.getElementsByTagName(a):c.qsa?b.querySelectorAll(a):void 0}:function(a,b){var c,d=[],e=0,f=b.getElementsByTagName(a);if("*"===a){while(c=f[e++])1===c.nodeType&&d.push(c);return d}return f},d.find.CLASS=c.getElementsByClassName&&function(a,b){return p?b.getElementsByClassName(a):void 0},r=[],q=[],(c.qsa=$.test(g.querySelectorAll))&&(ja(function(a){o.appendChild(a).innerHTML="",a.querySelectorAll("[msallowcapture^='']").length&&q.push("[*^$]="+L+"*(?:''|\"\")"),a.querySelectorAll("[selected]").length||q.push("\\["+L+"*(?:value|"+K+")"),a.querySelectorAll("[id~="+u+"-]").length||q.push("~="),a.querySelectorAll(":checked").length||q.push(":checked"),a.querySelectorAll("a#"+u+"+*").length||q.push(".#.+[+~]")}),ja(function(a){var b=g.createElement("input");b.setAttribute("type","hidden"),a.appendChild(b).setAttribute("name","D"),a.querySelectorAll("[name=d]").length&&q.push("name"+L+"*[*^$|!~]?="),a.querySelectorAll(":enabled").length||q.push(":enabled",":disabled"),a.querySelectorAll("*,:x"),q.push(",.*:")})),(c.matchesSelector=$.test(s=o.matches||o.webkitMatchesSelector||o.mozMatchesSelector||o.oMatchesSelector||o.msMatchesSelector))&&ja(function(a){c.disconnectedMatch=s.call(a,"div"),s.call(a,"[s!='']:x"),r.push("!=",P)}),q=q.length&&new RegExp(q.join("|")),r=r.length&&new RegExp(r.join("|")),b=$.test(o.compareDocumentPosition),t=b||$.test(o.contains)?function(a,b){var c=9===a.nodeType?a.documentElement:a,d=b&&b.parentNode;return a===d||!(!d||1!==d.nodeType||!(c.contains?c.contains(d):a.compareDocumentPosition&&16&a.compareDocumentPosition(d)))}:function(a,b){if(b)while(b=b.parentNode)if(b===a)return!0;return!1},B=b?function(a,b){if(a===b)return l=!0,0;var d=!a.compareDocumentPosition-!b.compareDocumentPosition;return d?d:(d=(a.ownerDocument||a)===(b.ownerDocument||b)?a.compareDocumentPosition(b):1,1&d||!c.sortDetached&&b.compareDocumentPosition(a)===d?a===g||a.ownerDocument===v&&t(v,a)?-1:b===g||b.ownerDocument===v&&t(v,b)?1:k?J(k,a)-J(k,b):0:4&d?-1:1)}:function(a,b){if(a===b)return l=!0,0;var c,d=0,e=a.parentNode,f=b.parentNode,h=[a],i=[b];if(!e||!f)return a===g?-1:b===g?1:e?-1:f?1:k?J(k,a)-J(k,b):0;if(e===f)return la(a,b);c=a;while(c=c.parentNode)h.unshift(c);c=b;while(c=c.parentNode)i.unshift(c);while(h[d]===i[d])d++;return d?la(h[d],i[d]):h[d]===v?-1:i[d]===v?1:0},g):n},ga.matches=function(a,b){return ga(a,null,null,b)},ga.matchesSelector=function(a,b){if((a.ownerDocument||a)!==n&&m(a),b=b.replace(U,"='$1']"),!(!c.matchesSelector||!p||r&&r.test(b)||q&&q.test(b)))try{var d=s.call(a,b);if(d||c.disconnectedMatch||a.document&&11!==a.document.nodeType)return d}catch(e){}return ga(b,n,null,[a]).length>0},ga.contains=function(a,b){return(a.ownerDocument||a)!==n&&m(a),t(a,b)},ga.attr=function(a,b){(a.ownerDocument||a)!==n&&m(a);var e=d.attrHandle[b.toLowerCase()],f=e&&D.call(d.attrHandle,b.toLowerCase())?e(a,b,!p):void 0;return void 0!==f?f:c.attributes||!p?a.getAttribute(b):(f=a.getAttributeNode(b))&&f.specified?f.value:null},ga.error=function(a){throw new Error("Syntax error, unrecognized expression: "+a)},ga.uniqueSort=function(a){var b,d=[],e=0,f=0;if(l=!c.detectDuplicates,k=!c.sortStable&&a.slice(0),a.sort(B),l){while(b=a[f++])b===a[f]&&(e=d.push(f));while(e--)a.splice(d[e],1)}return k=null,a},e=ga.getText=function(a){var b,c="",d=0,f=a.nodeType;if(f){if(1===f||9===f||11===f){if("string"==typeof a.textContent)return a.textContent;for(a=a.firstChild;a;a=a.nextSibling)c+=e(a)}else if(3===f||4===f)return a.nodeValue}else while(b=a[d++])c+=e(b);return c},d=ga.selectors={cacheLength:50,createPseudo:ia,match:X,attrHandle:{},find:{},relative:{">":{dir:"parentNode",first:!0}," ":{dir:"parentNode"},"+":{dir:"previousSibling",first:!0},"~":{dir:"previousSibling"}},preFilter:{ATTR:function(a){return a[1]=a[1].replace(ca,da),a[3]=(a[3]||a[4]||a[5]||"").replace(ca,da),"~="===a[2]&&(a[3]=" "+a[3]+" "),a.slice(0,4)},CHILD:function(a){return a[1]=a[1].toLowerCase(),"nth"===a[1].slice(0,3)?(a[3]||ga.error(a[0]),a[4]=+(a[4]?a[5]+(a[6]||1):2*("even"===a[3]||"odd"===a[3])),a[5]=+(a[7]+a[8]||"odd"===a[3])):a[3]&&ga.error(a[0]),a},PSEUDO:function(a){var b,c=!a[6]&&a[2];return X.CHILD.test(a[0])?null:(a[3]?a[2]=a[4]||a[5]||"":c&&V.test(c)&&(b=g(c,!0))&&(b=c.indexOf(")",c.length-b)-c.length)&&(a[0]=a[0].slice(0,b),a[2]=c.slice(0,b)),a.slice(0,3))}},filter:{TAG:function(a){var b=a.replace(ca,da).toLowerCase();return"*"===a?function(){return!0}:function(a){return a.nodeName&&a.nodeName.toLowerCase()===b}},CLASS:function(a){var b=y[a+" "];return b||(b=new RegExp("(^|"+L+")"+a+"("+L+"|$)"))&&y(a,function(a){return b.test("string"==typeof a.className&&a.className||"undefined"!=typeof a.getAttribute&&a.getAttribute("class")||"")})},ATTR:function(a,b,c){return function(d){var e=ga.attr(d,a);return null==e?"!="===b:b?(e+="","="===b?e===c:"!="===b?e!==c:"^="===b?c&&0===e.indexOf(c):"*="===b?c&&e.indexOf(c)>-1:"$="===b?c&&e.slice(-c.length)===c:"~="===b?(" "+e.replace(Q," ")+" ").indexOf(c)>-1:"|="===b?e===c||e.slice(0,c.length+1)===c+"-":!1):!0}},CHILD:function(a,b,c,d,e){var f="nth"!==a.slice(0,3),g="last"!==a.slice(-4),h="of-type"===b;return 1===d&&0===e?function(a){return!!a.parentNode}:function(b,c,i){var j,k,l,m,n,o,p=f!==g?"nextSibling":"previousSibling",q=b.parentNode,r=h&&b.nodeName.toLowerCase(),s=!i&&!h;if(q){if(f){while(p){l=b;while(l=l[p])if(h?l.nodeName.toLowerCase()===r:1===l.nodeType)return!1;o=p="only"===a&&!o&&"nextSibling"}return!0}if(o=[g?q.firstChild:q.lastChild],g&&s){k=q[u]||(q[u]={}),j=k[a]||[],n=j[0]===w&&j[1],m=j[0]===w&&j[2],l=n&&q.childNodes[n];while(l=++n&&l&&l[p]||(m=n=0)||o.pop())if(1===l.nodeType&&++m&&l===b){k[a]=[w,n,m];break}}else if(s&&(j=(b[u]||(b[u]={}))[a])&&j[0]===w)m=j[1];else while(l=++n&&l&&l[p]||(m=n=0)||o.pop())if((h?l.nodeName.toLowerCase()===r:1===l.nodeType)&&++m&&(s&&((l[u]||(l[u]={}))[a]=[w,m]),l===b))break;return m-=e,m===d||m%d===0&&m/d>=0}}},PSEUDO:function(a,b){var c,e=d.pseudos[a]||d.setFilters[a.toLowerCase()]||ga.error("unsupported pseudo: "+a);return e[u]?e(b):e.length>1?(c=[a,a,"",b],d.setFilters.hasOwnProperty(a.toLowerCase())?ia(function(a,c){var d,f=e(a,b),g=f.length;while(g--)d=J(a,f[g]),a[d]=!(c[d]=f[g])}):function(a){return e(a,0,c)}):e}},pseudos:{not:ia(function(a){var b=[],c=[],d=h(a.replace(R,"$1"));return d[u]?ia(function(a,b,c,e){var f,g=d(a,null,e,[]),h=a.length;while(h--)(f=g[h])&&(a[h]=!(b[h]=f))}):function(a,e,f){return b[0]=a,d(b,null,f,c),b[0]=null,!c.pop()}}),has:ia(function(a){return function(b){return ga(a,b).length>0}}),contains:ia(function(a){return a=a.replace(ca,da),function(b){return(b.textContent||b.innerText||e(b)).indexOf(a)>-1}}),lang:ia(function(a){return W.test(a||"")||ga.error("unsupported lang: "+a),a=a.replace(ca,da).toLowerCase(),function(b){var c;do if(c=p?b.lang:b.getAttribute("xml:lang")||b.getAttribute("lang"))return c=c.toLowerCase(),c===a||0===c.indexOf(a+"-");while((b=b.parentNode)&&1===b.nodeType);return!1}}),target:function(b){var c=a.location&&a.location.hash;return c&&c.slice(1)===b.id},root:function(a){return a===o},focus:function(a){return a===n.activeElement&&(!n.hasFocus||n.hasFocus())&&!!(a.type||a.href||~a.tabIndex)},enabled:function(a){return a.disabled===!1},disabled:function(a){return a.disabled===!0},checked:function(a){var b=a.nodeName.toLowerCase();return"input"===b&&!!a.checked||"option"===b&&!!a.selected},selected:function(a){return a.parentNode&&a.parentNode.selectedIndex,a.selected===!0},empty:function(a){for(a=a.firstChild;a;a=a.nextSibling)if(a.nodeType<6)return!1;return!0},parent:function(a){return!d.pseudos.empty(a)},header:function(a){return Z.test(a.nodeName)},input:function(a){return Y.test(a.nodeName)},button:function(a){var b=a.nodeName.toLowerCase();return"input"===b&&"button"===a.type||"button"===b},text:function(a){var b;return"input"===a.nodeName.toLowerCase()&&"text"===a.type&&(null==(b=a.getAttribute("type"))||"text"===b.toLowerCase())},first:oa(function(){return[0]}),last:oa(function(a,b){return[b-1]}),eq:oa(function(a,b,c){return[0>c?c+b:c]}),even:oa(function(a,b){for(var c=0;b>c;c+=2)a.push(c);return a}),odd:oa(function(a,b){for(var c=1;b>c;c+=2)a.push(c);return a}),lt:oa(function(a,b,c){for(var d=0>c?c+b:c;--d>=0;)a.push(d);return a}),gt:oa(function(a,b,c){for(var d=0>c?c+b:c;++db;b++)d+=a[b].value;return d}function sa(a,b,c){var d=b.dir,e=c&&"parentNode"===d,f=x++;return b.first?function(b,c,f){while(b=b[d])if(1===b.nodeType||e)return a(b,c,f)}:function(b,c,g){var h,i,j=[w,f];if(g){while(b=b[d])if((1===b.nodeType||e)&&a(b,c,g))return!0}else while(b=b[d])if(1===b.nodeType||e){if(i=b[u]||(b[u]={}),(h=i[d])&&h[0]===w&&h[1]===f)return j[2]=h[2];if(i[d]=j,j[2]=a(b,c,g))return!0}}}function ta(a){return a.length>1?function(b,c,d){var e=a.length;while(e--)if(!a[e](b,c,d))return!1;return!0}:a[0]}function ua(a,b,c){for(var d=0,e=b.length;e>d;d++)ga(a,b[d],c);return c}function va(a,b,c,d,e){for(var f,g=[],h=0,i=a.length,j=null!=b;i>h;h++)(f=a[h])&&(!c||c(f,d,e))&&(g.push(f),j&&b.push(h));return g}function wa(a,b,c,d,e,f){return d&&!d[u]&&(d=wa(d)),e&&!e[u]&&(e=wa(e,f)),ia(function(f,g,h,i){var j,k,l,m=[],n=[],o=g.length,p=f||ua(b||"*",h.nodeType?[h]:h,[]),q=!a||!f&&b?p:va(p,m,a,h,i),r=c?e||(f?a:o||d)?[]:g:q;if(c&&c(q,r,h,i),d){j=va(r,n),d(j,[],h,i),k=j.length;while(k--)(l=j[k])&&(r[n[k]]=!(q[n[k]]=l))}if(f){if(e||a){if(e){j=[],k=r.length;while(k--)(l=r[k])&&j.push(q[k]=l);e(null,r=[],j,i)}k=r.length;while(k--)(l=r[k])&&(j=e?J(f,l):m[k])>-1&&(f[j]=!(g[j]=l))}}else r=va(r===g?r.splice(o,r.length):r),e?e(null,g,r,i):H.apply(g,r)})}function xa(a){for(var b,c,e,f=a.length,g=d.relative[a[0].type],h=g||d.relative[" "],i=g?1:0,k=sa(function(a){return a===b},h,!0),l=sa(function(a){return J(b,a)>-1},h,!0),m=[function(a,c,d){var e=!g&&(d||c!==j)||((b=c).nodeType?k(a,c,d):l(a,c,d));return b=null,e}];f>i;i++)if(c=d.relative[a[i].type])m=[sa(ta(m),c)];else{if(c=d.filter[a[i].type].apply(null,a[i].matches),c[u]){for(e=++i;f>e;e++)if(d.relative[a[e].type])break;return wa(i>1&&ta(m),i>1&&ra(a.slice(0,i-1).concat({value:" "===a[i-2].type?"*":""})).replace(R,"$1"),c,e>i&&xa(a.slice(i,e)),f>e&&xa(a=a.slice(e)),f>e&&ra(a))}m.push(c)}return ta(m)}function ya(a,b){var c=b.length>0,e=a.length>0,f=function(f,g,h,i,k){var l,m,o,p=0,q="0",r=f&&[],s=[],t=j,u=f||e&&d.find.TAG("*",k),v=w+=null==t?1:Math.random()||.1,x=u.length;for(k&&(j=g!==n&&g);q!==x&&null!=(l=u[q]);q++){if(e&&l){m=0;while(o=a[m++])if(o(l,g,h)){i.push(l);break}k&&(w=v)}c&&((l=!o&&l)&&p--,f&&r.push(l))}if(p+=q,c&&q!==p){m=0;while(o=b[m++])o(r,s,g,h);if(f){if(p>0)while(q--)r[q]||s[q]||(s[q]=F.call(i));s=va(s)}H.apply(i,s),k&&!f&&s.length>0&&p+b.length>1&&ga.uniqueSort(i)}return k&&(w=v,j=t),r};return c?ia(f):f}return h=ga.compile=function(a,b){var c,d=[],e=[],f=A[a+" "];if(!f){b||(b=g(a)),c=b.length;while(c--)f=xa(b[c]),f[u]?d.push(f):e.push(f);f=A(a,ya(e,d)),f.selector=a}return f},i=ga.select=function(a,b,e,f){var i,j,k,l,m,n="function"==typeof a&&a,o=!f&&g(a=n.selector||a);if(e=e||[],1===o.length){if(j=o[0]=o[0].slice(0),j.length>2&&"ID"===(k=j[0]).type&&c.getById&&9===b.nodeType&&p&&d.relative[j[1].type]){if(b=(d.find.ID(k.matches[0].replace(ca,da),b)||[])[0],!b)return e;n&&(b=b.parentNode),a=a.slice(j.shift().value.length)}i=X.needsContext.test(a)?0:j.length;while(i--){if(k=j[i],d.relative[l=k.type])break;if((m=d.find[l])&&(f=m(k.matches[0].replace(ca,da),aa.test(j[0].type)&&pa(b.parentNode)||b))){if(j.splice(i,1),a=f.length&&ra(j),!a)return H.apply(e,f),e;break}}}return(n||h(a,o))(f,b,!p,e,aa.test(a)&&pa(b.parentNode)||b),e},c.sortStable=u.split("").sort(B).join("")===u,c.detectDuplicates=!!l,m(),c.sortDetached=ja(function(a){return 1&a.compareDocumentPosition(n.createElement("div"))}),ja(function(a){return a.innerHTML="","#"===a.firstChild.getAttribute("href")})||ka("type|href|height|width",function(a,b,c){return c?void 0:a.getAttribute(b,"type"===b.toLowerCase()?1:2)}),c.attributes&&ja(function(a){return a.innerHTML="",a.firstChild.setAttribute("value",""),""===a.firstChild.getAttribute("value")})||ka("value",function(a,b,c){return c||"input"!==a.nodeName.toLowerCase()?void 0:a.defaultValue}),ja(function(a){return null==a.getAttribute("disabled")})||ka(K,function(a,b,c){var d;return c?void 0:a[b]===!0?b.toLowerCase():(d=a.getAttributeNode(b))&&d.specified?d.value:null}),ga}(a);m.find=s,m.expr=s.selectors,m.expr[":"]=m.expr.pseudos,m.unique=s.uniqueSort,m.text=s.getText,m.isXMLDoc=s.isXML,m.contains=s.contains;var t=m.expr.match.needsContext,u=/^<(\w+)\s*\/?>(?:<\/\1>|)$/,v=/^.[^:#\[\.,]*$/;function w(a,b,c){if(m.isFunction(b))return m.grep(a,function(a,d){return!!b.call(a,d,a)!==c});if(b.nodeType)return m.grep(a,function(a){return a===b!==c});if("string"==typeof b){if(v.test(b))return m.filter(b,a,c);b=m.filter(b,a)}return m.grep(a,function(a){return m.inArray(a,b)>=0!==c})}m.filter=function(a,b,c){var d=b[0];return c&&(a=":not("+a+")"),1===b.length&&1===d.nodeType?m.find.matchesSelector(d,a)?[d]:[]:m.find.matches(a,m.grep(b,function(a){return 1===a.nodeType}))},m.fn.extend({find:function(a){var b,c=[],d=this,e=d.length;if("string"!=typeof a)return this.pushStack(m(a).filter(function(){for(b=0;e>b;b++)if(m.contains(d[b],this))return!0}));for(b=0;e>b;b++)m.find(a,d[b],c);return c=this.pushStack(e>1?m.unique(c):c),c.selector=this.selector?this.selector+" "+a:a,c},filter:function(a){return this.pushStack(w(this,a||[],!1))},not:function(a){return this.pushStack(w(this,a||[],!0))},is:function(a){return!!w(this,"string"==typeof a&&t.test(a)?m(a):a||[],!1).length}});var x,y=a.document,z=/^(?:\s*(<[\w\W]+>)[^>]*|#([\w-]*))$/,A=m.fn.init=function(a,b){var c,d;if(!a)return this;if("string"==typeof a){if(c="<"===a.charAt(0)&&">"===a.charAt(a.length-1)&&a.length>=3?[null,a,null]:z.exec(a),!c||!c[1]&&b)return!b||b.jquery?(b||x).find(a):this.constructor(b).find(a);if(c[1]){if(b=b instanceof m?b[0]:b,m.merge(this,m.parseHTML(c[1],b&&b.nodeType?b.ownerDocument||b:y,!0)),u.test(c[1])&&m.isPlainObject(b))for(c in b)m.isFunction(this[c])?this[c](b[c]):this.attr(c,b[c]);return this}if(d=y.getElementById(c[2]),d&&d.parentNode){if(d.id!==c[2])return x.find(a);this.length=1,this[0]=d}return this.context=y,this.selector=a,this}return a.nodeType?(this.context=this[0]=a,this.length=1,this):m.isFunction(a)?"undefined"!=typeof x.ready?x.ready(a):a(m):(void 0!==a.selector&&(this.selector=a.selector,this.context=a.context),m.makeArray(a,this))};A.prototype=m.fn,x=m(y);var B=/^(?:parents|prev(?:Until|All))/,C={children:!0,contents:!0,next:!0,prev:!0};m.extend({dir:function(a,b,c){var d=[],e=a[b];while(e&&9!==e.nodeType&&(void 0===c||1!==e.nodeType||!m(e).is(c)))1===e.nodeType&&d.push(e),e=e[b];return d},sibling:function(a,b){for(var c=[];a;a=a.nextSibling)1===a.nodeType&&a!==b&&c.push(a);return c}}),m.fn.extend({has:function(a){var b,c=m(a,this),d=c.length;return this.filter(function(){for(b=0;d>b;b++)if(m.contains(this,c[b]))return!0})},closest:function(a,b){for(var c,d=0,e=this.length,f=[],g=t.test(a)||"string"!=typeof a?m(a,b||this.context):0;e>d;d++)for(c=this[d];c&&c!==b;c=c.parentNode)if(c.nodeType<11&&(g?g.index(c)>-1:1===c.nodeType&&m.find.matchesSelector(c,a))){f.push(c);break}return this.pushStack(f.length>1?m.unique(f):f)},index:function(a){return a?"string"==typeof a?m.inArray(this[0],m(a)):m.inArray(a.jquery?a[0]:a,this):this[0]&&this[0].parentNode?this.first().prevAll().length:-1},add:function(a,b){return this.pushStack(m.unique(m.merge(this.get(),m(a,b))))},addBack:function(a){return this.add(null==a?this.prevObject:this.prevObject.filter(a))}});function D(a,b){do a=a[b];while(a&&1!==a.nodeType);return a}m.each({parent:function(a){var b=a.parentNode;return b&&11!==b.nodeType?b:null},parents:function(a){return m.dir(a,"parentNode")},parentsUntil:function(a,b,c){return m.dir(a,"parentNode",c)},next:function(a){return D(a,"nextSibling")},prev:function(a){return D(a,"previousSibling")},nextAll:function(a){return m.dir(a,"nextSibling")},prevAll:function(a){return m.dir(a,"previousSibling")},nextUntil:function(a,b,c){return m.dir(a,"nextSibling",c)},prevUntil:function(a,b,c){return m.dir(a,"previousSibling",c)},siblings:function(a){return m.sibling((a.parentNode||{}).firstChild,a)},children:function(a){return m.sibling(a.firstChild)},contents:function(a){return m.nodeName(a,"iframe")?a.contentDocument||a.contentWindow.document:m.merge([],a.childNodes)}},function(a,b){m.fn[a]=function(c,d){var e=m.map(this,b,c);return"Until"!==a.slice(-5)&&(d=c),d&&"string"==typeof d&&(e=m.filter(d,e)),this.length>1&&(C[a]||(e=m.unique(e)),B.test(a)&&(e=e.reverse())),this.pushStack(e)}});var E=/\S+/g,F={};function G(a){var b=F[a]={};return m.each(a.match(E)||[],function(a,c){b[c]=!0}),b}m.Callbacks=function(a){a="string"==typeof a?F[a]||G(a):m.extend({},a);var b,c,d,e,f,g,h=[],i=!a.once&&[],j=function(l){for(c=a.memory&&l,d=!0,f=g||0,g=0,e=h.length,b=!0;h&&e>f;f++)if(h[f].apply(l[0],l[1])===!1&&a.stopOnFalse){c=!1;break}b=!1,h&&(i?i.length&&j(i.shift()):c?h=[]:k.disable())},k={add:function(){if(h){var d=h.length;!function f(b){m.each(b,function(b,c){var d=m.type(c);"function"===d?a.unique&&k.has(c)||h.push(c):c&&c.length&&"string"!==d&&f(c)})}(arguments),b?e=h.length:c&&(g=d,j(c))}return this},remove:function(){return h&&m.each(arguments,function(a,c){var d;while((d=m.inArray(c,h,d))>-1)h.splice(d,1),b&&(e>=d&&e--,f>=d&&f--)}),this},has:function(a){return a?m.inArray(a,h)>-1:!(!h||!h.length)},empty:function(){return h=[],e=0,this},disable:function(){return h=i=c=void 0,this},disabled:function(){return!h},lock:function(){return i=void 0,c||k.disable(),this},locked:function(){return!i},fireWith:function(a,c){return!h||d&&!i||(c=c||[],c=[a,c.slice?c.slice():c],b?i.push(c):j(c)),this},fire:function(){return k.fireWith(this,arguments),this},fired:function(){return!!d}};return k},m.extend({Deferred:function(a){var b=[["resolve","done",m.Callbacks("once memory"),"resolved"],["reject","fail",m.Callbacks("once memory"),"rejected"],["notify","progress",m.Callbacks("memory")]],c="pending",d={state:function(){return c},always:function(){return e.done(arguments).fail(arguments),this},then:function(){var a=arguments;return m.Deferred(function(c){m.each(b,function(b,f){var g=m.isFunction(a[b])&&a[b];e[f[1]](function(){var a=g&&g.apply(this,arguments);a&&m.isFunction(a.promise)?a.promise().done(c.resolve).fail(c.reject).progress(c.notify):c[f[0]+"With"](this===d?c.promise():this,g?[a]:arguments)})}),a=null}).promise()},promise:function(a){return null!=a?m.extend(a,d):d}},e={};return d.pipe=d.then,m.each(b,function(a,f){var g=f[2],h=f[3];d[f[1]]=g.add,h&&g.add(function(){c=h},b[1^a][2].disable,b[2][2].lock),e[f[0]]=function(){return e[f[0]+"With"](this===e?d:this,arguments),this},e[f[0]+"With"]=g.fireWith}),d.promise(e),a&&a.call(e,e),e},when:function(a){var b=0,c=d.call(arguments),e=c.length,f=1!==e||a&&m.isFunction(a.promise)?e:0,g=1===f?a:m.Deferred(),h=function(a,b,c){return function(e){b[a]=this,c[a]=arguments.length>1?d.call(arguments):e,c===i?g.notifyWith(b,c):--f||g.resolveWith(b,c)}},i,j,k;if(e>1)for(i=new Array(e),j=new Array(e),k=new Array(e);e>b;b++)c[b]&&m.isFunction(c[b].promise)?c[b].promise().done(h(b,k,c)).fail(g.reject).progress(h(b,j,i)):--f;return f||g.resolveWith(k,c),g.promise()}});var H;m.fn.ready=function(a){return m.ready.promise().done(a),this},m.extend({isReady:!1,readyWait:1,holdReady:function(a){a?m.readyWait++:m.ready(!0)},ready:function(a){if(a===!0?!--m.readyWait:!m.isReady){if(!y.body)return setTimeout(m.ready);m.isReady=!0,a!==!0&&--m.readyWait>0||(H.resolveWith(y,[m]),m.fn.triggerHandler&&(m(y).triggerHandler("ready"),m(y).off("ready")))}}});function I(){y.addEventListener?(y.removeEventListener("DOMContentLoaded",J,!1),a.removeEventListener("load",J,!1)):(y.detachEvent("onreadystatechange",J),a.detachEvent("onload",J))}function J(){(y.addEventListener||"load"===event.type||"complete"===y.readyState)&&(I(),m.ready())}m.ready.promise=function(b){if(!H)if(H=m.Deferred(),"complete"===y.readyState)setTimeout(m.ready);else if(y.addEventListener)y.addEventListener("DOMContentLoaded",J,!1),a.addEventListener("load",J,!1);else{y.attachEvent("onreadystatechange",J),a.attachEvent("onload",J);var c=!1;try{c=null==a.frameElement&&y.documentElement}catch(d){}c&&c.doScroll&&!function e(){if(!m.isReady){try{c.doScroll("left")}catch(a){return setTimeout(e,50)}I(),m.ready()}}()}return H.promise(b)};var K="undefined",L;for(L in m(k))break;k.ownLast="0"!==L,k.inlineBlockNeedsLayout=!1,m(function(){var a,b,c,d;c=y.getElementsByTagName("body")[0],c&&c.style&&(b=y.createElement("div"),d=y.createElement("div"),d.style.cssText="position:absolute;border:0;width:0;height:0;top:0;left:-9999px",c.appendChild(d).appendChild(b),typeof b.style.zoom!==K&&(b.style.cssText="display:inline;margin:0;border:0;padding:1px;width:1px;zoom:1",k.inlineBlockNeedsLayout=a=3===b.offsetWidth,a&&(c.style.zoom=1)),c.removeChild(d))}),function(){var a=y.createElement("div");if(null==k.deleteExpando){k.deleteExpando=!0;try{delete a.test}catch(b){k.deleteExpando=!1}}a=null}(),m.acceptData=function(a){var b=m.noData[(a.nodeName+" ").toLowerCase()],c=+a.nodeType||1;return 1!==c&&9!==c?!1:!b||b!==!0&&a.getAttribute("classid")===b};var M=/^(?:\{[\w\W]*\}|\[[\w\W]*\])$/,N=/([A-Z])/g;function O(a,b,c){if(void 0===c&&1===a.nodeType){var d="data-"+b.replace(N,"-$1").toLowerCase();if(c=a.getAttribute(d),"string"==typeof c){try{c="true"===c?!0:"false"===c?!1:"null"===c?null:+c+""===c?+c:M.test(c)?m.parseJSON(c):c}catch(e){}m.data(a,b,c)}else c=void 0}return c}function P(a){var b;for(b in a)if(("data"!==b||!m.isEmptyObject(a[b]))&&"toJSON"!==b)return!1; + +return!0}function Q(a,b,d,e){if(m.acceptData(a)){var f,g,h=m.expando,i=a.nodeType,j=i?m.cache:a,k=i?a[h]:a[h]&&h;if(k&&j[k]&&(e||j[k].data)||void 0!==d||"string"!=typeof b)return k||(k=i?a[h]=c.pop()||m.guid++:h),j[k]||(j[k]=i?{}:{toJSON:m.noop}),("object"==typeof b||"function"==typeof b)&&(e?j[k]=m.extend(j[k],b):j[k].data=m.extend(j[k].data,b)),g=j[k],e||(g.data||(g.data={}),g=g.data),void 0!==d&&(g[m.camelCase(b)]=d),"string"==typeof b?(f=g[b],null==f&&(f=g[m.camelCase(b)])):f=g,f}}function R(a,b,c){if(m.acceptData(a)){var d,e,f=a.nodeType,g=f?m.cache:a,h=f?a[m.expando]:m.expando;if(g[h]){if(b&&(d=c?g[h]:g[h].data)){m.isArray(b)?b=b.concat(m.map(b,m.camelCase)):b in d?b=[b]:(b=m.camelCase(b),b=b in d?[b]:b.split(" ")),e=b.length;while(e--)delete d[b[e]];if(c?!P(d):!m.isEmptyObject(d))return}(c||(delete g[h].data,P(g[h])))&&(f?m.cleanData([a],!0):k.deleteExpando||g!=g.window?delete g[h]:g[h]=null)}}}m.extend({cache:{},noData:{"applet ":!0,"embed ":!0,"object ":"clsid:D27CDB6E-AE6D-11cf-96B8-444553540000"},hasData:function(a){return a=a.nodeType?m.cache[a[m.expando]]:a[m.expando],!!a&&!P(a)},data:function(a,b,c){return Q(a,b,c)},removeData:function(a,b){return R(a,b)},_data:function(a,b,c){return Q(a,b,c,!0)},_removeData:function(a,b){return R(a,b,!0)}}),m.fn.extend({data:function(a,b){var c,d,e,f=this[0],g=f&&f.attributes;if(void 0===a){if(this.length&&(e=m.data(f),1===f.nodeType&&!m._data(f,"parsedAttrs"))){c=g.length;while(c--)g[c]&&(d=g[c].name,0===d.indexOf("data-")&&(d=m.camelCase(d.slice(5)),O(f,d,e[d])));m._data(f,"parsedAttrs",!0)}return e}return"object"==typeof a?this.each(function(){m.data(this,a)}):arguments.length>1?this.each(function(){m.data(this,a,b)}):f?O(f,a,m.data(f,a)):void 0},removeData:function(a){return this.each(function(){m.removeData(this,a)})}}),m.extend({queue:function(a,b,c){var d;return a?(b=(b||"fx")+"queue",d=m._data(a,b),c&&(!d||m.isArray(c)?d=m._data(a,b,m.makeArray(c)):d.push(c)),d||[]):void 0},dequeue:function(a,b){b=b||"fx";var c=m.queue(a,b),d=c.length,e=c.shift(),f=m._queueHooks(a,b),g=function(){m.dequeue(a,b)};"inprogress"===e&&(e=c.shift(),d--),e&&("fx"===b&&c.unshift("inprogress"),delete f.stop,e.call(a,g,f)),!d&&f&&f.empty.fire()},_queueHooks:function(a,b){var c=b+"queueHooks";return m._data(a,c)||m._data(a,c,{empty:m.Callbacks("once memory").add(function(){m._removeData(a,b+"queue"),m._removeData(a,c)})})}}),m.fn.extend({queue:function(a,b){var c=2;return"string"!=typeof a&&(b=a,a="fx",c--),arguments.lengthh;h++)b(a[h],c,g?d:d.call(a[h],h,b(a[h],c)));return e?a:j?b.call(a):i?b(a[0],c):f},W=/^(?:checkbox|radio)$/i;!function(){var a=y.createElement("input"),b=y.createElement("div"),c=y.createDocumentFragment();if(b.innerHTML="
a",k.leadingWhitespace=3===b.firstChild.nodeType,k.tbody=!b.getElementsByTagName("tbody").length,k.htmlSerialize=!!b.getElementsByTagName("link").length,k.html5Clone="<:nav>"!==y.createElement("nav").cloneNode(!0).outerHTML,a.type="checkbox",a.checked=!0,c.appendChild(a),k.appendChecked=a.checked,b.innerHTML="",k.noCloneChecked=!!b.cloneNode(!0).lastChild.defaultValue,c.appendChild(b),b.innerHTML="",k.checkClone=b.cloneNode(!0).cloneNode(!0).lastChild.checked,k.noCloneEvent=!0,b.attachEvent&&(b.attachEvent("onclick",function(){k.noCloneEvent=!1}),b.cloneNode(!0).click()),null==k.deleteExpando){k.deleteExpando=!0;try{delete b.test}catch(d){k.deleteExpando=!1}}}(),function(){var b,c,d=y.createElement("div");for(b in{submit:!0,change:!0,focusin:!0})c="on"+b,(k[b+"Bubbles"]=c in a)||(d.setAttribute(c,"t"),k[b+"Bubbles"]=d.attributes[c].expando===!1);d=null}();var X=/^(?:input|select|textarea)$/i,Y=/^key/,Z=/^(?:mouse|pointer|contextmenu)|click/,$=/^(?:focusinfocus|focusoutblur)$/,_=/^([^.]*)(?:\.(.+)|)$/;function aa(){return!0}function ba(){return!1}function ca(){try{return y.activeElement}catch(a){}}m.event={global:{},add:function(a,b,c,d,e){var f,g,h,i,j,k,l,n,o,p,q,r=m._data(a);if(r){c.handler&&(i=c,c=i.handler,e=i.selector),c.guid||(c.guid=m.guid++),(g=r.events)||(g=r.events={}),(k=r.handle)||(k=r.handle=function(a){return typeof m===K||a&&m.event.triggered===a.type?void 0:m.event.dispatch.apply(k.elem,arguments)},k.elem=a),b=(b||"").match(E)||[""],h=b.length;while(h--)f=_.exec(b[h])||[],o=q=f[1],p=(f[2]||"").split(".").sort(),o&&(j=m.event.special[o]||{},o=(e?j.delegateType:j.bindType)||o,j=m.event.special[o]||{},l=m.extend({type:o,origType:q,data:d,handler:c,guid:c.guid,selector:e,needsContext:e&&m.expr.match.needsContext.test(e),namespace:p.join(".")},i),(n=g[o])||(n=g[o]=[],n.delegateCount=0,j.setup&&j.setup.call(a,d,p,k)!==!1||(a.addEventListener?a.addEventListener(o,k,!1):a.attachEvent&&a.attachEvent("on"+o,k))),j.add&&(j.add.call(a,l),l.handler.guid||(l.handler.guid=c.guid)),e?n.splice(n.delegateCount++,0,l):n.push(l),m.event.global[o]=!0);a=null}},remove:function(a,b,c,d,e){var f,g,h,i,j,k,l,n,o,p,q,r=m.hasData(a)&&m._data(a);if(r&&(k=r.events)){b=(b||"").match(E)||[""],j=b.length;while(j--)if(h=_.exec(b[j])||[],o=q=h[1],p=(h[2]||"").split(".").sort(),o){l=m.event.special[o]||{},o=(d?l.delegateType:l.bindType)||o,n=k[o]||[],h=h[2]&&new RegExp("(^|\\.)"+p.join("\\.(?:.*\\.|)")+"(\\.|$)"),i=f=n.length;while(f--)g=n[f],!e&&q!==g.origType||c&&c.guid!==g.guid||h&&!h.test(g.namespace)||d&&d!==g.selector&&("**"!==d||!g.selector)||(n.splice(f,1),g.selector&&n.delegateCount--,l.remove&&l.remove.call(a,g));i&&!n.length&&(l.teardown&&l.teardown.call(a,p,r.handle)!==!1||m.removeEvent(a,o,r.handle),delete k[o])}else for(o in k)m.event.remove(a,o+b[j],c,d,!0);m.isEmptyObject(k)&&(delete r.handle,m._removeData(a,"events"))}},trigger:function(b,c,d,e){var f,g,h,i,k,l,n,o=[d||y],p=j.call(b,"type")?b.type:b,q=j.call(b,"namespace")?b.namespace.split("."):[];if(h=l=d=d||y,3!==d.nodeType&&8!==d.nodeType&&!$.test(p+m.event.triggered)&&(p.indexOf(".")>=0&&(q=p.split("."),p=q.shift(),q.sort()),g=p.indexOf(":")<0&&"on"+p,b=b[m.expando]?b:new m.Event(p,"object"==typeof b&&b),b.isTrigger=e?2:3,b.namespace=q.join("."),b.namespace_re=b.namespace?new RegExp("(^|\\.)"+q.join("\\.(?:.*\\.|)")+"(\\.|$)"):null,b.result=void 0,b.target||(b.target=d),c=null==c?[b]:m.makeArray(c,[b]),k=m.event.special[p]||{},e||!k.trigger||k.trigger.apply(d,c)!==!1)){if(!e&&!k.noBubble&&!m.isWindow(d)){for(i=k.delegateType||p,$.test(i+p)||(h=h.parentNode);h;h=h.parentNode)o.push(h),l=h;l===(d.ownerDocument||y)&&o.push(l.defaultView||l.parentWindow||a)}n=0;while((h=o[n++])&&!b.isPropagationStopped())b.type=n>1?i:k.bindType||p,f=(m._data(h,"events")||{})[b.type]&&m._data(h,"handle"),f&&f.apply(h,c),f=g&&h[g],f&&f.apply&&m.acceptData(h)&&(b.result=f.apply(h,c),b.result===!1&&b.preventDefault());if(b.type=p,!e&&!b.isDefaultPrevented()&&(!k._default||k._default.apply(o.pop(),c)===!1)&&m.acceptData(d)&&g&&d[p]&&!m.isWindow(d)){l=d[g],l&&(d[g]=null),m.event.triggered=p;try{d[p]()}catch(r){}m.event.triggered=void 0,l&&(d[g]=l)}return b.result}},dispatch:function(a){a=m.event.fix(a);var b,c,e,f,g,h=[],i=d.call(arguments),j=(m._data(this,"events")||{})[a.type]||[],k=m.event.special[a.type]||{};if(i[0]=a,a.delegateTarget=this,!k.preDispatch||k.preDispatch.call(this,a)!==!1){h=m.event.handlers.call(this,a,j),b=0;while((f=h[b++])&&!a.isPropagationStopped()){a.currentTarget=f.elem,g=0;while((e=f.handlers[g++])&&!a.isImmediatePropagationStopped())(!a.namespace_re||a.namespace_re.test(e.namespace))&&(a.handleObj=e,a.data=e.data,c=((m.event.special[e.origType]||{}).handle||e.handler).apply(f.elem,i),void 0!==c&&(a.result=c)===!1&&(a.preventDefault(),a.stopPropagation()))}return k.postDispatch&&k.postDispatch.call(this,a),a.result}},handlers:function(a,b){var c,d,e,f,g=[],h=b.delegateCount,i=a.target;if(h&&i.nodeType&&(!a.button||"click"!==a.type))for(;i!=this;i=i.parentNode||this)if(1===i.nodeType&&(i.disabled!==!0||"click"!==a.type)){for(e=[],f=0;h>f;f++)d=b[f],c=d.selector+" ",void 0===e[c]&&(e[c]=d.needsContext?m(c,this).index(i)>=0:m.find(c,this,null,[i]).length),e[c]&&e.push(d);e.length&&g.push({elem:i,handlers:e})}return h]","i"),ha=/^\s+/,ia=/<(?!area|br|col|embed|hr|img|input|link|meta|param)(([\w:]+)[^>]*)\/>/gi,ja=/<([\w:]+)/,ka=/\s*$/g,ra={option:[1,""],legend:[1,"
","
"],area:[1,"",""],param:[1,"",""],thead:[1,"","
"],tr:[2,"","
"],col:[2,"","
"],td:[3,"","
"],_default:k.htmlSerialize?[0,"",""]:[1,"X
","
"]},sa=da(y),ta=sa.appendChild(y.createElement("div"));ra.optgroup=ra.option,ra.tbody=ra.tfoot=ra.colgroup=ra.caption=ra.thead,ra.th=ra.td;function ua(a,b){var c,d,e=0,f=typeof a.getElementsByTagName!==K?a.getElementsByTagName(b||"*"):typeof a.querySelectorAll!==K?a.querySelectorAll(b||"*"):void 0;if(!f)for(f=[],c=a.childNodes||a;null!=(d=c[e]);e++)!b||m.nodeName(d,b)?f.push(d):m.merge(f,ua(d,b));return void 0===b||b&&m.nodeName(a,b)?m.merge([a],f):f}function va(a){W.test(a.type)&&(a.defaultChecked=a.checked)}function wa(a,b){return m.nodeName(a,"table")&&m.nodeName(11!==b.nodeType?b:b.firstChild,"tr")?a.getElementsByTagName("tbody")[0]||a.appendChild(a.ownerDocument.createElement("tbody")):a}function xa(a){return a.type=(null!==m.find.attr(a,"type"))+"/"+a.type,a}function ya(a){var b=pa.exec(a.type);return b?a.type=b[1]:a.removeAttribute("type"),a}function za(a,b){for(var c,d=0;null!=(c=a[d]);d++)m._data(c,"globalEval",!b||m._data(b[d],"globalEval"))}function Aa(a,b){if(1===b.nodeType&&m.hasData(a)){var c,d,e,f=m._data(a),g=m._data(b,f),h=f.events;if(h){delete g.handle,g.events={};for(c in h)for(d=0,e=h[c].length;e>d;d++)m.event.add(b,c,h[c][d])}g.data&&(g.data=m.extend({},g.data))}}function Ba(a,b){var c,d,e;if(1===b.nodeType){if(c=b.nodeName.toLowerCase(),!k.noCloneEvent&&b[m.expando]){e=m._data(b);for(d in e.events)m.removeEvent(b,d,e.handle);b.removeAttribute(m.expando)}"script"===c&&b.text!==a.text?(xa(b).text=a.text,ya(b)):"object"===c?(b.parentNode&&(b.outerHTML=a.outerHTML),k.html5Clone&&a.innerHTML&&!m.trim(b.innerHTML)&&(b.innerHTML=a.innerHTML)):"input"===c&&W.test(a.type)?(b.defaultChecked=b.checked=a.checked,b.value!==a.value&&(b.value=a.value)):"option"===c?b.defaultSelected=b.selected=a.defaultSelected:("input"===c||"textarea"===c)&&(b.defaultValue=a.defaultValue)}}m.extend({clone:function(a,b,c){var d,e,f,g,h,i=m.contains(a.ownerDocument,a);if(k.html5Clone||m.isXMLDoc(a)||!ga.test("<"+a.nodeName+">")?f=a.cloneNode(!0):(ta.innerHTML=a.outerHTML,ta.removeChild(f=ta.firstChild)),!(k.noCloneEvent&&k.noCloneChecked||1!==a.nodeType&&11!==a.nodeType||m.isXMLDoc(a)))for(d=ua(f),h=ua(a),g=0;null!=(e=h[g]);++g)d[g]&&Ba(e,d[g]);if(b)if(c)for(h=h||ua(a),d=d||ua(f),g=0;null!=(e=h[g]);g++)Aa(e,d[g]);else Aa(a,f);return d=ua(f,"script"),d.length>0&&za(d,!i&&ua(a,"script")),d=h=e=null,f},buildFragment:function(a,b,c,d){for(var e,f,g,h,i,j,l,n=a.length,o=da(b),p=[],q=0;n>q;q++)if(f=a[q],f||0===f)if("object"===m.type(f))m.merge(p,f.nodeType?[f]:f);else if(la.test(f)){h=h||o.appendChild(b.createElement("div")),i=(ja.exec(f)||["",""])[1].toLowerCase(),l=ra[i]||ra._default,h.innerHTML=l[1]+f.replace(ia,"<$1>")+l[2],e=l[0];while(e--)h=h.lastChild;if(!k.leadingWhitespace&&ha.test(f)&&p.push(b.createTextNode(ha.exec(f)[0])),!k.tbody){f="table"!==i||ka.test(f)?""!==l[1]||ka.test(f)?0:h:h.firstChild,e=f&&f.childNodes.length;while(e--)m.nodeName(j=f.childNodes[e],"tbody")&&!j.childNodes.length&&f.removeChild(j)}m.merge(p,h.childNodes),h.textContent="";while(h.firstChild)h.removeChild(h.firstChild);h=o.lastChild}else p.push(b.createTextNode(f));h&&o.removeChild(h),k.appendChecked||m.grep(ua(p,"input"),va),q=0;while(f=p[q++])if((!d||-1===m.inArray(f,d))&&(g=m.contains(f.ownerDocument,f),h=ua(o.appendChild(f),"script"),g&&za(h),c)){e=0;while(f=h[e++])oa.test(f.type||"")&&c.push(f)}return h=null,o},cleanData:function(a,b){for(var d,e,f,g,h=0,i=m.expando,j=m.cache,l=k.deleteExpando,n=m.event.special;null!=(d=a[h]);h++)if((b||m.acceptData(d))&&(f=d[i],g=f&&j[f])){if(g.events)for(e in g.events)n[e]?m.event.remove(d,e):m.removeEvent(d,e,g.handle);j[f]&&(delete j[f],l?delete d[i]:typeof d.removeAttribute!==K?d.removeAttribute(i):d[i]=null,c.push(f))}}}),m.fn.extend({text:function(a){return V(this,function(a){return void 0===a?m.text(this):this.empty().append((this[0]&&this[0].ownerDocument||y).createTextNode(a))},null,a,arguments.length)},append:function(){return this.domManip(arguments,function(a){if(1===this.nodeType||11===this.nodeType||9===this.nodeType){var b=wa(this,a);b.appendChild(a)}})},prepend:function(){return this.domManip(arguments,function(a){if(1===this.nodeType||11===this.nodeType||9===this.nodeType){var b=wa(this,a);b.insertBefore(a,b.firstChild)}})},before:function(){return this.domManip(arguments,function(a){this.parentNode&&this.parentNode.insertBefore(a,this)})},after:function(){return this.domManip(arguments,function(a){this.parentNode&&this.parentNode.insertBefore(a,this.nextSibling)})},remove:function(a,b){for(var c,d=a?m.filter(a,this):this,e=0;null!=(c=d[e]);e++)b||1!==c.nodeType||m.cleanData(ua(c)),c.parentNode&&(b&&m.contains(c.ownerDocument,c)&&za(ua(c,"script")),c.parentNode.removeChild(c));return this},empty:function(){for(var a,b=0;null!=(a=this[b]);b++){1===a.nodeType&&m.cleanData(ua(a,!1));while(a.firstChild)a.removeChild(a.firstChild);a.options&&m.nodeName(a,"select")&&(a.options.length=0)}return this},clone:function(a,b){return a=null==a?!1:a,b=null==b?a:b,this.map(function(){return m.clone(this,a,b)})},html:function(a){return V(this,function(a){var b=this[0]||{},c=0,d=this.length;if(void 0===a)return 1===b.nodeType?b.innerHTML.replace(fa,""):void 0;if(!("string"!=typeof a||ma.test(a)||!k.htmlSerialize&&ga.test(a)||!k.leadingWhitespace&&ha.test(a)||ra[(ja.exec(a)||["",""])[1].toLowerCase()])){a=a.replace(ia,"<$1>");try{for(;d>c;c++)b=this[c]||{},1===b.nodeType&&(m.cleanData(ua(b,!1)),b.innerHTML=a);b=0}catch(e){}}b&&this.empty().append(a)},null,a,arguments.length)},replaceWith:function(){var a=arguments[0];return this.domManip(arguments,function(b){a=this.parentNode,m.cleanData(ua(this)),a&&a.replaceChild(b,this)}),a&&(a.length||a.nodeType)?this:this.remove()},detach:function(a){return this.remove(a,!0)},domManip:function(a,b){a=e.apply([],a);var c,d,f,g,h,i,j=0,l=this.length,n=this,o=l-1,p=a[0],q=m.isFunction(p);if(q||l>1&&"string"==typeof p&&!k.checkClone&&na.test(p))return this.each(function(c){var d=n.eq(c);q&&(a[0]=p.call(this,c,d.html())),d.domManip(a,b)});if(l&&(i=m.buildFragment(a,this[0].ownerDocument,!1,this),c=i.firstChild,1===i.childNodes.length&&(i=c),c)){for(g=m.map(ua(i,"script"),xa),f=g.length;l>j;j++)d=i,j!==o&&(d=m.clone(d,!0,!0),f&&m.merge(g,ua(d,"script"))),b.call(this[j],d,j);if(f)for(h=g[g.length-1].ownerDocument,m.map(g,ya),j=0;f>j;j++)d=g[j],oa.test(d.type||"")&&!m._data(d,"globalEval")&&m.contains(h,d)&&(d.src?m._evalUrl&&m._evalUrl(d.src):m.globalEval((d.text||d.textContent||d.innerHTML||"").replace(qa,"")));i=c=null}return this}}),m.each({appendTo:"append",prependTo:"prepend",insertBefore:"before",insertAfter:"after",replaceAll:"replaceWith"},function(a,b){m.fn[a]=function(a){for(var c,d=0,e=[],g=m(a),h=g.length-1;h>=d;d++)c=d===h?this:this.clone(!0),m(g[d])[b](c),f.apply(e,c.get());return this.pushStack(e)}});var Ca,Da={};function Ea(b,c){var d,e=m(c.createElement(b)).appendTo(c.body),f=a.getDefaultComputedStyle&&(d=a.getDefaultComputedStyle(e[0]))?d.display:m.css(e[0],"display");return e.detach(),f}function Fa(a){var b=y,c=Da[a];return c||(c=Ea(a,b),"none"!==c&&c||(Ca=(Ca||m("'),t.object.on("click",function(e){e.stopPropagation()}).css("cursor","auto"),t.width&&t.height||(t.width="600",t.height="400");break;case"ajax":t.object=e('
'),t.object.on("click",function(e){e.stopPropagation()}).css("cursor","auto").css("overflow","auto"),t.width&&t.height||(t.width="600",t.height="400");break;case"soundcloud":t.object=e(''),t.src="//w.soundcloud.com/player/?url="+escape(t.src)+(t.options?"&"+t.options:""),t.width="600",t.height="166";break;case"youtube":t.object=e(''),t.src="//www.youtube.com/embed/"+p[1]+(t.options?"?"+t.options:""),t.width&&t.height||(t.width="800",t.height="480");break;case"vimeo":t.object=e(''),t.src="//player.vimeo.com/video/"+p[1]+(t.options?"?"+t.options:""),t.width&&t.height||(t.width="800",t.height="480");break;case"wistia":t.object=e(''),t.src="//fast.wistia.net/"+p[1]+(t.options?"?"+t.options:""),t.width&&t.height||(t.width="800",t.height="480");break;case"bcove":t.object=e(''),t.src="//bcove.me/"+p[1]+(t.options?"?"+t.options:""),t.width&&t.height||(t.width="640",t.height="360");break;default:if(t.object=e(''),r.preload){var p=document.createElement("img");p.src=t.src,f.push(p)}t.width=i.attr("width"),t.height=i.attr("height")}"file:"==window.location.protocol&&t.src.match(/^\/\//)&&(t.src="http:"+t.src),d.push(t),s.removeAttr("title"),i.removeAttr("href").css("cursor","pointer").css("outline",0).on("click",function(e){e.preventDefault(),e.stopPropagation(),x.trigger("poptrox_open",[o])})}}),n.prop("_poptrox",r),n}}(jQuery); diff --git a/assets/js/main.js b/assets/js/main.js new file mode 100755 index 00000000..33f7980d --- /dev/null +++ b/assets/js/main.js @@ -0,0 +1,5 @@ +addResults(dress, document.getElementById("dress")); +addResults(shirt, document.getElementById("shirt")); +addResults(large, document.getElementById("large")); +addResults(xlarge, document.getElementById("xlarge")); +updateVideoSpeed(2); \ No newline at end of file diff --git a/assets/js/skel.min.js b/assets/js/skel.min.js new file mode 100755 index 00000000..0e7633aa --- /dev/null +++ b/assets/js/skel.min.js @@ -0,0 +1,2 @@ +/* skel.js v3.0.1 | (c) skel.io | MIT licensed */ +var skel=function(){"use strict";var t={breakpointIds:null,events:{},isInit:!1,obj:{attachments:{},breakpoints:{},head:null,states:{}},sd:"/",state:null,stateHandlers:{},stateId:"",vars:{},DOMReady:null,indexOf:null,isArray:null,iterate:null,matchesMedia:null,extend:function(e,n){t.iterate(n,function(i){t.isArray(n[i])?(t.isArray(e[i])||(e[i]=[]),t.extend(e[i],n[i])):"object"==typeof n[i]?("object"!=typeof e[i]&&(e[i]={}),t.extend(e[i],n[i])):e[i]=n[i]})},newStyle:function(t){var e=document.createElement("style");return e.type="text/css",e.innerHTML=t,e},_canUse:null,canUse:function(e){t._canUse||(t._canUse=document.createElement("div"));var n=t._canUse.style,i=e.charAt(0).toUpperCase()+e.slice(1);return e in n||"Moz"+i in n||"Webkit"+i in n||"O"+i in n||"ms"+i in n},on:function(e,n){var i=e.split(/[\s]+/);return t.iterate(i,function(e){var a=i[e];if(t.isInit){if("init"==a)return void n();if("change"==a)n();else{var r=a.charAt(0);if("+"==r||"!"==r){var o=a.substring(1);if(o in t.obj.breakpoints)if("+"==r&&t.obj.breakpoints[o].active)n();else if("!"==r&&!t.obj.breakpoints[o].active)return void n()}}}t.events[a]||(t.events[a]=[]),t.events[a].push(n)}),t},trigger:function(e){return t.events[e]&&0!=t.events[e].length?(t.iterate(t.events[e],function(n){t.events[e][n]()}),t):void 0},breakpoint:function(e){return t.obj.breakpoints[e]},breakpoints:function(e){function n(t,e){this.name=this.id=t,this.media=e,this.active=!1,this.wasActive=!1}return n.prototype.matches=function(){return t.matchesMedia(this.media)},n.prototype.sync=function(){this.wasActive=this.active,this.active=this.matches()},t.iterate(e,function(i){t.obj.breakpoints[i]=new n(i,e[i])}),window.setTimeout(function(){t.poll()},0),t},addStateHandler:function(e,n){t.stateHandlers[e]=n},callStateHandler:function(e){var n=t.stateHandlers[e]();t.iterate(n,function(e){t.state.attachments.push(n[e])})},changeState:function(e){t.iterate(t.obj.breakpoints,function(e){t.obj.breakpoints[e].sync()}),t.vars.lastStateId=t.stateId,t.stateId=e,t.breakpointIds=t.stateId===t.sd?[]:t.stateId.substring(1).split(t.sd),t.obj.states[t.stateId]?t.state=t.obj.states[t.stateId]:(t.obj.states[t.stateId]={attachments:[]},t.state=t.obj.states[t.stateId],t.iterate(t.stateHandlers,t.callStateHandler)),t.detachAll(t.state.attachments),t.attachAll(t.state.attachments),t.vars.stateId=t.stateId,t.vars.state=t.state,t.trigger("change"),t.iterate(t.obj.breakpoints,function(e){t.obj.breakpoints[e].active?t.obj.breakpoints[e].wasActive||t.trigger("+"+e):t.obj.breakpoints[e].wasActive&&t.trigger("-"+e)})},generateStateConfig:function(e,n){var i={};return t.extend(i,e),t.iterate(t.breakpointIds,function(e){t.extend(i,n[t.breakpointIds[e]])}),i},getStateId:function(){var e="";return t.iterate(t.obj.breakpoints,function(n){var i=t.obj.breakpoints[n];i.matches()&&(e+=t.sd+i.id)}),e},poll:function(){var e="";e=t.getStateId(),""===e&&(e=t.sd),e!==t.stateId&&t.changeState(e)},_attach:null,attach:function(e){var n=t.obj.head,i=e.element;return i.parentNode&&i.parentNode.tagName?!1:(t._attach||(t._attach=n.firstChild),n.insertBefore(i,t._attach.nextSibling),e.permanent&&(t._attach=i),!0)},attachAll:function(e){var n=[];t.iterate(e,function(t){n[e[t].priority]||(n[e[t].priority]=[]),n[e[t].priority].push(e[t])}),n.reverse(),t.iterate(n,function(e){t.iterate(n[e],function(i){t.attach(n[e][i])})})},detach:function(t){var e=t.element;return t.permanent||!e.parentNode||e.parentNode&&!e.parentNode.tagName?!1:(e.parentNode.removeChild(e),!0)},detachAll:function(e){var n={};t.iterate(e,function(t){n[e[t].id]=!0}),t.iterate(t.obj.attachments,function(e){e in n||t.detach(t.obj.attachments[e])})},attachment:function(e){return e in t.obj.attachments?t.obj.attachments[e]:null},newAttachment:function(e,n,i,a){return t.obj.attachments[e]={id:e,element:n,priority:i,permanent:a}},init:function(){t.initMethods(),t.initVars(),t.initEvents(),t.obj.head=document.getElementsByTagName("head")[0],t.isInit=!0,t.trigger("init")},initEvents:function(){t.on("resize",function(){t.poll()}),t.on("orientationChange",function(){t.poll()}),t.DOMReady(function(){t.trigger("ready")}),window.onload&&t.on("load",window.onload),window.onload=function(){t.trigger("load")},window.onresize&&t.on("resize",window.onresize),window.onresize=function(){t.trigger("resize")},window.onorientationchange&&t.on("orientationChange",window.onorientationchange),window.onorientationchange=function(){t.trigger("orientationChange")}},initMethods:function(){document.addEventListener?!function(e,n){t.DOMReady=n()}("domready",function(){function t(t){for(r=1;t=n.shift();)t()}var e,n=[],i=document,a="DOMContentLoaded",r=/^loaded|^c/.test(i.readyState);return i.addEventListener(a,e=function(){i.removeEventListener(a,e),t()}),function(t){r?t():n.push(t)}}):!function(e,n){t.DOMReady=n()}("domready",function(t){function e(t){for(h=1;t=i.shift();)t()}var n,i=[],a=!1,r=document,o=r.documentElement,s=o.doScroll,c="DOMContentLoaded",d="addEventListener",u="onreadystatechange",l="readyState",f=s?/^loaded|^c/:/^loaded|c/,h=f.test(r[l]);return r[d]&&r[d](c,n=function(){r.removeEventListener(c,n,a),e()},a),s&&r.attachEvent(u,n=function(){/^c/.test(r[l])&&(r.detachEvent(u,n),e())}),t=s?function(e){self!=top?h?e():i.push(e):function(){try{o.doScroll("left")}catch(n){return setTimeout(function(){t(e)},50)}e()}()}:function(t){h?t():i.push(t)}}),Array.prototype.indexOf?t.indexOf=function(t,e){return t.indexOf(e)}:t.indexOf=function(t,e){if("string"==typeof t)return t.indexOf(e);var n,i,a=e?e:0;if(!this)throw new TypeError;if(i=this.length,0===i||a>=i)return-1;for(0>a&&(a=i-Math.abs(a)),n=a;i>n;n++)if(this[n]===t)return n;return-1},Array.isArray?t.isArray=function(t){return Array.isArray(t)}:t.isArray=function(t){return"[object Array]"===Object.prototype.toString.call(t)},Object.keys?t.iterate=function(t,e){if(!t)return[];var n,i=Object.keys(t);for(n=0;i[n]&&e(i[n],t[i[n]])!==!1;n++);}:t.iterate=function(t,e){if(!t)return[];var n;for(n in t)if(Object.prototype.hasOwnProperty.call(t,n)&&e(n,t[n])===!1)break},window.matchMedia?t.matchesMedia=function(t){return""==t?!0:window.matchMedia(t).matches}:window.styleMedia||window.media?t.matchesMedia=function(t){if(""==t)return!0;var e=window.styleMedia||window.media;return e.matchMedium(t||"all")}:window.getComputedStyle?t.matchesMedia=function(t){if(""==t)return!0;var e=document.createElement("style"),n=document.getElementsByTagName("script")[0],i=null;e.type="text/css",e.id="matchmediajs-test",n.parentNode.insertBefore(e,n),i="getComputedStyle"in window&&window.getComputedStyle(e,null)||e.currentStyle;var a="@media "+t+"{ #matchmediajs-test { width: 1px; } }";return e.styleSheet?e.styleSheet.cssText=a:e.textContent=a,"1px"===i.width}:t.matchesMedia=function(t){if(""==t)return!0;var e,n,i,a,r={"min-width":null,"max-width":null},o=!1;for(i=t.split(/\s+and\s+/),e=0;er["max-width"]||null!==r["min-height"]&&cr["max-height"]?!1:!0},navigator.userAgent.match(/MSIE ([0-9]+)/)&&RegExp.$1<9&&(t.newStyle=function(t){var e=document.createElement("span");return e.innerHTML=' ",e})},initVars:function(){var e,n,i,a=navigator.userAgent;e="other",n=0,i=[["firefox",/Firefox\/([0-9\.]+)/],["bb",/BlackBerry.+Version\/([0-9\.]+)/],["bb",/BB[0-9]+.+Version\/([0-9\.]+)/],["opera",/OPR\/([0-9\.]+)/],["opera",/Opera\/([0-9\.]+)/],["edge",/Edge\/([0-9\.]+)/],["safari",/Version\/([0-9\.]+).+Safari/],["chrome",/Chrome\/([0-9\.]+)/],["ie",/MSIE ([0-9]+)/],["ie",/Trident\/.+rv:([0-9]+)/]],t.iterate(i,function(t,i){return a.match(i[1])?(e=i[0],n=parseFloat(RegExp.$1),!1):void 0}),t.vars.browser=e,t.vars.browserVersion=n,e="other",n=0,i=[["ios",/([0-9_]+) like Mac OS X/,function(t){return t.replace("_",".").replace("_","")}],["ios",/CPU like Mac OS X/,function(t){return 0}],["wp",/Windows Phone ([0-9\.]+)/,null],["android",/Android ([0-9\.]+)/,null],["mac",/Macintosh.+Mac OS X ([0-9_]+)/,function(t){return t.replace("_",".").replace("_","")}],["windows",/Windows NT ([0-9\.]+)/,null],["bb",/BlackBerry.+Version\/([0-9\.]+)/,null],["bb",/BB[0-9]+.+Version\/([0-9\.]+)/,null]],t.iterate(i,function(t,i){return a.match(i[1])?(e=i[0],n=parseFloat(i[2]?i[2](RegExp.$1):RegExp.$1),!1):void 0}),t.vars.os=e,t.vars.osVersion=n,t.vars.IEVersion="ie"==t.vars.browser?t.vars.browserVersion:99,t.vars.touch="wp"==t.vars.os?navigator.msMaxTouchPoints>0:!!("ontouchstart"in window),t.vars.mobile="wp"==t.vars.os||"android"==t.vars.os||"ios"==t.vars.os||"bb"==t.vars.os}};return t.init(),t}();!function(t,e){"function"==typeof define&&define.amd?define([],e):"object"==typeof exports?module.exports=e():t.skel=e()}(this,function(){return skel}); diff --git a/assets/js/util.js b/assets/js/util.js new file mode 100644 index 00000000..912b2367 --- /dev/null +++ b/assets/js/util.js @@ -0,0 +1,235 @@ +let resultsContainer = document.getElementById("realWorldResults"); +let rootPath = "videos"; +let transparentize = (color, opacity) => { + var alpha = opacity === undefined ? 0.5 : 1 - opacity; + return Color(color).alpha(alpha).rgbString(); +} +let animateBox = false; +let animSpeed = 0.1; +let colors = ['#ff6361', '#003f5c', '#ffa600', '#45B39D']; +let pad = false; +let lerp = (v0, v1, t) => { + return v0 * (1 - t) + v1 * t +} +let range = (start, stop, step) => { + if (typeof stop == 'undefined') { + // one param defined + stop = start; + start = 0; + } + + if (typeof step == 'undefined') { + step = 1; + } + + if ((step > 0 && start >= stop) || (step < 0 && start <= stop)) { + return []; + } + + var result = []; + for (var i = start; step > 0 ? i < stop : i > stop; i += step) { + result.push(i); + } + + return result; +}; +Chart.defaults.global.defaultFontSize = 12; +let createCoveragePlot = (ctx, datasets, videoId, steps, showLegend = false) => { + labels = pad ? [1, 2, 3, 4, 5, 6, 7, 8, 9] : range(0, datasets[0].data.length - 1); + labels.unshift('Init'); + let setupVideo = document.getElementById(`${videoId}`); + showLegend = datasets.length > 1; + let config = { + type: 'line', + data: { + labels: labels, + datasets: datasets + }, + options: { + legend: { + display: showLegend + }, + layout: { + padding: { + left: 0, + right: 0, + top: 20, + bottom: 0 + } + }, + tooltips: { + enabled: true, + mode: 'nearest', + intersect: false + }, + maintainAspectRatio: false, + spanGaps: false, + elements: { + line: { + tension: 0.000001 + } + }, + plugins: { + filler: { + propagate: false + }, + annotation: { + drawTime: 'afterDatasetsDraw', + dblClickSpeed: 350, + annotations: [{ + drawTime: 'afterDraw', + display: true, + type: 'box', + scaleID: 'y', + value: '25', + borderColor: 'red', + borderWidth: 2 + }] + } + }, + scales: { + xAxes: [{ + ticks: { + autoSkip: false, + maxRotation: 0, + padding: 0, + }, + scaleLabel: { + display: true, + labelString: "Episode Steps" + } + }], + yAxes: [{ + ticks: { + beginAtZero: false, + stepSize: 0.2, + suggestedMax: 1.0, + suggestedMin: 0.1, + padding: 0, + }, + scaleLabel: { + display: true, + labelString: "Coverage (%)" + } + }] + }, + responsive: false, + annotation: { + annotations: [{ + type: 'box', + events: [], + display: true, + drawTime: 'afterDatasetsDraw', + xScaleID: 'x-axis-0', + yScaleID: 'y-axis-0', + xMin: -2, + xMax: -1, + yMax: 1, + yMin: 0, + borderColor: 'grey', + borderWidth: 0, + backgroundColor: transparentize('grey', 0.7), + cornerRadius: 0 + }] + }, + onClick: function (e, a) { + // Move video to x axis step clicked + let step = this.chart.scales['x-axis-0'].getValueForPixel(e.offsetX); + let time = steps[step].start; + // decode to time + setupVideo.currentTime = time; + topVideo.currentTime = time; + }, + onHover: (e) => { + e.target.style.cursor = 'pointer'; + } + } + }; + let chart = new Chart(ctx, config); + chart.video = setupVideo; + chart.steps = steps; + chart.step = -1; + return chart; +}; + +let addResults = (collection, parentNode = resultsContainer) => { + collection.forEach(item => { + let container = document.createElement("div"); + container.style.display = "flex"; + container.style.flexDirection = "column"; + container.style.margin = "10px"; + container.style.position = "relative"; + // container.style.width = "380px"; + container.style.width = "320px"; + let videoPath = item.path; + let coverages = item.coverages; + while (coverages.length < 10 && pad) { + coverages.push(coverages[coverages.length - 1]); + } + + let color = item.color; + let steps = item.steps; + let name = item.name + let itemId = videoPath.replaceAll("/", "-"); + let canvasId = `${itemId}-canvas`; + let videoId = `${itemId}`; + container.insertAdjacentHTML("beforeend", `

${name}

2x

`); + parentNode.appendChild(container); + let chart = createCoveragePlot( + document.getElementById(canvasId).getContext('2d'), + [{ + backgroundColor: transparentize(color, 0.6), + borderColor: color, + data: coverages, + label: name, + fill: 'start' + }], + videoId, steps + ); + let setupVideo = document.getElementById(`${videoId}`); + setupVideo.ontimeupdate = () => { + if (steps != undefined) { + // find current step + for (let i = 0; i < steps.length; ++i) { + if (steps[i].start < setupVideo.currentTime && steps[i].end > setupVideo.currentTime) { + // currently at ith step + if (chart.step == i) { + break; + } + chart.step = i; + if (animateBox) { + // Slows down after a while, need to look into this + chart.animId = setInterval(() => { + let box = chart.options.annotation.annotations[0]; + box.xMin = lerp(box.xMin, chart.step - 1, animSpeed); + box.xMax = lerp(box.xMax, chart.step, animSpeed); + if (Math.abs(box.xMin - chart.step - 1) > 0.001) { + chart.update(); + } else { + clearInterval(chart.animId); + } + }, 5); + } else { + let box = chart.options.annotation.annotations[0]; + box.xMin = chart.step - 1; + box.xMax = chart.step; + chart.update() + } + break + } + } + } + } + }); +}; + + +function updateVideoSpeed(speed) { + let vids = document.getElementsByTagName('video'); + // vids is an HTMLCollection + for (let i = 0; i < vids.length; i++) { + if (vids.item(i).id != '1x_speed'){ + vids.item(i).playbackRate = speed; + } + } +} \ No newline at end of file diff --git a/Assets/pic-ProblemFormulation.svg b/assets/pic-ProblemFormulation.svg similarity index 100% rename from Assets/pic-ProblemFormulation.svg rename to assets/pic-ProblemFormulation.svg diff --git a/Assets/pic-TrajectoryFormulation.svg b/assets/pic-TrajectoryFormulation.svg similarity index 100% rename from Assets/pic-TrajectoryFormulation.svg rename to assets/pic-TrajectoryFormulation.svg diff --git a/assets/sass/ie8.scss b/assets/sass/ie8.scss new file mode 100755 index 00000000..5a3822c4 --- /dev/null +++ b/assets/sass/ie8.scss @@ -0,0 +1,87 @@ +@import 'libs/vars'; +@import 'libs/functions'; +@import 'libs/mixins'; +@import 'libs/skel'; + +/* + Strata by HTML5 UP + html5up.net | @ajlkn + Free for personal and commercial use under the CCA 3.0 license (html5up.net/license) +*/ + +/* Button */ + + input[type="submit"], + input[type="reset"], + input[type="button"], + .button { + position: relative; + -ms-behavior: url('assets/js/ie/PIE.htc'); + } + +/* Form */ + + input[type="text"], + input[type="password"], + input[type="email"], + select, + textarea { + position: relative; + -ms-behavior: url('assets/js/ie/PIE.htc'); + } + + input[type="text"], + input[type="password"], + input[type="email"], + select { + height: _size(element-height); + line-height: _size(element-height); + } + + input[type="checkbox"], + input[type="radio"] { + & + label { + &:before { + display: none; + } + } + } + +/* Image */ + + .image { + position: relative; + -ms-behavior: url('assets/js/ie/PIE.htc'); + + &:before, &:after { + display: none !important; + } + + img { + position: relative; + -ms-behavior: url('assets/js/ie/PIE.htc'); + } + } + +/* Header */ + + #header { + background-image: url('../../images/bg.jpg'); + background-repeat: no-repeat; + background-size: cover; + -ms-behavior: url('assets/js/ie/backgroundsize.min.htc'); + + h1 { + color: _palette(accent2, fg-bold); + } + } + +/* Footer */ + + #footer { + .icons { + a { + color: _palette(accent2, fg-bold); + } + } + } \ No newline at end of file diff --git a/assets/sass/libs/_functions.scss b/assets/sass/libs/_functions.scss new file mode 100755 index 00000000..0e08c1a6 --- /dev/null +++ b/assets/sass/libs/_functions.scss @@ -0,0 +1,34 @@ +/// Gets a duration value. +/// @param {string} $keys Key(s). +/// @return {string} Value. +@function _duration($keys...) { + @return val($duration, $keys...); +} + +/// Gets a font value. +/// @param {string} $keys Key(s). +/// @return {string} Value. +@function _font($keys...) { + @return val($font, $keys...); +} + +/// Gets a misc value. +/// @param {string} $keys Key(s). +/// @return {string} Value. +@function _misc($keys...) { + @return val($misc, $keys...); +} + +/// Gets a palette value. +/// @param {string} $keys Key(s). +/// @return {string} Value. +@function _palette($keys...) { + @return val($palette, $keys...); +} + +/// Gets a size value. +/// @param {string} $keys Key(s). +/// @return {string} Value. +@function _size($keys...) { + @return val($size, $keys...); +} \ No newline at end of file diff --git a/assets/sass/libs/_mixins.scss b/assets/sass/libs/_mixins.scss new file mode 100755 index 00000000..d4bf3c84 --- /dev/null +++ b/assets/sass/libs/_mixins.scss @@ -0,0 +1,398 @@ +/// Makes an element's :before pseudoelement a FontAwesome icon. +/// @param {string} $content Optional content value to use. +/// @param {string} $where Optional pseudoelement to target (before or after). +@mixin icon($content: false, $where: before) { + + text-decoration: none; + + &:#{$where} { + + @if $content { + content: $content; + } + + -moz-osx-font-smoothing: grayscale; + -webkit-font-smoothing: antialiased; + font-family: FontAwesome; + font-style: normal; + font-weight: normal; + text-transform: none !important; + + } + +} + +/// Applies padding to an element, taking the current element-margin value into account. +/// @param {mixed} $tb Top/bottom padding. +/// @param {mixed} $lr Left/right padding. +/// @param {list} $pad Optional extra padding (in the following order top, right, bottom, left) +/// @param {bool} $important If true, adds !important. +@mixin padding($tb, $lr, $pad: (0,0,0,0), $important: null) { + + @if $important { + $important: '!important'; + } + + $x: 0.1em; + + @if unit(_size(element-margin)) == 'rem' { + $x: 0.1rem; + } + + padding: ($tb + nth($pad,1)) ($lr + nth($pad,2)) max($x, $tb - _size(element-margin) + nth($pad,3)) ($lr + nth($pad,4)) #{$important}; + +} + +/// Encodes a SVG data URL so IE doesn't choke (via codepen.io/jakob-e/pen/YXXBrp). +/// @param {string} $svg SVG data URL. +/// @return {string} Encoded SVG data URL. +@function svg-url($svg) { + + $svg: str-replace($svg, '"', '\''); + $svg: str-replace($svg, '%', '%25'); + $svg: str-replace($svg, '<', '%3C'); + $svg: str-replace($svg, '>', '%3E'); + $svg: str-replace($svg, '&', '%26'); + $svg: str-replace($svg, '#', '%23'); + $svg: str-replace($svg, '{', '%7B'); + $svg: str-replace($svg, '}', '%7D'); + $svg: str-replace($svg, ';', '%3B'); + + @return url("data:image/svg+xml;charset=utf8,#{$svg}"); + +} + +/// Initializes base flexgrid classes. +/// @param {string} $vertical-align Vertical alignment of cells. +/// @param {string} $horizontal-align Horizontal alignment of cells. +@mixin flexgrid-base($vertical-align: null, $horizontal-align: null) { + + // Grid. + @include vendor('display', 'flex'); + @include vendor('flex-wrap', 'wrap'); + + // Vertical alignment. + @if ($vertical-align == top) { + @include vendor('align-items', 'flex-start'); + } + @else if ($vertical-align == bottom) { + @include vendor('align-items', 'flex-end'); + } + @else if ($vertical-align == center) { + @include vendor('align-items', 'center'); + } + @else { + @include vendor('align-items', 'stretch'); + } + + // Horizontal alignment. + @if ($horizontal-align != null) { + text-align: $horizontal-align; + } + + // Cells. + > * { + @include vendor('flex-shrink', '1'); + @include vendor('flex-grow', '0'); + } + +} + +/// Sets up flexgrid columns. +/// @param {integer} $columns Columns. +@mixin flexgrid-columns($columns) { + + > * { + $cell-width: 100% / $columns; + width: #{$cell-width}; + } + +} + +/// Sets up flexgrid gutters. +/// @param {integer} $columns Columns. +/// @param {number} $gutters Gutters. +@mixin flexgrid-gutters($columns, $gutters) { + + // Apply padding. + > * { + $cell-width: 100% / $columns; + + padding: ($gutters * 0.5); + width: $cell-width; + } + +} + +/// Sets up flexgrid gutters (flush). +/// @param {integer} $columns Columns. +/// @param {number} $gutters Gutters. +@mixin flexgrid-gutters-flush($columns, $gutters) { + + // Apply padding. + > * { + $cell-width: 100% / $columns; + $cell-width-pad: $gutters / $columns; + + padding: ($gutters * 0.5); + width: calc(#{$cell-width} + #{$cell-width-pad}); + } + + // Clear top/bottom gutters. + > :nth-child(-n + #{$columns}) { + padding-top: 0; + } + + > :nth-last-child(-n + #{$columns}) { + padding-bottom: 0; + } + + // Clear left/right gutters. + > :nth-child(#{$columns}n + 1) { + padding-left: 0; + } + + > :nth-child(#{$columns}n) { + padding-right: 0; + } + + // Adjust widths of leftmost and rightmost cells. + > :nth-child(#{$columns}n + 1), + > :nth-child(#{$columns}n) { + $cell-width: 100% / $columns; + $cell-width-pad: ($gutters / $columns) - ($gutters / 2); + + width: calc(#{$cell-width} + #{$cell-width-pad}); + } + +} + +/// Reset flexgrid gutters (flush only). +/// Used to override a previous set of flexgrid gutter classes. +/// @param {integer} $columns Columns. +/// @param {number} $gutters Gutters. +/// @param {integer} $prev-columns Previous columns. +@mixin flexgrid-gutters-flush-reset($columns, $gutters, $prev-columns) { + + // Apply padding. + > * { + $cell-width: 100% / $prev-columns; + $cell-width-pad: $gutters / $prev-columns; + + padding: ($gutters * 0.5); + width: calc(#{$cell-width} + #{$cell-width-pad}); + } + + // Clear top/bottom gutters. + > :nth-child(-n + #{$prev-columns}) { + padding-top: ($gutters * 0.5); + } + + > :nth-last-child(-n + #{$prev-columns}) { + padding-bottom: ($gutters * 0.5); + } + + // Clear left/right gutters. + > :nth-child(#{$prev-columns}n + 1) { + padding-left: ($gutters * 0.5); + } + + > :nth-child(#{$prev-columns}n) { + padding-right: ($gutters * 0.5); + } + + // Adjust widths of leftmost and rightmost cells. + > :nth-child(#{$prev-columns}n + 1), + > :nth-child(#{$prev-columns}n) { + $cell-width: 100% / $columns; + $cell-width-pad: $gutters / $columns; + + padding: ($gutters * 0.5); + width: calc(#{$cell-width} + #{$cell-width-pad}); + } + +} + +/// Adds debug styles to current flexgrid element. +@mixin flexgrid-debug() { + + box-shadow: 0 0 0 1px red; + + > * { + box-shadow: inset 0 0 0 1px blue; + position: relative; + + > * { + position: relative; + box-shadow: inset 0 0 0 1px green; + } + } + +} + +/// Initializes the current element as a flexgrid. +/// @param {integer} $columns Columns (optional). +/// @param {number} $gutters Gutters (optional). +/// @param {bool} $flush If true, clears padding around the very edge of the grid. +@mixin flexgrid($settings: ()) { + + // Settings. + + // Debug. + $debug: false; + + @if (map-has-key($settings, 'debug')) { + $debug: map-get($settings, 'debug'); + } + + // Vertical align. + $vertical-align: null; + + @if (map-has-key($settings, 'vertical-align')) { + $vertical-align: map-get($settings, 'vertical-align'); + } + + // Horizontal align. + $horizontal-align: null; + + @if (map-has-key($settings, 'horizontal-align')) { + $horizontal-align: map-get($settings, 'horizontal-align'); + } + + // Columns. + $columns: null; + + @if (map-has-key($settings, 'columns')) { + $columns: map-get($settings, 'columns'); + } + + // Gutters. + $gutters: 0; + + @if (map-has-key($settings, 'gutters')) { + $gutters: map-get($settings, 'gutters'); + } + + // Flush. + $flush: true; + + @if (map-has-key($settings, 'flush')) { + $flush: map-get($settings, 'flush'); + } + + // Initialize base grid. + @include flexgrid-base($vertical-align, $horizontal-align); + + // Debug? + @if ($debug) { + @include flexgrid-debug; + } + + // Columns specified? + @if ($columns != null) { + + // Initialize columns. + @include flexgrid-columns($columns); + + // Gutters specified? + @if ($gutters > 0) { + + // Flush gutters? + @if ($flush) { + + // Initialize gutters (flush). + @include flexgrid-gutters-flush($columns, $gutters); + + } + + // Otherwise ... + @else { + + // Initialize gutters. + @include flexgrid-gutters($columns, $gutters); + + } + + } + + } + +} + +/// Resizes a previously-initialized grid. +/// @param {integer} $columns Columns. +/// @param {number} $gutters Gutters (optional). +/// @param {list} $reset A list of previously-initialized grid columns (only if $flush is true). +/// @param {bool} $flush If true, clears padding around the very edge of the grid. +@mixin flexgrid-resize($settings: ()) { + + // Settings. + + // Columns. + $columns: 1; + + @if (map-has-key($settings, 'columns')) { + $columns: map-get($settings, 'columns'); + } + + // Gutters. + $gutters: 0; + + @if (map-has-key($settings, 'gutters')) { + $gutters: map-get($settings, 'gutters'); + } + + // Previous columns. + $prev-columns: false; + + @if (map-has-key($settings, 'prev-columns')) { + $prev-columns: map-get($settings, 'prev-columns'); + } + + // Flush. + $flush: true; + + @if (map-has-key($settings, 'flush')) { + $flush: map-get($settings, 'flush'); + } + + // Resize columns. + @include flexgrid-columns($columns); + + // Gutters specified? + @if ($gutters > 0) { + + // Flush gutters? + @if ($flush) { + + // Previous columns specified? + @if ($prev-columns) { + + // Convert to list if it isn't one already. + @if (type-of($prev-columns) != list) { + $prev-columns: ($prev-columns); + } + + // Step through list of previous columns and reset them. + @each $x in $prev-columns { + @include flexgrid-gutters-flush-reset($columns, $gutters, $x); + } + + } + + // Resize gutters (flush). + @include flexgrid-gutters-flush($columns, $gutters); + + } + + // Otherwise ... + @else { + + // Resize gutters. + @include flexgrid-gutters($columns, $gutters); + + } + + } + +} \ No newline at end of file diff --git a/assets/sass/libs/_skel.scss b/assets/sass/libs/_skel.scss new file mode 100755 index 00000000..f5e0dcd1 --- /dev/null +++ b/assets/sass/libs/_skel.scss @@ -0,0 +1,587 @@ +// skel.scss v3.0.2-dev | (c) skel.io | MIT licensed */ + +// Vars. + + /// Breakpoints. + /// @var {list} + $breakpoints: () !global; + + /// Vendor prefixes. + /// @var {list} + $vendor-prefixes: ( + '-moz-', + '-webkit-', + '-ms-', + '' + ); + + /// Properties that should be vendorized. + /// @var {list} + $vendor-properties: ( + 'align-content', + 'align-items', + 'align-self', + 'animation', + 'animation-delay', + 'animation-direction', + 'animation-duration', + 'animation-fill-mode', + 'animation-iteration-count', + 'animation-name', + 'animation-play-state', + 'animation-timing-function', + 'appearance', + 'backface-visibility', + 'box-sizing', + 'filter', + 'flex', + 'flex-basis', + 'flex-direction', + 'flex-flow', + 'flex-grow', + 'flex-shrink', + 'flex-wrap', + 'justify-content', + 'object-fit', + 'object-position', + 'order', + 'perspective', + 'pointer-events', + 'transform', + 'transform-origin', + 'transform-style', + 'transition', + 'transition-delay', + 'transition-duration', + 'transition-property', + 'transition-timing-function', + 'user-select' + ); + + /// Values that should be vendorized. + /// @var {list} + $vendor-values: ( + 'filter', + 'flex', + 'linear-gradient', + 'radial-gradient', + 'transform' + ); + +// Functions. + + /// Removes a specific item from a list. + /// @author Hugo Giraudel + /// @param {list} $list List. + /// @param {integer} $index Index. + /// @return {list} Updated list. + @function remove-nth($list, $index) { + + $result: null; + + @if type-of($index) != number { + @warn "$index: #{quote($index)} is not a number for `remove-nth`."; + } + @else if $index == 0 { + @warn "List index 0 must be a non-zero integer for `remove-nth`."; + } + @else if abs($index) > length($list) { + @warn "List index is #{$index} but list is only #{length($list)} item long for `remove-nth`."; + } + @else { + + $result: (); + $index: if($index < 0, length($list) + $index + 1, $index); + + @for $i from 1 through length($list) { + + @if $i != $index { + $result: append($result, nth($list, $i)); + } + + } + + } + + @return $result; + + } + + /// Replaces a substring within another string. + /// @author Hugo Giraudel + /// @param {string} $string String. + /// @param {string} $search Substring. + /// @param {string} $replace Replacement. + /// @return {string} Updated string. + @function str-replace($string, $search, $replace: '') { + + $index: str-index($string, $search); + + @if $index { + @return str-slice($string, 1, $index - 1) + $replace + str-replace(str-slice($string, $index + str-length($search)), $search, $replace); + } + + @return $string; + + } + + /// Replaces a substring within each string in a list. + /// @param {list} $strings List of strings. + /// @param {string} $search Substring. + /// @param {string} $replace Replacement. + /// @return {list} Updated list of strings. + @function str-replace-all($strings, $search, $replace: '') { + + @each $string in $strings { + $strings: set-nth($strings, index($strings, $string), str-replace($string, $search, $replace)); + } + + @return $strings; + + } + + /// Gets a value from a map. + /// @author Hugo Giraudel + /// @param {map} $map Map. + /// @param {string} $keys Key(s). + /// @return {string} Value. + @function val($map, $keys...) { + + @if nth($keys, 1) == null { + $keys: remove-nth($keys, 1); + } + + @each $key in $keys { + $map: map-get($map, $key); + } + + @return $map; + + } + +// Mixins. + + /// Sets the global box model. + /// @param {string} $model Model (default is content). + @mixin boxModel($model: 'content') { + + $x: $model + '-box'; + + *, *:before, *:after { + -moz-box-sizing: #{$x}; + -webkit-box-sizing: #{$x}; + box-sizing: #{$x}; + } + + } + + /// Wraps @content in a @media block using a given breakpoint. + /// @param {string} $breakpoint Breakpoint. + /// @param {map} $queries Additional queries. + @mixin breakpoint($breakpoint: null, $queries: null) { + + $query: 'screen'; + + // Breakpoint. + @if $breakpoint and map-has-key($breakpoints, $breakpoint) { + $query: $query + ' and ' + map-get($breakpoints, $breakpoint); + } + + // Queries. + @if $queries { + @each $k, $v in $queries { + $query: $query + ' and (' + $k + ':' + $v + ')'; + } + } + + @media #{$query} { + @content; + } + + } + + /// Wraps @content in a @media block targeting a specific orientation. + /// @param {string} $orientation Orientation. + @mixin orientation($orientation) { + @media screen and (orientation: #{$orientation}) { + @content; + } + } + + /// Utility mixin for containers. + /// @param {mixed} $width Width. + @mixin containers($width) { + + // Locked? + $lock: false; + + @if length($width) == 2 { + $width: nth($width, 1); + $lock: true; + } + + // Modifiers. + .container.\31 25\25 { width: 100%; max-width: $width * 1.25; min-width: $width; } + .container.\37 5\25 { width: $width * 0.75; } + .container.\35 0\25 { width: $width * 0.5; } + .container.\32 5\25 { width: $width * 0.25; } + + // Main class. + .container { + @if $lock { + width: $width !important; + } + @else { + width: $width; + } + } + + } + + /// Utility mixin for grid. + /// @param {list} $gutters Column and row gutters (default is 40px). + /// @param {string} $breakpointName Optional breakpoint name. + @mixin grid($gutters: 40px, $breakpointName: null) { + + // Gutters. + @include grid-gutters($gutters); + @include grid-gutters($gutters, \32 00\25, 2); + @include grid-gutters($gutters, \31 50\25, 1.5); + @include grid-gutters($gutters, \35 0\25, 0.5); + @include grid-gutters($gutters, \32 5\25, 0.25); + + // Cells. + $x: ''; + + @if $breakpointName { + $x: '\\28' + $breakpointName + '\\29'; + } + + .\31 2u#{$x}, .\31 2u\24#{$x} { width: 100%; clear: none; margin-left: 0; } + .\31 1u#{$x}, .\31 1u\24#{$x} { width: 91.6666666667%; clear: none; margin-left: 0; } + .\31 0u#{$x}, .\31 0u\24#{$x} { width: 83.3333333333%; clear: none; margin-left: 0; } + .\39 u#{$x}, .\39 u\24#{$x} { width: 75%; clear: none; margin-left: 0; } + .\38 u#{$x}, .\38 u\24#{$x} { width: 66.6666666667%; clear: none; margin-left: 0; } + .\37 u#{$x}, .\37 u\24#{$x} { width: 58.3333333333%; clear: none; margin-left: 0; } + .\36 u#{$x}, .\36 u\24#{$x} { width: 50%; clear: none; margin-left: 0; } + .\35 u#{$x}, .\35 u\24#{$x} { width: 41.6666666667%; clear: none; margin-left: 0; } + .\34 u#{$x}, .\34 u\24#{$x} { width: 33.3333333333%; clear: none; margin-left: 0; } + .\33 u#{$x}, .\33 u\24#{$x} { width: 25%; clear: none; margin-left: 0; } + .\32 u#{$x}, .\32 u\24#{$x} { width: 16.6666666667%; clear: none; margin-left: 0; } + .\31 u#{$x}, .\31 u\24#{$x} { width: 8.3333333333%; clear: none; margin-left: 0; } + + .\31 2u\24#{$x} + *, + .\31 1u\24#{$x} + *, + .\31 0u\24#{$x} + *, + .\39 u\24#{$x} + *, + .\38 u\24#{$x} + *, + .\37 u\24#{$x} + *, + .\36 u\24#{$x} + *, + .\35 u\24#{$x} + *, + .\34 u\24#{$x} + *, + .\33 u\24#{$x} + *, + .\32 u\24#{$x} + *, + .\31 u\24#{$x} + * { + clear: left; + } + + .\-11u#{$x} { margin-left: 91.6666666667% } + .\-10u#{$x} { margin-left: 83.3333333333% } + .\-9u#{$x} { margin-left: 75% } + .\-8u#{$x} { margin-left: 66.6666666667% } + .\-7u#{$x} { margin-left: 58.3333333333% } + .\-6u#{$x} { margin-left: 50% } + .\-5u#{$x} { margin-left: 41.6666666667% } + .\-4u#{$x} { margin-left: 33.3333333333% } + .\-3u#{$x} { margin-left: 25% } + .\-2u#{$x} { margin-left: 16.6666666667% } + .\-1u#{$x} { margin-left: 8.3333333333% } + + } + + /// Utility mixin for grid. + /// @param {list} $gutters Gutters. + /// @param {string} $class Optional class name. + /// @param {integer} $multiplier Multiplier (default is 1). + @mixin grid-gutters($gutters, $class: null, $multiplier: 1) { + + // Expand gutters if it's not a list. + @if length($gutters) == 1 { + $gutters: ($gutters, 0); + } + + // Get column and row gutter values. + $c: nth($gutters, 1); + $r: nth($gutters, 2); + + // Get class (if provided). + $x: ''; + + @if $class { + $x: '.' + $class; + } + + // Default. + .row#{$x} > * { padding: ($r * $multiplier) 0 0 ($c * $multiplier); } + .row#{$x} { margin: ($r * $multiplier * -1) 0 -1px ($c * $multiplier * -1); } + + // Uniform. + .row.uniform#{$x} > * { padding: ($c * $multiplier) 0 0 ($c * $multiplier); } + .row.uniform#{$x} { margin: ($c * $multiplier * -1) 0 -1px ($c * $multiplier * -1); } + + } + + /// Wraps @content in vendorized keyframe blocks. + /// @param {string} $name Name. + @mixin keyframes($name) { + + @-moz-keyframes #{$name} { @content; } + @-webkit-keyframes #{$name} { @content; } + @-ms-keyframes #{$name} { @content; } + @keyframes #{$name} { @content; } + + } + + /// + /// Sets breakpoints. + /// @param {map} $x Breakpoints. + /// + @mixin skel-breakpoints($x: ()) { + $breakpoints: $x !global; + } + + /// + /// Initializes layout module. + /// @param {map} config Config. + /// + @mixin skel-layout($config: ()) { + + // Config. + $configPerBreakpoint: (); + + $z: map-get($config, 'breakpoints'); + + @if $z { + $configPerBreakpoint: $z; + } + + // Reset. + $x: map-get($config, 'reset'); + + @if $x { + + /* Reset */ + + @include reset($x); + + } + + // Box model. + $x: map-get($config, 'boxModel'); + + @if $x { + + /* Box Model */ + + @include boxModel($x); + + } + + // Containers. + $containers: map-get($config, 'containers'); + + @if $containers { + + /* Containers */ + + .container { + margin-left: auto; + margin-right: auto; + } + + // Use default is $containers is just "true". + @if $containers == true { + $containers: 960px; + } + + // Apply base. + @include containers($containers); + + // Apply per-breakpoint. + @each $name in map-keys($breakpoints) { + + // Get/use breakpoint setting if it exists. + $x: map-get($configPerBreakpoint, $name); + + // Per-breakpoint config exists? + @if $x { + $y: map-get($x, 'containers'); + + // Setting exists? Use it. + @if $y { + $containers: $y; + } + + } + + // Create @media block. + @media screen and #{map-get($breakpoints, $name)} { + @include containers($containers); + } + + } + + } + + // Grid. + $grid: map-get($config, 'grid'); + + @if $grid { + + /* Grid */ + + // Use defaults if $grid is just "true". + @if $grid == true { + $grid: (); + } + + // Sub-setting: Gutters. + $grid-gutters: 40px; + $x: map-get($grid, 'gutters'); + + @if $x { + $grid-gutters: $x; + } + + // Rows. + .row { + border-bottom: solid 1px transparent; + -moz-box-sizing: border-box; + -webkit-box-sizing: border-box; + box-sizing: border-box; + } + + .row > * { + float: left; + -moz-box-sizing: border-box; + -webkit-box-sizing: border-box; + box-sizing: border-box; + } + + .row:after, .row:before { + content: ''; + display: block; + clear: both; + height: 0; + } + + .row.uniform > * > :first-child { + margin-top: 0; + } + + .row.uniform > * > :last-child { + margin-bottom: 0; + } + + // Gutters (0%). + @include grid-gutters($grid-gutters, \30 \25, 0); + + // Apply base. + @include grid($grid-gutters); + + // Apply per-breakpoint. + @each $name in map-keys($breakpoints) { + + // Get/use breakpoint setting if it exists. + $x: map-get($configPerBreakpoint, $name); + + // Per-breakpoint config exists? + @if $x { + $y: map-get($x, 'grid'); + + // Setting exists? + @if $y { + + // Sub-setting: Gutters. + $x: map-get($y, 'gutters'); + + @if $x { + $grid-gutters: $x; + } + + } + + } + + // Create @media block. + @media screen and #{map-get($breakpoints, $name)} { + @include grid($grid-gutters, $name); + } + + } + + } + + } + + /// Resets browser styles. + /// @param {string} $mode Mode (default is 'normalize'). + @mixin reset($mode: 'normalize') { + + @if $mode == 'normalize' { + + // normalize.css v3.0.2 | MIT License | git.io/normalize + html{font-family:sans-serif;-ms-text-size-adjust:100%;-webkit-text-size-adjust:100%}body{margin:0}article,aside,details,figcaption,figure,footer,header,hgroup,main,menu,nav,section,summary{display:block}audio,canvas,progress,video{display:inline-block;vertical-align:baseline}audio:not([controls]){display:none;height:0}[hidden],template{display:none}a{background-color:transparent}a:active,a:hover{outline:0}abbr[title]{border-bottom:1px dotted}b,strong{font-weight:700}dfn{font-style:italic}h1{font-size:2em;margin:.67em 0}mark{background:#ff0;color:#000}small{font-size:80%}sub,sup{font-size:75%;line-height:0;position:relative;vertical-align:baseline}sup{top:-.5em}sub{bottom:-.25em}img{border:0}svg:not(:root){overflow:hidden}figure{margin:1em 40px}hr{-moz-box-sizing:content-box;box-sizing:content-box;height:0}pre{overflow:auto}code,kbd,pre,samp{font-family:monospace,monospace;font-size:1em}button,input,optgroup,select,textarea{color:inherit;font:inherit;margin:0}button{overflow:visible}button,select{text-transform:none}button,html input[type=button],input[type=reset],input[type=submit]{-webkit-appearance:button;cursor:pointer}button[disabled],html input[disabled]{cursor:default}button::-moz-focus-inner,input::-moz-focus-inner{border:0;padding:0}input{line-height:normal}input[type=checkbox],input[type=radio]{box-sizing:border-box;padding:0}input[type=number]::-webkit-inner-spin-button,input[type=number]::-webkit-outer-spin-button{height:auto}input[type=search]{-webkit-appearance:textfield;-moz-box-sizing:content-box;-webkit-box-sizing:content-box;box-sizing:content-box}input[type=search]::-webkit-search-cancel-button,input[type=search]::-webkit-search-decoration{-webkit-appearance:none}fieldset{border:1px solid silver;margin:0 2px;padding:.35em .625em .75em}legend{border:0;padding:0}textarea{overflow:auto}optgroup{font-weight:700}table{border-collapse:collapse;border-spacing:0}td,th{padding:0} + + } + @else if $mode == 'full' { + + // meyerweb.com/eric/tools/css/reset v2.0 | 20110126 | License: none (public domain) + html,body,div,span,applet,object,iframe,h1,h2,h3,h4,h5,h6,p,blockquote,pre,a,abbr,acronym,address,big,cite,code,del,dfn,em,img,ins,kbd,q,s,samp,small,strike,strong,sub,sup,tt,var,b,u,i,center,dl,dt,dd,ol,ul,li,fieldset,form,label,legend,table,caption,tbody,tfoot,thead,tr,th,td,article,aside,canvas,details,embed,figure,figcaption,footer,header,hgroup,menu,nav,output,ruby,section,summary,time,mark,audio,video{margin:0;padding:0;border:0;font-size:100%;font:inherit;vertical-align:baseline;}article,aside,details,figcaption,figure,footer,header,hgroup,menu,nav,section{display:block;}body{line-height:1;}ol,ul{list-style:none;}blockquote,q{quotes:none;}blockquote:before,blockquote:after,q:before,q:after{content:'';content:none;}table{border-collapse:collapse;border-spacing:0;}body{-webkit-text-size-adjust:none} + + } + + } + + /// Vendorizes a declaration's property and/or value(s). + /// @param {string} $property Property. + /// @param {mixed} $value String/list of value(s). + @mixin vendor($property, $value) { + + // Determine if property should expand. + $expandProperty: index($vendor-properties, $property); + + // Determine if value should expand (and if so, add '-prefix-' placeholder). + $expandValue: false; + + @each $x in $value { + @each $y in $vendor-values { + @if $y == str-slice($x, 1, str-length($y)) { + + $value: set-nth($value, index($value, $x), '-prefix-' + $x); + $expandValue: true; + + } + } + } + + // Expand property? + @if $expandProperty { + @each $vendor in $vendor-prefixes { + #{$vendor}#{$property}: #{str-replace-all($value, '-prefix-', $vendor)}; + } + } + + // Expand just the value? + @elseif $expandValue { + @each $vendor in $vendor-prefixes { + #{$property}: #{str-replace-all($value, '-prefix-', $vendor)}; + } + } + + // Neither? Treat them as a normal declaration. + @else { + #{$property}: #{$value}; + } + + } \ No newline at end of file diff --git a/assets/sass/libs/_vars.scss b/assets/sass/libs/_vars.scss new file mode 100755 index 00000000..82840504 --- /dev/null +++ b/assets/sass/libs/_vars.scss @@ -0,0 +1,58 @@ +// Misc. + $misc: ( + z-index-base: 10000 + ); + +// Duration. + $duration: ( + transition: 0.2s + ); + +// Size. + $size: ( + border-radius: 0.35em, + element-height: 2.75em, + element-margin: 2em + ); + +// Font. + $font: ( + family: ('Source Sans Pro', Helvetica, sans-serif), + family-fixed: ('Courier New', monospace), + weight: 400, + weight-bold: 400 + ); + +// Palette. + $palette: ( + bg: #fff, + fg: #a2a2a2, + fg-bold: #787878, + fg-light: #b2b2b2, + border: #efefef, + border-bg: #f7f7f7, + border2: #dfdfdf, + border2-bg: #e7e7e7, + + accent1: ( + bg: #49bf9d, + fg: mix(#49bf9d, #ffffff, 25%), + fg-bold: #ffffff, + fg-light: mix(#49bf9d, #ffffff, 40%), + border: rgba(255,255,255,0.25), + border-bg: rgba(255,255,255,0.075), + border2: rgba(255,255,255,0.5), + border2-bg: rgba(255,255,255,0.2) + ), + + accent2: ( + bg: #1f1815, + fg: rgba(255,255,255,0.5), + fg-bold: #ffffff, + fg-light: rgba(255,255,255,0.4), + border: rgba(255,255,255,0.25), + border-bg: rgba(255,255,255,0.075), + border2: rgba(255,255,255,0.5), + border2-bg: rgba(255,255,255,0.2) + ) + ); \ No newline at end of file diff --git a/assets/sass/main.scss b/assets/sass/main.scss new file mode 100755 index 00000000..c4f56019 --- /dev/null +++ b/assets/sass/main.scss @@ -0,0 +1,1498 @@ +@import 'libs/vars'; +@import 'libs/functions'; +@import 'libs/mixins'; +@import url('font-awesome.min.css'); +@import url("https://fonts.googleapis.com/css?family=Source+Sans+Pro:400,400italic"); + +/* + Strata by HTML5 UP + html5up.net | @ajlkn + Free for personal and commercial use under the CCA 3.0 license (html5up.net/license) +*/ + + @import "libs/skel"; + + @include skel-breakpoints(( + xlarge: '(max-width: 1800px)', + large: '(max-width: 1280px)', + medium: '(max-width: 980px)', + small: '(max-width: 736px)', + xsmall: '(max-width: 480px)' + )); + + @include skel-layout(( + reset: 'full', + boxModel: 'border', + grid: ( gutters: 2.5em ), + conditionals: true, + containers: true, + breakpoints: ( + large: ( + grid: ( + gutters: 2em + ), + ), + small: ( + grid: ( + gutters: 1.5em + ), + ) + ) + )); + + $size-header-width: 35%; + $size-header-pad: 4em; + +/* Basic */ + + body { + background: _palette(bg); + + &.is-loading { + *, *:before, *:after { + @include vendor('animation', 'none !important'); + @include vendor('transition', 'none !important'); + } + } + } + + body, input, select, textarea { + color: _palette(fg); + font-family: _font(family); + font-size: 16pt; + font-weight: _font(weight); + line-height: 1.75em; + } + + a { + @include vendor('transition', ('color #{_duration(transition)} ease-in-out', 'border-color #{_duration(transition)} ease-in-out')); + border-bottom: dotted 1px; + color: _palette(accent1, bg); + text-decoration: none; + + &:hover { + border-bottom-color: transparent; + color: _palette(accent1, bg) !important; + text-decoration: none; + } + } + + strong, b { + color: _palette(fg-bold); + font-weight: _font(weight-bold); + } + + em, i { + font-style: italic; + } + + p { + margin: 0 0 _size(element-margin) 0; + } + + h1, h2, h3, h4, h5, h6 { + color: _palette(fg-bold); + font-weight: _font(weight-bold); + line-height: 1em; + margin: 0 0 (_size(element-margin) * 0.5) 0; + + a { + color: inherit; + text-decoration: none; + } + } + + h1 { + font-size: 2em; + line-height: 1.5em; + } + + h2 { + font-size: 1.5em; + line-height: 1.5em; + } + + h3 { + font-size: 1.25em; + line-height: 1.5em; + } + + h4 { + font-size: 1.1em; + line-height: 1.5em; + } + + h5 { + font-size: 0.9em; + line-height: 1.5em; + } + + h6 { + font-size: 0.7em; + line-height: 1.5em; + } + + sub { + font-size: 0.8em; + position: relative; + top: 0.5em; + } + + sup { + font-size: 0.8em; + position: relative; + top: -0.5em; + } + + hr { + border: 0; + border-bottom: solid 2px _palette(border); + + // This is the *only* instance where we need to rely on margin collapse. + margin: _size(element-margin) 0; + + &.major { + margin: (_size(element-margin) * 1.5) 0; + } + } + + blockquote { + border-left: solid 6px _palette(border); + font-style: italic; + margin: 0 0 _size(element-margin) 0; + padding: 0.5em 0 0.5em 1.5em; + } + + code { + background: _palette(border-bg); + border-radius: _size(border-radius); + border: solid 2px _palette(border); + font-family: _font(family-fixed); + font-size: 0.9em; + margin: 0 0.25em; + padding: 0.25em 0.65em; + } + + pre { + -webkit-overflow-scrolling: touch; + font-family: _font(family-fixed); + font-size: 0.9em; + margin: 0 0 _size(element-margin) 0; + + code { + display: block; + line-height: 1.75em; + padding: 1em 1.5em; + overflow-x: auto; + } + } + + .align-left { + text-align: left; + } + + .align-center { + text-align: center; + } + + .align-right { + text-align: right; + } + +/* Section/Article */ + + section, article { + &.special { + text-align: center; + } + } + + header { + p { + color: _palette(fg-light); + position: relative; + margin: 0 0 (_size(element-margin) * 0.75) 0; + } + + h2 + p { + font-size: 1.25em; + margin-top: (_size(element-margin) * -0.5); + line-height: 1.5em; + } + + h3 + p { + font-size: 1.1em; + margin-top: (_size(element-margin) * -0.4); + line-height: 1.5em; + } + + h4 + p, + h5 + p, + h6 + p { + font-size: 0.9em; + margin-top: (_size(element-margin) * -0.3); + line-height: 1.5em; + } + + &.major { + h2 { + font-size: 2em; + } + } + } + +/* Form */ + + form { + margin: 0 0 _size(element-margin) 0; + } + + label { + color: _palette(fg-bold); + display: block; + font-size: 0.9em; + font-weight: _font(weight-bold); + margin: 0 0 (_size(element-margin) * 0.5) 0; + } + + input[type="text"], + input[type="password"], + input[type="email"], + select, + textarea { + @include vendor('appearance', 'none'); + background: _palette(border-bg); + border-radius: _size(border-radius); + border: solid 2px transparent; + color: inherit; + display: block; + outline: 0; + padding: 0 0.75em; + text-decoration: none; + width: 100%; + + &:invalid { + box-shadow: none; + } + + &:focus { + border-color: _palette(accent1, bg); + } + } + + .select-wrapper { + @include icon; + display: block; + position: relative; + + &:before { + color: _palette(border2); + content: '\f078'; + display: block; + height: _size(element-height); + line-height: _size(element-height); + pointer-events: none; + position: absolute; + right: 0; + text-align: center; + top: 0; + width: _size(element-height); + } + + select::-ms-expand { + display: none; + } + } + + input[type="text"], + input[type="password"], + input[type="email"], + select { + height: _size(element-height); + } + + textarea { + padding: 0.75em; + } + + input[type="checkbox"], + input[type="radio"] { + @include vendor('appearance', 'none'); + display: block; + float: left; + margin-right: -2em; + opacity: 0; + width: 1em; + z-index: -1; + + & + label { + @include icon; + color: _palette(fg); + cursor: pointer; + display: inline-block; + font-size: 1em; + font-weight: _font(weight); + padding-left: (_size(element-height) * 0.6) + 0.75em; + padding-right: 0.75em; + position: relative; + + &:before { + background: _palette(border-bg); + border-radius: _size(border-radius); + border: solid 2px transparent; + content: ''; + display: inline-block; + height: (_size(element-height) * 0.6); + left: 0; + line-height: (_size(element-height) * 0.575); + position: absolute; + text-align: center; + top: 0; + width: (_size(element-height) * 0.6); + } + } + + &:checked + label { + &:before { + background: _palette(fg-bold); + border-color: _palette(fg-bold); + color: _palette(bg); + content: '\f00c'; + } + } + + &:focus + label { + &:before { + border-color: _palette(accent1, bg); + } + } + } + + input[type="checkbox"] { + & + label { + &:before { + border-radius: _size(border-radius); + } + } + } + + input[type="radio"] { + & + label { + &:before { + border-radius: 100%; + } + } + } + + ::-webkit-input-placeholder { + color: _palette(fg-light) !important; + opacity: 1.0; + } + + :-moz-placeholder { + color: _palette(fg-light) !important; + opacity: 1.0; + } + + ::-moz-placeholder { + color: _palette(fg-light) !important; + opacity: 1.0; + } + + :-ms-input-placeholder { + color: _palette(fg-light) !important; + opacity: 1.0; + } + + .formerize-placeholder { + color: _palette(fg-light) !important; + opacity: 1.0; + } + +/* Box */ + + .box { + border-radius: _size(border-radius); + border: solid 2px _palette(border); + margin-bottom: _size(element-margin); + padding: 1.5em; + + > :last-child, + > :last-child > :last-child, + > :last-child > :last-child > :last-child { + margin-bottom: 0; + } + + &.alt { + border: 0; + border-radius: 0; + padding: 0; + } + } + +/* Icon */ + + .icon { + @include icon; + border-bottom: none; + position: relative; + + > .label { + display: none; + } + } + +/* Image */ + + .image { + border-radius: _size(border-radius); + border: 0; + display: inline-block; + position: relative; + + &:before { + @include vendor('transition', 'opacity #{_duration(transition)} ease-in-out'); + background: url('images/overlay.png'); + border-radius: _size(border-radius); + content: ''; + display: block; + height: 100%; + left: 0; + opacity: 0.5; + position: absolute; + top: 0; + width: 100%; + } + + &.thumb { + text-align: center; + + &:after { + @include vendor('transition', 'opacity #{_duration(transition)} ease-in-out'); + border-radius: _size(border-radius); + border: solid 3px rgba(255,255,255,0.5); + color: #fff; + content: 'View'; + display: inline-block; + font-size: 0.8em; + font-weight: _font(weight-bold); + left: 50%; + line-height: 2.25em; + margin: -1.25em 0 0 -3em; + opacity: 0; + padding: 0 1.5em; + position: absolute; + text-align: center; + text-decoration: none; + top: 50%; + white-space: nowrap; + } + + &:hover { + &:after { + opacity: 1.0; + } + + &:before { + background: url('images/overlay.png'), url('images/overlay.png'); + opacity: 1.0; + } + } + } + + img { + border-radius: _size(border-radius); + display: block; + } + + &.left { + float: left; + margin: 0 1.5em 1em 0; + top: 0.25em; + } + + &.right { + float: right; + margin: 0 0 1em 1.5em; + top: 0.25em; + } + + &.left, + &.right { + max-width: 40%; + + img { + width: 100%; + } + } + + &.fit { + display: block; + margin: 0 0 _size(element-margin) 0; + width: 100%; + + img { + width: 100%; + } + } + + &.avatar { + border-radius: 100%; + + &:before { + display: none; + } + + img { + border-radius: 100%; + width: 100%; + } + } + } + +/* List */ + + ol { + list-style: decimal; + margin: 0 0 _size(element-margin) 0; + padding-left: 1.25em; + + li { + padding-left: 0.25em; + } + } + + ul { + list-style: disc; + margin: 0 0 _size(element-margin) 0; + padding-left: 1em; + + li { + padding-left: 0.5em; + } + + &.alt { + list-style: none; + padding-left: 0; + + li { + border-top: solid 2px _palette(border); + padding: 0.5em 0; + + &:first-child { + border-top: 0; + padding-top: 0; + } + } + } + + &.icons { + cursor: default; + list-style: none; + padding-left: 0; + + li { + display: inline-block; + padding: 0 1em 0 0; + + &:last-child { + padding-right: 0; + } + + .icon { + &:before { + font-size: 1.5em; + } + } + } + } + + &.actions { + cursor: default; + list-style: none; + padding-left: 0; + + li { + display: inline-block; + padding: 0 (_size(element-margin) * 0.5) 0 0; + vertical-align: middle; + + &:last-child { + padding-right: 0; + } + } + + &.small { + li { + padding: 0 (_size(element-margin) * 0.25) 0 0; + } + } + + &.vertical { + li { + display: block; + padding: (_size(element-margin) * 0.5) 0 0 0; + + &:first-child { + padding-top: 0; + } + + > * { + margin-bottom: 0; + } + } + + &.small { + li { + padding: (_size(element-margin) * 0.25) 0 0 0; + + &:first-child { + padding-top: 0; + } + } + } + } + + &.fit { + display: table; + margin-left: (_size(element-margin) * -0.5); + padding: 0; + table-layout: fixed; + width: calc(100% + #{(_size(element-margin) * 0.5)}); + + li { + display: table-cell; + padding: 0 0 0 (_size(element-margin) * 0.5); + + > * { + margin-bottom: 0; + } + } + + &.small { + margin-left: (_size(element-margin) * -0.25); + width: calc(100% + #{(_size(element-margin) * 0.25)}); + + li { + padding: 0 0 0 (_size(element-margin) * 0.25); + } + } + } + } + + &.labeled-icons { + list-style: none; + padding: 0; + + li { + line-height: 1.75em; + margin: 1.5em 0 0 0; + padding-left: 2.25em; + position: relative; + + &:first-child { + margin-top: 0; + } + + a { + color: inherit; + } + + h3 { + color: _palette(fg-light); + left: 0; + position: absolute; + text-align: center; + top: 0; + width: 1em; + } + } + } + } + + dl { + margin: 0 0 _size(element-margin) 0; + } + +/* Table */ + + .table-wrapper { + -webkit-overflow-scrolling: touch; + overflow-x: auto; + } + + table { + margin: 0 0 _size(element-margin) 0; + width: 100%; + + tbody { + tr { + border: solid 1px _palette(border); + border-left: 0; + border-right: 0; + + &:nth-child(2n + 1) { + background-color: _palette(border-bg); + } + } + } + + td { + padding: 0.75em 0.75em; + } + + th { + color: _palette(fg-bold); + font-size: 0.9em; + font-weight: _font(weight-bold); + padding: 0 0.75em 0.75em 0.75em; + text-align: left; + } + + thead { + border-bottom: solid 2px _palette(border); + } + + tfoot { + border-top: solid 2px _palette(border); + } + + &.alt { + border-collapse: separate; + + tbody { + tr { + td { + border: solid 2px _palette(border); + border-left-width: 0; + border-top-width: 0; + + &:first-child { + border-left-width: 2px; + } + } + + &:first-child { + td { + border-top-width: 2px; + } + } + } + } + + thead { + border-bottom: 0; + } + + tfoot { + border-top: 0; + } + } + } + +/* Button */ + + input[type="submit"], + input[type="reset"], + input[type="button"], + .button { + @include vendor('appearance', 'none'); + @include vendor('transition', ('background-color #{_duration(transition)} ease-in-out', 'color #{_duration(transition)} ease-in-out', 'border-color #{_duration(transition)} ease-in-out')); + background-color: transparent; + border-radius: _size(border-radius); + border: solid 3px _palette(border); + color: _palette(fg-bold) !important; + cursor: pointer; + display: inline-block; + font-weight: _font(weight-bold); + height: 3.15em; + height: calc(2.75em + 6px); + line-height: 2.75em; + min-width: 10em; + padding: 0 1.5em; + text-align: center; + text-decoration: none; + white-space: nowrap; + + &:hover { + border-color: _palette(accent1, bg); + color: _palette(accent1, bg) !important; + } + + &:active { + background-color: transparentize(_palette(accent1, bg), 0.9); + border-color: _palette(accent1, bg); + color: _palette(accent1, bg) !important; + } + + &.icon { + padding-left: 1.35em; + + &:before { + margin-right: 0.5em; + } + } + + &.fit { + display: block; + margin: 0 0 (_size(element-margin) * 0.5) 0; + min-width: 0; + width: 100%; + } + + &.small { + font-size: 0.8em; + } + + &.big { + font-size: 1.35em; + } + + &.special { + background-color: _palette(accent1, bg); + border-color: _palette(accent1, bg); + color: _palette(accent1, fg-bold) !important; + + &:hover { + background-color: lighten(_palette(accent1, bg), 5); + border-color: lighten(_palette(accent1, bg), 5); + } + + &:active { + background-color: darken(_palette(accent1, bg), 5); + border-color: darken(_palette(accent1, bg), 5); + } + } + + &.disabled, + &:disabled { + background-color: _palette(border2-bg) !important; + border-color: _palette(border2-bg) !important; + color: _palette(fg-light) !important; + cursor: default; + } + } + +/* Work Item */ + + .work-item { + margin: 0 0 _size(element-margin) 0; + + .image { + margin: 0 0 (_size(element-margin) * 0.75) 0; + } + + h3 { + font-size: 1em; + margin: 0 0 (_size(element-margin) * 0.25) 0; + } + + p { + font-size: 0.8em; + line-height: 1.5em; + margin: 0; + } + } + +/* Header */ + + #header { + @include vendor('display', 'flex'); + @include vendor('flex-direction', 'column'); + @include vendor('align-items', 'flex-end'); + @include vendor('justify-content', 'space-between'); + background-color: _palette(accent2, bg); + background-attachment: scroll, fixed; + background-image: url('images/overlay.png'), url('../../images/bg.jpg'); + background-position: top left, top left; + background-repeat: repeat, no-repeat; + background-size: auto, auto 100%; + color: _palette(accent2, fg); + height: 100%; + left: 0; + padding: ($size-header-pad * 2) $size-header-pad; + position: fixed; + text-align: right; + top: 0; + width: $size-header-width; + + > * { + @include vendor('flex-shrink', '0'); + width: 100%; + } + + > .inner { + @include vendor('flex-grow', '1'); + margin: 0 0 ($size-header-pad * 0.5) 0; + } + + strong, b { + color: _palette(accent2, fg-bold); + } + + h2, h3, h4, h5, h6 { + color: _palette(accent2, fg-bold); + } + + h1 { + color: _palette(accent2, fg); + font-size: 1.35em; + line-height: 1.75em; + margin: 0; + } + + .image.avatar { + margin: 0 0 (_size(element-margin) * 0.5) 0; + width: 6.25em; + } + } + +/* Footer */ + + #footer { + .icons { + margin: (_size(element-margin) * 0.5) 0 0 0; + + a { + color: _palette(accent2, fg-light); + } + } + + .copyright { + color: _palette(accent2, fg-light); + font-size: 0.8em; + list-style: none; + margin: (_size(element-margin) * 0.5) 0 0 0; + padding: 0; + + li { + border-left: solid 1px _palette(accent2, border); + display: inline-block; + line-height: 1em; + margin-left: 0.75em; + padding-left: 0.75em; + + &:first-child { + border-left: 0; + margin-left: 0; + padding-left: 0; + } + + a { + color: inherit; + } + } + } + } + +/* Main */ + + #main { + margin-left: $size-header-width; + max-width: 50em + $size-header-pad; + padding: ($size-header-pad * 2) $size-header-pad $size-header-pad $size-header-pad; + width: calc(100% - #{$size-header-width}); + + > section { + border-top: solid 2px _palette(border); + margin: $size-header-pad 0 0 0; + padding: $size-header-pad 0 0 0; + + &:first-child { + border-top: 0; + margin-top: 0; + padding-top: 0; + } + } + } + +/* Poptrox */ + + @include keyframes('spin') { + 0% { @include vendor('transform', 'rotate(0deg)'); } + 100% { @include vendor('transform', 'rotate(360deg)'); } + }; + + .poptrox-popup { + @include vendor('box-sizing', 'content-box'); + -webkit-tap-highlight-color: rgba(255,255,255,0); + background: #fff; + border-radius: _size(border-radius); + box-shadow: 0 0.1em 0.15em 0 rgba(0,0,0,0.15); + overflow: hidden; + padding-bottom: 3em; + + .loader { + @include icon; + @include vendor('animation', 'spin 1s linear infinite'); + font-size: 1.5em; + height: 1em; + left: 50%; + line-height: 1em; + margin: -0.5em 0 0 -0.5em; + position: absolute; + top: 50%; + width: 1em; + + &:before { + content: '\f1ce'; + } + } + + .caption { + background: #fff; + bottom: 0; + cursor: default; + font-size: 0.9em; + height: 3em; + left: 0; + line-height: 2.8em; + position: absolute; + text-align: center; + width: 100%; + z-index: 1; + } + + .nav-next, + .nav-previous { + @include icon; + @include vendor('transition', 'opacity #{_duration(transition)} ease-in-out'); + -webkit-tap-highlight-color: rgba(255,255,255,0); + background: rgba(0,0,0,0.01); + cursor: pointer; + height: 100%; + opacity: 0; + position: absolute; + top: 0; + width: 50%; + + &:before { + color: #fff; + font-size: 2.5em; + height: 1em; + line-height: 1em; + margin-top: -0.75em; + position: absolute; + text-align: center; + top: 50%; + width: 1.5em; + } + } + + .nav-next { + right: 0; + + &:before { + content: '\f105'; + right: 0; + } + } + + .nav-previous { + left: 0; + + &:before { + content: '\f104'; + left: 0; + } + } + + .closer { + @include icon; + @include vendor('transition', 'opacity #{_duration(transition)} ease-in-out'); + -webkit-tap-highlight-color: rgba(255,255,255,0); + color: #fff; + height: 4em; + line-height: 4em; + opacity: 0; + position: absolute; + right: 0; + text-align: center; + top: 0; + width: 4em; + z-index: 2; + + &:before { + @include vendor('box-sizing', 'content-box'); + border-radius: 100%; + border: solid 3px rgba(255,255,255,0.5); + content: '\f00d'; + display: block; + font-size: 1em; + height: 1.75em; + left: 50%; + line-height: 1.75em; + margin: -0.875em 0 0 -0.875em; + position: absolute; + top: 50%; + width: 1.75em; + } + } + + &:hover { + .nav-next, + .nav-previous { + opacity: 0.5; + + &:hover { + opacity: 1.0; + } + } + + .closer { + opacity: 0.5; + + &:hover { + opacity: 1.0; + } + } + } + } + +/* Touch */ + + body.is-touch { + + .image { + &.thumb { + &:before { + opacity: 0.5 !important; + } + + &:after { + display: none !important; + } + } + } + + #header { + background-attachment: scroll; + background-size: auto, cover; + } + + .poptrox-popup { + .nav-next, + .nav-previous, + .closer { + opacity: 1.0 !important; + } + } + + } + +/* XLarge */ + + @include breakpoint(xlarge) { + + /* Basic */ + + body, input, select, textarea { + font-size: 12pt; + } + + } + +/* Large */ + + @include breakpoint(large) { + + $size-header-width: 30%; + $size-header-pad: 3em; + + /* Header */ + + #header { + padding: ($size-header-pad * 2) $size-header-pad $size-header-pad $size-header-pad; + width: $size-header-width; + + h1 { + font-size: 1.25em; + + br { + display: none; + } + } + } + + /* Footer */ + + #footer { + .copyright { + li { + border-left-width: 0; + display: block; + line-height: 2.25em; + margin-left: 0; + padding-left: 0; + } + } + } + + /* Main */ + + #main { + margin-left: $size-header-width; + max-width: none; + padding: ($size-header-pad * 2) $size-header-pad $size-header-pad $size-header-pad; + width: calc(100% - #{$size-header-width}); + } + + } + +/* Medium */ + + @include breakpoint(medium) { + + $size-header-pad: 4em; + + /* Basic */ + + h1, h2, h3, h4, h5, h6 { + br { + display: none; + } + } + + /* List */ + + ul { + &.icons { + li { + .icon { + font-size: 1.25em; + } + } + } + } + + /* Header */ + + #header { + background-attachment: scroll; + background-position: top left, center center; + background-size: auto, cover; + left: auto; + padding: ($size-header-pad * 1.5) $size-header-pad; + position: relative; + text-align: center; + top: auto; + width: 100%; + display: block; + + h1 { + font-size: 1.75em; + + br { + display: inline; + } + } + } + + /* Footer */ + + #footer { + background-attachment: scroll; + background-color: _palette(accent2, bg); + background-image: url('images/overlay.png'), url('../../images/bg.jpg'); + background-position: top left, bottom center; + background-repeat: repeat, no-repeat; + background-size: auto, cover; + bottom: auto; + left: auto; + padding: $size-header-pad $size-header-pad ($size-header-pad * 1.5) $size-header-pad; + position: relative; + text-align: center; + width: 100%; + + .icons { + margin: 0 0 (_size(element-margin) * 0.5) 0; + } + + .copyright { + margin: 0 0 (_size(element-margin) * 0.5) 0; + + li { + border-left-width: 1px; + display: inline-block; + line-height: 1em; + margin-left: 0.75em; + padding-left: 0.75em; + } + } + } + + /* Main */ + + #main { + margin: 0; + padding: ($size-header-pad * 1.5) $size-header-pad; + width: 100%; + } + + } + +/* Small */ + + @include breakpoint(small) { + + $size-header-pad: 1.5em; + + /* Basic */ + + h1 { + font-size: 1.5em; + } + + h2 { + font-size: 1.2em; + } + + h3 { + font-size: 1em; + } + + /* Section/Article */ + + section, article { + &.special { + text-align: center; + } + } + + header { + &.major { + h2 { + font-size: 1.35em; + } + } + } + + /* List */ + + ul { + &.labeled-icons { + li { + padding-left: 2em; + + h3 { + line-height: 1.75em; + } + } + } + } + + /* Header */ + + #header { + padding: ($size-header-pad * 1.5) $size-header-pad; + + h1 { + font-size: 1.35em; + } + } + + /* Footer */ + + #footer { + padding: ($size-header-pad * 1.5) $size-header-pad; + } + + /* Main */ + + #main { + padding: ($size-header-pad * 1.5) $size-header-pad (($size-header-pad * 1.5) - _size(element-margin)) $size-header-pad; + + > section { + margin: ($size-header-pad * 1.5) 0 0 0; + padding: ($size-header-pad * 1.5) 0 0 0; + } + } + + /* Poptrox */ + + .poptrox-popup { + border-radius: 0; + + .nav-next, + .nav-previous { + &:before { + margin-top: -1em; + } + } + } + + } + +/* XSmall */ + + @include breakpoint(xsmall) { + + $size-header-pad: 1.5em; + + /* List */ + + ul { + &.actions { + margin: 0 0 _size(element-margin) 0; + + li { + display: block; + padding: (_size(element-margin) * 0.5) 0 0 0; + text-align: center; + width: 100%; + + &:first-child { + padding-top: 0; + } + + > * { + margin: 0 !important; + width: 100%; + + &.icon { + &:before { + margin-left: -2em; + } + } + } + } + + &.small { + li { + padding: (_size(element-margin) * 0.25) 0 0 0; + + &:first-child { + padding-top: 0; + } + } + } + } + } + + /* Header */ + + #header { + padding: ($size-header-pad * 3) $size-header-pad; + + h1 { + br { + display: none; + } + } + } + + /* Footer */ + + #footer { + .copyright { + li { + border-left-width: 0; + display: block; + line-height: 2.25em; + margin-left: 0; + padding-left: 0; + } + } + } + + } \ No newline at end of file diff --git a/extern/pybind b/extern/pybind deleted file mode 160000 index aec6cc54..00000000 --- a/extern/pybind +++ /dev/null @@ -1 +0,0 @@ -Subproject commit aec6cc5406edb076f5a489c2d7f84bb07052c4a3 diff --git a/qrlocal/index.html b/qrlocal/index.html new file mode 100644 index 00000000..e4443924 --- /dev/null +++ b/qrlocal/index.html @@ -0,0 +1,217 @@ + + + + + + + + + + + + + + + +
+

Precision Date and Time (Local)

+ + + + + + + +

Refresh Rate FPS

+ +

Timezone hour minute

+ +

Daylight Saving Time

+ +

QR Command Set Time

+ +

Simply point your Labs enabled camera at this animated QR Code, to set your date and time very accurately to local time. This is particularly useful for multi-camera shoots, as it helps synchronize the timecode between cameras. As the camera’s internal clock will drift slowly over time, use this QR Code just before your multi-camera shoot for the best synchronization.

+ +
+
+
+
+

QR Command:

+ +

Compatibility: Labs enabled HERO5 Session, HERO7, HERO8, HERO9, HERO10, HERO11, MAX and BONES

+ + + +
+ + diff --git a/qrlocal/jquery.min.js b/qrlocal/jquery.min.js new file mode 100644 index 00000000..2740cc4c --- /dev/null +++ b/qrlocal/jquery.min.js @@ -0,0 +1,2 @@ +/*! jQuery v1.8.3 jquery.com | jquery.org/license */ +(function(e,t){function _(e){var t=M[e]={};return v.each(e.split(y),function(e,n){t[n]=!0}),t}function H(e,n,r){if(r===t&&e.nodeType===1){var i="data-"+n.replace(P,"-$1").toLowerCase();r=e.getAttribute(i);if(typeof r=="string"){try{r=r==="true"?!0:r==="false"?!1:r==="null"?null:+r+""===r?+r:D.test(r)?v.parseJSON(r):r}catch(s){}v.data(e,n,r)}else r=t}return r}function B(e){var t;for(t in e){if(t==="data"&&v.isEmptyObject(e[t]))continue;if(t!=="toJSON")return!1}return!0}function et(){return!1}function tt(){return!0}function ut(e){return!e||!e.parentNode||e.parentNode.nodeType===11}function at(e,t){do e=e[t];while(e&&e.nodeType!==1);return e}function ft(e,t,n){t=t||0;if(v.isFunction(t))return v.grep(e,function(e,r){var i=!!t.call(e,r,e);return i===n});if(t.nodeType)return v.grep(e,function(e,r){return e===t===n});if(typeof t=="string"){var r=v.grep(e,function(e){return e.nodeType===1});if(it.test(t))return v.filter(t,r,!n);t=v.filter(t,r)}return v.grep(e,function(e,r){return v.inArray(e,t)>=0===n})}function lt(e){var t=ct.split("|"),n=e.createDocumentFragment();if(n.createElement)while(t.length)n.createElement(t.pop());return n}function Lt(e,t){return e.getElementsByTagName(t)[0]||e.appendChild(e.ownerDocument.createElement(t))}function At(e,t){if(t.nodeType!==1||!v.hasData(e))return;var n,r,i,s=v._data(e),o=v._data(t,s),u=s.events;if(u){delete o.handle,o.events={};for(n in u)for(r=0,i=u[n].length;r").appendTo(i.body),n=t.css("display");t.remove();if(n==="none"||n===""){Pt=i.body.appendChild(Pt||v.extend(i.createElement("iframe"),{frameBorder:0,width:0,height:0}));if(!Ht||!Pt.createElement)Ht=(Pt.contentWindow||Pt.contentDocument).document,Ht.write(""),Ht.close();t=Ht.body.appendChild(Ht.createElement(e)),n=Dt(t,"display"),i.body.removeChild(Pt)}return Wt[e]=n,n}function fn(e,t,n,r){var i;if(v.isArray(t))v.each(t,function(t,i){n||sn.test(e)?r(e,i):fn(e+"["+(typeof i=="object"?t:"")+"]",i,n,r)});else if(!n&&v.type(t)==="object")for(i in t)fn(e+"["+i+"]",t[i],n,r);else r(e,t)}function Cn(e){return function(t,n){typeof t!="string"&&(n=t,t="*");var r,i,s,o=t.toLowerCase().split(y),u=0,a=o.length;if(v.isFunction(n))for(;u)[^>]*$|#([\w\-]*)$)/,E=/^<(\w+)\s*\/?>(?:<\/\1>|)$/,S=/^[\],:{}\s]*$/,x=/(?:^|:|,)(?:\s*\[)+/g,T=/\\(?:["\\\/bfnrt]|u[\da-fA-F]{4})/g,N=/"[^"\\\r\n]*"|true|false|null|-?(?:\d\d*\.|)\d+(?:[eE][\-+]?\d+|)/g,C=/^-ms-/,k=/-([\da-z])/gi,L=function(e,t){return(t+"").toUpperCase()},A=function(){i.addEventListener?(i.removeEventListener("DOMContentLoaded",A,!1),v.ready()):i.readyState==="complete"&&(i.detachEvent("onreadystatechange",A),v.ready())},O={};v.fn=v.prototype={constructor:v,init:function(e,n,r){var s,o,u,a;if(!e)return this;if(e.nodeType)return this.context=this[0]=e,this.length=1,this;if(typeof e=="string"){e.charAt(0)==="<"&&e.charAt(e.length-1)===">"&&e.length>=3?s=[null,e,null]:s=w.exec(e);if(s&&(s[1]||!n)){if(s[1])return n=n instanceof v?n[0]:n,a=n&&n.nodeType?n.ownerDocument||n:i,e=v.parseHTML(s[1],a,!0),E.test(s[1])&&v.isPlainObject(n)&&this.attr.call(e,n,!0),v.merge(this,e);o=i.getElementById(s[2]);if(o&&o.parentNode){if(o.id!==s[2])return r.find(e);this.length=1,this[0]=o}return this.context=i,this.selector=e,this}return!n||n.jquery?(n||r).find(e):this.constructor(n).find(e)}return v.isFunction(e)?r.ready(e):(e.selector!==t&&(this.selector=e.selector,this.context=e.context),v.makeArray(e,this))},selector:"",jquery:"1.8.3",length:0,size:function(){return this.length},toArray:function(){return l.call(this)},get:function(e){return e==null?this.toArray():e<0?this[this.length+e]:this[e]},pushStack:function(e,t,n){var r=v.merge(this.constructor(),e);return r.prevObject=this,r.context=this.context,t==="find"?r.selector=this.selector+(this.selector?" ":"")+n:t&&(r.selector=this.selector+"."+t+"("+n+")"),r},each:function(e,t){return v.each(this,e,t)},ready:function(e){return v.ready.promise().done(e),this},eq:function(e){return e=+e,e===-1?this.slice(e):this.slice(e,e+1)},first:function(){return this.eq(0)},last:function(){return this.eq(-1)},slice:function(){return this.pushStack(l.apply(this,arguments),"slice",l.call(arguments).join(","))},map:function(e){return this.pushStack(v.map(this,function(t,n){return e.call(t,n,t)}))},end:function(){return this.prevObject||this.constructor(null)},push:f,sort:[].sort,splice:[].splice},v.fn.init.prototype=v.fn,v.extend=v.fn.extend=function(){var e,n,r,i,s,o,u=arguments[0]||{},a=1,f=arguments.length,l=!1;typeof u=="boolean"&&(l=u,u=arguments[1]||{},a=2),typeof u!="object"&&!v.isFunction(u)&&(u={}),f===a&&(u=this,--a);for(;a0)return;r.resolveWith(i,[v]),v.fn.trigger&&v(i).trigger("ready").off("ready")},isFunction:function(e){return v.type(e)==="function"},isArray:Array.isArray||function(e){return v.type(e)==="array"},isWindow:function(e){return e!=null&&e==e.window},isNumeric:function(e){return!isNaN(parseFloat(e))&&isFinite(e)},type:function(e){return e==null?String(e):O[h.call(e)]||"object"},isPlainObject:function(e){if(!e||v.type(e)!=="object"||e.nodeType||v.isWindow(e))return!1;try{if(e.constructor&&!p.call(e,"constructor")&&!p.call(e.constructor.prototype,"isPrototypeOf"))return!1}catch(n){return!1}var r;for(r in e);return r===t||p.call(e,r)},isEmptyObject:function(e){var t;for(t in e)return!1;return!0},error:function(e){throw new Error(e)},parseHTML:function(e,t,n){var r;return!e||typeof e!="string"?null:(typeof t=="boolean"&&(n=t,t=0),t=t||i,(r=E.exec(e))?[t.createElement(r[1])]:(r=v.buildFragment([e],t,n?null:[]),v.merge([],(r.cacheable?v.clone(r.fragment):r.fragment).childNodes)))},parseJSON:function(t){if(!t||typeof t!="string")return null;t=v.trim(t);if(e.JSON&&e.JSON.parse)return e.JSON.parse(t);if(S.test(t.replace(T,"@").replace(N,"]").replace(x,"")))return(new Function("return "+t))();v.error("Invalid JSON: "+t)},parseXML:function(n){var r,i;if(!n||typeof n!="string")return null;try{e.DOMParser?(i=new DOMParser,r=i.parseFromString(n,"text/xml")):(r=new ActiveXObject("Microsoft.XMLDOM"),r.async="false",r.loadXML(n))}catch(s){r=t}return(!r||!r.documentElement||r.getElementsByTagName("parsererror").length)&&v.error("Invalid XML: "+n),r},noop:function(){},globalEval:function(t){t&&g.test(t)&&(e.execScript||function(t){e.eval.call(e,t)})(t)},camelCase:function(e){return e.replace(C,"ms-").replace(k,L)},nodeName:function(e,t){return e.nodeName&&e.nodeName.toLowerCase()===t.toLowerCase()},each:function(e,n,r){var i,s=0,o=e.length,u=o===t||v.isFunction(e);if(r){if(u){for(i in e)if(n.apply(e[i],r)===!1)break}else for(;s0&&e[0]&&e[a-1]||a===0||v.isArray(e));if(f)for(;u-1)a.splice(n,1),i&&(n<=o&&o--,n<=u&&u--)}),this},has:function(e){return v.inArray(e,a)>-1},empty:function(){return a=[],this},disable:function(){return a=f=n=t,this},disabled:function(){return!a},lock:function(){return f=t,n||c.disable(),this},locked:function(){return!f},fireWith:function(e,t){return t=t||[],t=[e,t.slice?t.slice():t],a&&(!r||f)&&(i?f.push(t):l(t)),this},fire:function(){return c.fireWith(this,arguments),this},fired:function(){return!!r}};return c},v.extend({Deferred:function(e){var t=[["resolve","done",v.Callbacks("once memory"),"resolved"],["reject","fail",v.Callbacks("once memory"),"rejected"],["notify","progress",v.Callbacks("memory")]],n="pending",r={state:function(){return n},always:function(){return i.done(arguments).fail(arguments),this},then:function(){var e=arguments;return v.Deferred(function(n){v.each(t,function(t,r){var s=r[0],o=e[t];i[r[1]](v.isFunction(o)?function(){var e=o.apply(this,arguments);e&&v.isFunction(e.promise)?e.promise().done(n.resolve).fail(n.reject).progress(n.notify):n[s+"With"](this===i?n:this,[e])}:n[s])}),e=null}).promise()},promise:function(e){return e!=null?v.extend(e,r):r}},i={};return r.pipe=r.then,v.each(t,function(e,s){var o=s[2],u=s[3];r[s[1]]=o.add,u&&o.add(function(){n=u},t[e^1][2].disable,t[2][2].lock),i[s[0]]=o.fire,i[s[0]+"With"]=o.fireWith}),r.promise(i),e&&e.call(i,i),i},when:function(e){var t=0,n=l.call(arguments),r=n.length,i=r!==1||e&&v.isFunction(e.promise)?r:0,s=i===1?e:v.Deferred(),o=function(e,t,n){return function(r){t[e]=this,n[e]=arguments.length>1?l.call(arguments):r,n===u?s.notifyWith(t,n):--i||s.resolveWith(t,n)}},u,a,f;if(r>1){u=new Array(r),a=new Array(r),f=new Array(r);for(;t
a",n=p.getElementsByTagName("*"),r=p.getElementsByTagName("a")[0];if(!n||!r||!n.length)return{};s=i.createElement("select"),o=s.appendChild(i.createElement("option")),u=p.getElementsByTagName("input")[0],r.style.cssText="top:1px;float:left;opacity:.5",t={leadingWhitespace:p.firstChild.nodeType===3,tbody:!p.getElementsByTagName("tbody").length,htmlSerialize:!!p.getElementsByTagName("link").length,style:/top/.test(r.getAttribute("style")),hrefNormalized:r.getAttribute("href")==="/a",opacity:/^0.5/.test(r.style.opacity),cssFloat:!!r.style.cssFloat,checkOn:u.value==="on",optSelected:o.selected,getSetAttribute:p.className!=="t",enctype:!!i.createElement("form").enctype,html5Clone:i.createElement("nav").cloneNode(!0).outerHTML!=="<:nav>",boxModel:i.compatMode==="CSS1Compat",submitBubbles:!0,changeBubbles:!0,focusinBubbles:!1,deleteExpando:!0,noCloneEvent:!0,inlineBlockNeedsLayout:!1,shrinkWrapBlocks:!1,reliableMarginRight:!0,boxSizingReliable:!0,pixelPosition:!1},u.checked=!0,t.noCloneChecked=u.cloneNode(!0).checked,s.disabled=!0,t.optDisabled=!o.disabled;try{delete p.test}catch(d){t.deleteExpando=!1}!p.addEventListener&&p.attachEvent&&p.fireEvent&&(p.attachEvent("onclick",h=function(){t.noCloneEvent=!1}),p.cloneNode(!0).fireEvent("onclick"),p.detachEvent("onclick",h)),u=i.createElement("input"),u.value="t",u.setAttribute("type","radio"),t.radioValue=u.value==="t",u.setAttribute("checked","checked"),u.setAttribute("name","t"),p.appendChild(u),a=i.createDocumentFragment(),a.appendChild(p.lastChild),t.checkClone=a.cloneNode(!0).cloneNode(!0).lastChild.checked,t.appendChecked=u.checked,a.removeChild(u),a.appendChild(p);if(p.attachEvent)for(l in{submit:!0,change:!0,focusin:!0})f="on"+l,c=f in p,c||(p.setAttribute(f,"return;"),c=typeof p[f]=="function"),t[l+"Bubbles"]=c;return v(function(){var n,r,s,o,u="padding:0;margin:0;border:0;display:block;overflow:hidden;",a=i.getElementsByTagName("body")[0];if(!a)return;n=i.createElement("div"),n.style.cssText="visibility:hidden;border:0;width:0;height:0;position:static;top:0;margin-top:1px",a.insertBefore(n,a.firstChild),r=i.createElement("div"),n.appendChild(r),r.innerHTML="
t
",s=r.getElementsByTagName("td"),s[0].style.cssText="padding:0;margin:0;border:0;display:none",c=s[0].offsetHeight===0,s[0].style.display="",s[1].style.display="none",t.reliableHiddenOffsets=c&&s[0].offsetHeight===0,r.innerHTML="",r.style.cssText="box-sizing:border-box;-moz-box-sizing:border-box;-webkit-box-sizing:border-box;padding:1px;border:1px;display:block;width:4px;margin-top:1%;position:absolute;top:1%;",t.boxSizing=r.offsetWidth===4,t.doesNotIncludeMarginInBodyOffset=a.offsetTop!==1,e.getComputedStyle&&(t.pixelPosition=(e.getComputedStyle(r,null)||{}).top!=="1%",t.boxSizingReliable=(e.getComputedStyle(r,null)||{width:"4px"}).width==="4px",o=i.createElement("div"),o.style.cssText=r.style.cssText=u,o.style.marginRight=o.style.width="0",r.style.width="1px",r.appendChild(o),t.reliableMarginRight=!parseFloat((e.getComputedStyle(o,null)||{}).marginRight)),typeof r.style.zoom!="undefined"&&(r.innerHTML="",r.style.cssText=u+"width:1px;padding:1px;display:inline;zoom:1",t.inlineBlockNeedsLayout=r.offsetWidth===3,r.style.display="block",r.style.overflow="visible",r.innerHTML="
",r.firstChild.style.width="5px",t.shrinkWrapBlocks=r.offsetWidth!==3,n.style.zoom=1),a.removeChild(n),n=r=s=o=null}),a.removeChild(p),n=r=s=o=u=a=p=null,t}();var D=/(?:\{[\s\S]*\}|\[[\s\S]*\])$/,P=/([A-Z])/g;v.extend({cache:{},deletedIds:[],uuid:0,expando:"jQuery"+(v.fn.jquery+Math.random()).replace(/\D/g,""),noData:{embed:!0,object:"clsid:D27CDB6E-AE6D-11cf-96B8-444553540000",applet:!0},hasData:function(e){return e=e.nodeType?v.cache[e[v.expando]]:e[v.expando],!!e&&!B(e)},data:function(e,n,r,i){if(!v.acceptData(e))return;var s,o,u=v.expando,a=typeof n=="string",f=e.nodeType,l=f?v.cache:e,c=f?e[u]:e[u]&&u;if((!c||!l[c]||!i&&!l[c].data)&&a&&r===t)return;c||(f?e[u]=c=v.deletedIds.pop()||v.guid++:c=u),l[c]||(l[c]={},f||(l[c].toJSON=v.noop));if(typeof n=="object"||typeof n=="function")i?l[c]=v.extend(l[c],n):l[c].data=v.extend(l[c].data,n);return s=l[c],i||(s.data||(s.data={}),s=s.data),r!==t&&(s[v.camelCase(n)]=r),a?(o=s[n],o==null&&(o=s[v.camelCase(n)])):o=s,o},removeData:function(e,t,n){if(!v.acceptData(e))return;var r,i,s,o=e.nodeType,u=o?v.cache:e,a=o?e[v.expando]:v.expando;if(!u[a])return;if(t){r=n?u[a]:u[a].data;if(r){v.isArray(t)||(t in r?t=[t]:(t=v.camelCase(t),t in r?t=[t]:t=t.split(" ")));for(i=0,s=t.length;i1,null,!1))},removeData:function(e){return this.each(function(){v.removeData(this,e)})}}),v.extend({queue:function(e,t,n){var r;if(e)return t=(t||"fx")+"queue",r=v._data(e,t),n&&(!r||v.isArray(n)?r=v._data(e,t,v.makeArray(n)):r.push(n)),r||[]},dequeue:function(e,t){t=t||"fx";var n=v.queue(e,t),r=n.length,i=n.shift(),s=v._queueHooks(e,t),o=function(){v.dequeue(e,t)};i==="inprogress"&&(i=n.shift(),r--),i&&(t==="fx"&&n.unshift("inprogress"),delete s.stop,i.call(e,o,s)),!r&&s&&s.empty.fire()},_queueHooks:function(e,t){var n=t+"queueHooks";return v._data(e,n)||v._data(e,n,{empty:v.Callbacks("once memory").add(function(){v.removeData(e,t+"queue",!0),v.removeData(e,n,!0)})})}}),v.fn.extend({queue:function(e,n){var r=2;return typeof e!="string"&&(n=e,e="fx",r--),arguments.length1)},removeAttr:function(e){return this.each(function(){v.removeAttr(this,e)})},prop:function(e,t){return v.access(this,v.prop,e,t,arguments.length>1)},removeProp:function(e){return e=v.propFix[e]||e,this.each(function(){try{this[e]=t,delete this[e]}catch(n){}})},addClass:function(e){var t,n,r,i,s,o,u;if(v.isFunction(e))return this.each(function(t){v(this).addClass(e.call(this,t,this.className))});if(e&&typeof e=="string"){t=e.split(y);for(n=0,r=this.length;n=0)r=r.replace(" "+n[s]+" "," ");i.className=e?v.trim(r):""}}}return this},toggleClass:function(e,t){var n=typeof e,r=typeof t=="boolean";return v.isFunction(e)?this.each(function(n){v(this).toggleClass(e.call(this,n,this.className,t),t)}):this.each(function(){if(n==="string"){var i,s=0,o=v(this),u=t,a=e.split(y);while(i=a[s++])u=r?u:!o.hasClass(i),o[u?"addClass":"removeClass"](i)}else if(n==="undefined"||n==="boolean")this.className&&v._data(this,"__className__",this.className),this.className=this.className||e===!1?"":v._data(this,"__className__")||""})},hasClass:function(e){var t=" "+e+" ",n=0,r=this.length;for(;n=0)return!0;return!1},val:function(e){var n,r,i,s=this[0];if(!arguments.length){if(s)return n=v.valHooks[s.type]||v.valHooks[s.nodeName.toLowerCase()],n&&"get"in n&&(r=n.get(s,"value"))!==t?r:(r=s.value,typeof r=="string"?r.replace(R,""):r==null?"":r);return}return i=v.isFunction(e),this.each(function(r){var s,o=v(this);if(this.nodeType!==1)return;i?s=e.call(this,r,o.val()):s=e,s==null?s="":typeof s=="number"?s+="":v.isArray(s)&&(s=v.map(s,function(e){return e==null?"":e+""})),n=v.valHooks[this.type]||v.valHooks[this.nodeName.toLowerCase()];if(!n||!("set"in n)||n.set(this,s,"value")===t)this.value=s})}}),v.extend({valHooks:{option:{get:function(e){var t=e.attributes.value;return!t||t.specified?e.value:e.text}},select:{get:function(e){var t,n,r=e.options,i=e.selectedIndex,s=e.type==="select-one"||i<0,o=s?null:[],u=s?i+1:r.length,a=i<0?u:s?i:0;for(;a=0}),n.length||(e.selectedIndex=-1),n}}},attrFn:{},attr:function(e,n,r,i){var s,o,u,a=e.nodeType;if(!e||a===3||a===8||a===2)return;if(i&&v.isFunction(v.fn[n]))return v(e)[n](r);if(typeof e.getAttribute=="undefined")return v.prop(e,n,r);u=a!==1||!v.isXMLDoc(e),u&&(n=n.toLowerCase(),o=v.attrHooks[n]||(X.test(n)?F:j));if(r!==t){if(r===null){v.removeAttr(e,n);return}return o&&"set"in o&&u&&(s=o.set(e,r,n))!==t?s:(e.setAttribute(n,r+""),r)}return o&&"get"in o&&u&&(s=o.get(e,n))!==null?s:(s=e.getAttribute(n),s===null?t:s)},removeAttr:function(e,t){var n,r,i,s,o=0;if(t&&e.nodeType===1){r=t.split(y);for(;o=0}})});var $=/^(?:textarea|input|select)$/i,J=/^([^\.]*|)(?:\.(.+)|)$/,K=/(?:^|\s)hover(\.\S+|)\b/,Q=/^key/,G=/^(?:mouse|contextmenu)|click/,Y=/^(?:focusinfocus|focusoutblur)$/,Z=function(e){return v.event.special.hover?e:e.replace(K,"mouseenter$1 mouseleave$1")};v.event={add:function(e,n,r,i,s){var o,u,a,f,l,c,h,p,d,m,g;if(e.nodeType===3||e.nodeType===8||!n||!r||!(o=v._data(e)))return;r.handler&&(d=r,r=d.handler,s=d.selector),r.guid||(r.guid=v.guid++),a=o.events,a||(o.events=a={}),u=o.handle,u||(o.handle=u=function(e){return typeof v=="undefined"||!!e&&v.event.triggered===e.type?t:v.event.dispatch.apply(u.elem,arguments)},u.elem=e),n=v.trim(Z(n)).split(" ");for(f=0;f=0&&(y=y.slice(0,-1),a=!0),y.indexOf(".")>=0&&(b=y.split("."),y=b.shift(),b.sort());if((!s||v.event.customEvent[y])&&!v.event.global[y])return;n=typeof n=="object"?n[v.expando]?n:new v.Event(y,n):new v.Event(y),n.type=y,n.isTrigger=!0,n.exclusive=a,n.namespace=b.join("."),n.namespace_re=n.namespace?new RegExp("(^|\\.)"+b.join("\\.(?:.*\\.|)")+"(\\.|$)"):null,h=y.indexOf(":")<0?"on"+y:"";if(!s){u=v.cache;for(f in u)u[f].events&&u[f].events[y]&&v.event.trigger(n,r,u[f].handle.elem,!0);return}n.result=t,n.target||(n.target=s),r=r!=null?v.makeArray(r):[],r.unshift(n),p=v.event.special[y]||{};if(p.trigger&&p.trigger.apply(s,r)===!1)return;m=[[s,p.bindType||y]];if(!o&&!p.noBubble&&!v.isWindow(s)){g=p.delegateType||y,l=Y.test(g+y)?s:s.parentNode;for(c=s;l;l=l.parentNode)m.push([l,g]),c=l;c===(s.ownerDocument||i)&&m.push([c.defaultView||c.parentWindow||e,g])}for(f=0;f=0:v.find(h,this,null,[s]).length),u[h]&&f.push(c);f.length&&w.push({elem:s,matches:f})}d.length>m&&w.push({elem:this,matches:d.slice(m)});for(r=0;r0?this.on(t,null,e,n):this.trigger(t)},Q.test(t)&&(v.event.fixHooks[t]=v.event.keyHooks),G.test(t)&&(v.event.fixHooks[t]=v.event.mouseHooks)}),function(e,t){function nt(e,t,n,r){n=n||[],t=t||g;var i,s,a,f,l=t.nodeType;if(!e||typeof e!="string")return n;if(l!==1&&l!==9)return[];a=o(t);if(!a&&!r)if(i=R.exec(e))if(f=i[1]){if(l===9){s=t.getElementById(f);if(!s||!s.parentNode)return n;if(s.id===f)return n.push(s),n}else if(t.ownerDocument&&(s=t.ownerDocument.getElementById(f))&&u(t,s)&&s.id===f)return n.push(s),n}else{if(i[2])return S.apply(n,x.call(t.getElementsByTagName(e),0)),n;if((f=i[3])&&Z&&t.getElementsByClassName)return S.apply(n,x.call(t.getElementsByClassName(f),0)),n}return vt(e.replace(j,"$1"),t,n,r,a)}function rt(e){return function(t){var n=t.nodeName.toLowerCase();return n==="input"&&t.type===e}}function it(e){return function(t){var n=t.nodeName.toLowerCase();return(n==="input"||n==="button")&&t.type===e}}function st(e){return N(function(t){return t=+t,N(function(n,r){var i,s=e([],n.length,t),o=s.length;while(o--)n[i=s[o]]&&(n[i]=!(r[i]=n[i]))})})}function ot(e,t,n){if(e===t)return n;var r=e.nextSibling;while(r){if(r===t)return-1;r=r.nextSibling}return 1}function ut(e,t){var n,r,s,o,u,a,f,l=L[d][e+" "];if(l)return t?0:l.slice(0);u=e,a=[],f=i.preFilter;while(u){if(!n||(r=F.exec(u)))r&&(u=u.slice(r[0].length)||u),a.push(s=[]);n=!1;if(r=I.exec(u))s.push(n=new m(r.shift())),u=u.slice(n.length),n.type=r[0].replace(j," ");for(o in i.filter)(r=J[o].exec(u))&&(!f[o]||(r=f[o](r)))&&(s.push(n=new m(r.shift())),u=u.slice(n.length),n.type=o,n.matches=r);if(!n)break}return t?u.length:u?nt.error(e):L(e,a).slice(0)}function at(e,t,r){var i=t.dir,s=r&&t.dir==="parentNode",o=w++;return t.first?function(t,n,r){while(t=t[i])if(s||t.nodeType===1)return e(t,n,r)}:function(t,r,u){if(!u){var a,f=b+" "+o+" ",l=f+n;while(t=t[i])if(s||t.nodeType===1){if((a=t[d])===l)return t.sizset;if(typeof a=="string"&&a.indexOf(f)===0){if(t.sizset)return t}else{t[d]=l;if(e(t,r,u))return t.sizset=!0,t;t.sizset=!1}}}else while(t=t[i])if(s||t.nodeType===1)if(e(t,r,u))return t}}function ft(e){return e.length>1?function(t,n,r){var i=e.length;while(i--)if(!e[i](t,n,r))return!1;return!0}:e[0]}function lt(e,t,n,r,i){var s,o=[],u=0,a=e.length,f=t!=null;for(;u-1&&(s[f]=!(o[f]=c))}}else g=lt(g===o?g.splice(d,g.length):g),i?i(null,o,g,a):S.apply(o,g)})}function ht(e){var t,n,r,s=e.length,o=i.relative[e[0].type],u=o||i.relative[" "],a=o?1:0,f=at(function(e){return e===t},u,!0),l=at(function(e){return T.call(t,e)>-1},u,!0),h=[function(e,n,r){return!o&&(r||n!==c)||((t=n).nodeType?f(e,n,r):l(e,n,r))}];for(;a1&&ft(h),a>1&&e.slice(0,a-1).join("").replace(j,"$1"),n,a0,s=e.length>0,o=function(u,a,f,l,h){var p,d,v,m=[],y=0,w="0",x=u&&[],T=h!=null,N=c,C=u||s&&i.find.TAG("*",h&&a.parentNode||a),k=b+=N==null?1:Math.E;T&&(c=a!==g&&a,n=o.el);for(;(p=C[w])!=null;w++){if(s&&p){for(d=0;v=e[d];d++)if(v(p,a,f)){l.push(p);break}T&&(b=k,n=++o.el)}r&&((p=!v&&p)&&y--,u&&x.push(p))}y+=w;if(r&&w!==y){for(d=0;v=t[d];d++)v(x,m,a,f);if(u){if(y>0)while(w--)!x[w]&&!m[w]&&(m[w]=E.call(l));m=lt(m)}S.apply(l,m),T&&!u&&m.length>0&&y+t.length>1&&nt.uniqueSort(l)}return T&&(b=k,c=N),x};return o.el=0,r?N(o):o}function dt(e,t,n){var r=0,i=t.length;for(;r2&&(f=u[0]).type==="ID"&&t.nodeType===9&&!s&&i.relative[u[1].type]){t=i.find.ID(f.matches[0].replace($,""),t,s)[0];if(!t)return n;e=e.slice(u.shift().length)}for(o=J.POS.test(e)?-1:u.length-1;o>=0;o--){f=u[o];if(i.relative[l=f.type])break;if(c=i.find[l])if(r=c(f.matches[0].replace($,""),z.test(u[0].type)&&t.parentNode||t,s)){u.splice(o,1),e=r.length&&u.join("");if(!e)return S.apply(n,x.call(r,0)),n;break}}}return a(e,h)(r,t,s,n,z.test(e)),n}function mt(){}var n,r,i,s,o,u,a,f,l,c,h=!0,p="undefined",d=("sizcache"+Math.random()).replace(".",""),m=String,g=e.document,y=g.documentElement,b=0,w=0,E=[].pop,S=[].push,x=[].slice,T=[].indexOf||function(e){var t=0,n=this.length;for(;ti.cacheLength&&delete e[t.shift()],e[n+" "]=r},e)},k=C(),L=C(),A=C(),O="[\\x20\\t\\r\\n\\f]",M="(?:\\\\.|[-\\w]|[^\\x00-\\xa0])+",_=M.replace("w","w#"),D="([*^$|!~]?=)",P="\\["+O+"*("+M+")"+O+"*(?:"+D+O+"*(?:(['\"])((?:\\\\.|[^\\\\])*?)\\3|("+_+")|)|)"+O+"*\\]",H=":("+M+")(?:\\((?:(['\"])((?:\\\\.|[^\\\\])*?)\\2|([^()[\\]]*|(?:(?:"+P+")|[^:]|\\\\.)*|.*))\\)|)",B=":(even|odd|eq|gt|lt|nth|first|last)(?:\\("+O+"*((?:-\\d)?\\d*)"+O+"*\\)|)(?=[^-]|$)",j=new RegExp("^"+O+"+|((?:^|[^\\\\])(?:\\\\.)*)"+O+"+$","g"),F=new RegExp("^"+O+"*,"+O+"*"),I=new RegExp("^"+O+"*([\\x20\\t\\r\\n\\f>+~])"+O+"*"),q=new RegExp(H),R=/^(?:#([\w\-]+)|(\w+)|\.([\w\-]+))$/,U=/^:not/,z=/[\x20\t\r\n\f]*[+~]/,W=/:not\($/,X=/h\d/i,V=/input|select|textarea|button/i,$=/\\(?!\\)/g,J={ID:new RegExp("^#("+M+")"),CLASS:new RegExp("^\\.("+M+")"),NAME:new RegExp("^\\[name=['\"]?("+M+")['\"]?\\]"),TAG:new RegExp("^("+M.replace("w","w*")+")"),ATTR:new RegExp("^"+P),PSEUDO:new RegExp("^"+H),POS:new RegExp(B,"i"),CHILD:new RegExp("^:(only|nth|first|last)-child(?:\\("+O+"*(even|odd|(([+-]|)(\\d*)n|)"+O+"*(?:([+-]|)"+O+"*(\\d+)|))"+O+"*\\)|)","i"),needsContext:new RegExp("^"+O+"*[>+~]|"+B,"i")},K=function(e){var t=g.createElement("div");try{return e(t)}catch(n){return!1}finally{t=null}},Q=K(function(e){return e.appendChild(g.createComment("")),!e.getElementsByTagName("*").length}),G=K(function(e){return e.innerHTML="",e.firstChild&&typeof e.firstChild.getAttribute!==p&&e.firstChild.getAttribute("href")==="#"}),Y=K(function(e){e.innerHTML="";var t=typeof e.lastChild.getAttribute("multiple");return t!=="boolean"&&t!=="string"}),Z=K(function(e){return e.innerHTML="",!e.getElementsByClassName||!e.getElementsByClassName("e").length?!1:(e.lastChild.className="e",e.getElementsByClassName("e").length===2)}),et=K(function(e){e.id=d+0,e.innerHTML="
",y.insertBefore(e,y.firstChild);var t=g.getElementsByName&&g.getElementsByName(d).length===2+g.getElementsByName(d+0).length;return r=!g.getElementById(d),y.removeChild(e),t});try{x.call(y.childNodes,0)[0].nodeType}catch(tt){x=function(e){var t,n=[];for(;t=this[e];e++)n.push(t);return n}}nt.matches=function(e,t){return nt(e,null,null,t)},nt.matchesSelector=function(e,t){return nt(t,null,null,[e]).length>0},s=nt.getText=function(e){var t,n="",r=0,i=e.nodeType;if(i){if(i===1||i===9||i===11){if(typeof e.textContent=="string")return e.textContent;for(e=e.firstChild;e;e=e.nextSibling)n+=s(e)}else if(i===3||i===4)return e.nodeValue}else for(;t=e[r];r++)n+=s(t);return n},o=nt.isXML=function(e){var t=e&&(e.ownerDocument||e).documentElement;return t?t.nodeName!=="HTML":!1},u=nt.contains=y.contains?function(e,t){var n=e.nodeType===9?e.documentElement:e,r=t&&t.parentNode;return e===r||!!(r&&r.nodeType===1&&n.contains&&n.contains(r))}:y.compareDocumentPosition?function(e,t){return t&&!!(e.compareDocumentPosition(t)&16)}:function(e,t){while(t=t.parentNode)if(t===e)return!0;return!1},nt.attr=function(e,t){var n,r=o(e);return r||(t=t.toLowerCase()),(n=i.attrHandle[t])?n(e):r||Y?e.getAttribute(t):(n=e.getAttributeNode(t),n?typeof e[t]=="boolean"?e[t]?t:null:n.specified?n.value:null:null)},i=nt.selectors={cacheLength:50,createPseudo:N,match:J,attrHandle:G?{}:{href:function(e){return e.getAttribute("href",2)},type:function(e){return e.getAttribute("type")}},find:{ID:r?function(e,t,n){if(typeof t.getElementById!==p&&!n){var r=t.getElementById(e);return r&&r.parentNode?[r]:[]}}:function(e,n,r){if(typeof n.getElementById!==p&&!r){var i=n.getElementById(e);return i?i.id===e||typeof i.getAttributeNode!==p&&i.getAttributeNode("id").value===e?[i]:t:[]}},TAG:Q?function(e,t){if(typeof t.getElementsByTagName!==p)return t.getElementsByTagName(e)}:function(e,t){var n=t.getElementsByTagName(e);if(e==="*"){var r,i=[],s=0;for(;r=n[s];s++)r.nodeType===1&&i.push(r);return i}return n},NAME:et&&function(e,t){if(typeof t.getElementsByName!==p)return t.getElementsByName(name)},CLASS:Z&&function(e,t,n){if(typeof t.getElementsByClassName!==p&&!n)return t.getElementsByClassName(e)}},relative:{">":{dir:"parentNode",first:!0}," ":{dir:"parentNode"},"+":{dir:"previousSibling",first:!0},"~":{dir:"previousSibling"}},preFilter:{ATTR:function(e){return e[1]=e[1].replace($,""),e[3]=(e[4]||e[5]||"").replace($,""),e[2]==="~="&&(e[3]=" "+e[3]+" "),e.slice(0,4)},CHILD:function(e){return e[1]=e[1].toLowerCase(),e[1]==="nth"?(e[2]||nt.error(e[0]),e[3]=+(e[3]?e[4]+(e[5]||1):2*(e[2]==="even"||e[2]==="odd")),e[4]=+(e[6]+e[7]||e[2]==="odd")):e[2]&&nt.error(e[0]),e},PSEUDO:function(e){var t,n;if(J.CHILD.test(e[0]))return null;if(e[3])e[2]=e[3];else if(t=e[4])q.test(t)&&(n=ut(t,!0))&&(n=t.indexOf(")",t.length-n)-t.length)&&(t=t.slice(0,n),e[0]=e[0].slice(0,n)),e[2]=t;return e.slice(0,3)}},filter:{ID:r?function(e){return e=e.replace($,""),function(t){return t.getAttribute("id")===e}}:function(e){return e=e.replace($,""),function(t){var n=typeof t.getAttributeNode!==p&&t.getAttributeNode("id");return n&&n.value===e}},TAG:function(e){return e==="*"?function(){return!0}:(e=e.replace($,"").toLowerCase(),function(t){return t.nodeName&&t.nodeName.toLowerCase()===e})},CLASS:function(e){var t=k[d][e+" "];return t||(t=new RegExp("(^|"+O+")"+e+"("+O+"|$)"))&&k(e,function(e){return t.test(e.className||typeof e.getAttribute!==p&&e.getAttribute("class")||"")})},ATTR:function(e,t,n){return function(r,i){var s=nt.attr(r,e);return s==null?t==="!=":t?(s+="",t==="="?s===n:t==="!="?s!==n:t==="^="?n&&s.indexOf(n)===0:t==="*="?n&&s.indexOf(n)>-1:t==="$="?n&&s.substr(s.length-n.length)===n:t==="~="?(" "+s+" ").indexOf(n)>-1:t==="|="?s===n||s.substr(0,n.length+1)===n+"-":!1):!0}},CHILD:function(e,t,n,r){return e==="nth"?function(e){var t,i,s=e.parentNode;if(n===1&&r===0)return!0;if(s){i=0;for(t=s.firstChild;t;t=t.nextSibling)if(t.nodeType===1){i++;if(e===t)break}}return i-=r,i===n||i%n===0&&i/n>=0}:function(t){var n=t;switch(e){case"only":case"first":while(n=n.previousSibling)if(n.nodeType===1)return!1;if(e==="first")return!0;n=t;case"last":while(n=n.nextSibling)if(n.nodeType===1)return!1;return!0}}},PSEUDO:function(e,t){var n,r=i.pseudos[e]||i.setFilters[e.toLowerCase()]||nt.error("unsupported pseudo: "+e);return r[d]?r(t):r.length>1?(n=[e,e,"",t],i.setFilters.hasOwnProperty(e.toLowerCase())?N(function(e,n){var i,s=r(e,t),o=s.length;while(o--)i=T.call(e,s[o]),e[i]=!(n[i]=s[o])}):function(e){return r(e,0,n)}):r}},pseudos:{not:N(function(e){var t=[],n=[],r=a(e.replace(j,"$1"));return r[d]?N(function(e,t,n,i){var s,o=r(e,null,i,[]),u=e.length;while(u--)if(s=o[u])e[u]=!(t[u]=s)}):function(e,i,s){return t[0]=e,r(t,null,s,n),!n.pop()}}),has:N(function(e){return function(t){return nt(e,t).length>0}}),contains:N(function(e){return function(t){return(t.textContent||t.innerText||s(t)).indexOf(e)>-1}}),enabled:function(e){return e.disabled===!1},disabled:function(e){return e.disabled===!0},checked:function(e){var t=e.nodeName.toLowerCase();return t==="input"&&!!e.checked||t==="option"&&!!e.selected},selected:function(e){return e.parentNode&&e.parentNode.selectedIndex,e.selected===!0},parent:function(e){return!i.pseudos.empty(e)},empty:function(e){var t;e=e.firstChild;while(e){if(e.nodeName>"@"||(t=e.nodeType)===3||t===4)return!1;e=e.nextSibling}return!0},header:function(e){return X.test(e.nodeName)},text:function(e){var t,n;return e.nodeName.toLowerCase()==="input"&&(t=e.type)==="text"&&((n=e.getAttribute("type"))==null||n.toLowerCase()===t)},radio:rt("radio"),checkbox:rt("checkbox"),file:rt("file"),password:rt("password"),image:rt("image"),submit:it("submit"),reset:it("reset"),button:function(e){var t=e.nodeName.toLowerCase();return t==="input"&&e.type==="button"||t==="button"},input:function(e){return V.test(e.nodeName)},focus:function(e){var t=e.ownerDocument;return e===t.activeElement&&(!t.hasFocus||t.hasFocus())&&!!(e.type||e.href||~e.tabIndex)},active:function(e){return e===e.ownerDocument.activeElement},first:st(function(){return[0]}),last:st(function(e,t){return[t-1]}),eq:st(function(e,t,n){return[n<0?n+t:n]}),even:st(function(e,t){for(var n=0;n=0;)e.push(r);return e}),gt:st(function(e,t,n){for(var r=n<0?n+t:n;++r",e.querySelectorAll("[selected]").length||i.push("\\["+O+"*(?:checked|disabled|ismap|multiple|readonly|selected|value)"),e.querySelectorAll(":checked").length||i.push(":checked")}),K(function(e){e.innerHTML="

",e.querySelectorAll("[test^='']").length&&i.push("[*^$]="+O+"*(?:\"\"|'')"),e.innerHTML="",e.querySelectorAll(":enabled").length||i.push(":enabled",":disabled")}),i=new RegExp(i.join("|")),vt=function(e,r,s,o,u){if(!o&&!u&&!i.test(e)){var a,f,l=!0,c=d,h=r,p=r.nodeType===9&&e;if(r.nodeType===1&&r.nodeName.toLowerCase()!=="object"){a=ut(e),(l=r.getAttribute("id"))?c=l.replace(n,"\\$&"):r.setAttribute("id",c),c="[id='"+c+"'] ",f=a.length;while(f--)a[f]=c+a[f].join("");h=z.test(e)&&r.parentNode||r,p=a.join(",")}if(p)try{return S.apply(s,x.call(h.querySelectorAll(p),0)),s}catch(v){}finally{l||r.removeAttribute("id")}}return t(e,r,s,o,u)},u&&(K(function(t){e=u.call(t,"div");try{u.call(t,"[test!='']:sizzle"),s.push("!=",H)}catch(n){}}),s=new RegExp(s.join("|")),nt.matchesSelector=function(t,n){n=n.replace(r,"='$1']");if(!o(t)&&!s.test(n)&&!i.test(n))try{var a=u.call(t,n);if(a||e||t.document&&t.document.nodeType!==11)return a}catch(f){}return nt(n,null,null,[t]).length>0})}(),i.pseudos.nth=i.pseudos.eq,i.filters=mt.prototype=i.pseudos,i.setFilters=new mt,nt.attr=v.attr,v.find=nt,v.expr=nt.selectors,v.expr[":"]=v.expr.pseudos,v.unique=nt.uniqueSort,v.text=nt.getText,v.isXMLDoc=nt.isXML,v.contains=nt.contains}(e);var nt=/Until$/,rt=/^(?:parents|prev(?:Until|All))/,it=/^.[^:#\[\.,]*$/,st=v.expr.match.needsContext,ot={children:!0,contents:!0,next:!0,prev:!0};v.fn.extend({find:function(e){var t,n,r,i,s,o,u=this;if(typeof e!="string")return v(e).filter(function(){for(t=0,n=u.length;t0)for(i=r;i=0:v.filter(e,this).length>0:this.filter(e).length>0)},closest:function(e,t){var n,r=0,i=this.length,s=[],o=st.test(e)||typeof e!="string"?v(e,t||this.context):0;for(;r-1:v.find.matchesSelector(n,e)){s.push(n);break}n=n.parentNode}}return s=s.length>1?v.unique(s):s,this.pushStack(s,"closest",e)},index:function(e){return e?typeof e=="string"?v.inArray(this[0],v(e)):v.inArray(e.jquery?e[0]:e,this):this[0]&&this[0].parentNode?this.prevAll().length:-1},add:function(e,t){var n=typeof e=="string"?v(e,t):v.makeArray(e&&e.nodeType?[e]:e),r=v.merge(this.get(),n);return this.pushStack(ut(n[0])||ut(r[0])?r:v.unique(r))},addBack:function(e){return this.add(e==null?this.prevObject:this.prevObject.filter(e))}}),v.fn.andSelf=v.fn.addBack,v.each({parent:function(e){var t=e.parentNode;return t&&t.nodeType!==11?t:null},parents:function(e){return v.dir(e,"parentNode")},parentsUntil:function(e,t,n){return v.dir(e,"parentNode",n)},next:function(e){return at(e,"nextSibling")},prev:function(e){return at(e,"previousSibling")},nextAll:function(e){return v.dir(e,"nextSibling")},prevAll:function(e){return v.dir(e,"previousSibling")},nextUntil:function(e,t,n){return v.dir(e,"nextSibling",n)},prevUntil:function(e,t,n){return v.dir(e,"previousSibling",n)},siblings:function(e){return v.sibling((e.parentNode||{}).firstChild,e)},children:function(e){return v.sibling(e.firstChild)},contents:function(e){return v.nodeName(e,"iframe")?e.contentDocument||e.contentWindow.document:v.merge([],e.childNodes)}},function(e,t){v.fn[e]=function(n,r){var i=v.map(this,t,n);return nt.test(e)||(r=n),r&&typeof r=="string"&&(i=v.filter(r,i)),i=this.length>1&&!ot[e]?v.unique(i):i,this.length>1&&rt.test(e)&&(i=i.reverse()),this.pushStack(i,e,l.call(arguments).join(","))}}),v.extend({filter:function(e,t,n){return n&&(e=":not("+e+")"),t.length===1?v.find.matchesSelector(t[0],e)?[t[0]]:[]:v.find.matches(e,t)},dir:function(e,n,r){var i=[],s=e[n];while(s&&s.nodeType!==9&&(r===t||s.nodeType!==1||!v(s).is(r)))s.nodeType===1&&i.push(s),s=s[n];return i},sibling:function(e,t){var n=[];for(;e;e=e.nextSibling)e.nodeType===1&&e!==t&&n.push(e);return n}});var ct="abbr|article|aside|audio|bdi|canvas|data|datalist|details|figcaption|figure|footer|header|hgroup|mark|meter|nav|output|progress|section|summary|time|video",ht=/ jQuery\d+="(?:null|\d+)"/g,pt=/^\s+/,dt=/<(?!area|br|col|embed|hr|img|input|link|meta|param)(([\w:]+)[^>]*)\/>/gi,vt=/<([\w:]+)/,mt=/]","i"),Et=/^(?:checkbox|radio)$/,St=/checked\s*(?:[^=]|=\s*.checked.)/i,xt=/\/(java|ecma)script/i,Tt=/^\s*\s*$/g,Nt={option:[1,""],legend:[1,"
","
"],thead:[1,"","
"],tr:[2,"","
"],td:[3,"","
"],col:[2,"","
"],area:[1,"",""],_default:[0,"",""]},Ct=lt(i),kt=Ct.appendChild(i.createElement("div"));Nt.optgroup=Nt.option,Nt.tbody=Nt.tfoot=Nt.colgroup=Nt.caption=Nt.thead,Nt.th=Nt.td,v.support.htmlSerialize||(Nt._default=[1,"X
","
"]),v.fn.extend({text:function(e){return v.access(this,function(e){return e===t?v.text(this):this.empty().append((this[0]&&this[0].ownerDocument||i).createTextNode(e))},null,e,arguments.length)},wrapAll:function(e){if(v.isFunction(e))return this.each(function(t){v(this).wrapAll(e.call(this,t))});if(this[0]){var t=v(e,this[0].ownerDocument).eq(0).clone(!0);this[0].parentNode&&t.insertBefore(this[0]),t.map(function(){var e=this;while(e.firstChild&&e.firstChild.nodeType===1)e=e.firstChild;return e}).append(this)}return this},wrapInner:function(e){return v.isFunction(e)?this.each(function(t){v(this).wrapInner(e.call(this,t))}):this.each(function(){var t=v(this),n=t.contents();n.length?n.wrapAll(e):t.append(e)})},wrap:function(e){var t=v.isFunction(e);return this.each(function(n){v(this).wrapAll(t?e.call(this,n):e)})},unwrap:function(){return this.parent().each(function(){v.nodeName(this,"body")||v(this).replaceWith(this.childNodes)}).end()},append:function(){return this.domManip(arguments,!0,function(e){(this.nodeType===1||this.nodeType===11)&&this.appendChild(e)})},prepend:function(){return this.domManip(arguments,!0,function(e){(this.nodeType===1||this.nodeType===11)&&this.insertBefore(e,this.firstChild)})},before:function(){if(!ut(this[0]))return this.domManip(arguments,!1,function(e){this.parentNode.insertBefore(e,this)});if(arguments.length){var e=v.clean(arguments);return this.pushStack(v.merge(e,this),"before",this.selector)}},after:function(){if(!ut(this[0]))return this.domManip(arguments,!1,function(e){this.parentNode.insertBefore(e,this.nextSibling)});if(arguments.length){var e=v.clean(arguments);return this.pushStack(v.merge(this,e),"after",this.selector)}},remove:function(e,t){var n,r=0;for(;(n=this[r])!=null;r++)if(!e||v.filter(e,[n]).length)!t&&n.nodeType===1&&(v.cleanData(n.getElementsByTagName("*")),v.cleanData([n])),n.parentNode&&n.parentNode.removeChild(n);return this},empty:function(){var e,t=0;for(;(e=this[t])!=null;t++){e.nodeType===1&&v.cleanData(e.getElementsByTagName("*"));while(e.firstChild)e.removeChild(e.firstChild)}return this},clone:function(e,t){return e=e==null?!1:e,t=t==null?e:t,this.map(function(){return v.clone(this,e,t)})},html:function(e){return v.access(this,function(e){var n=this[0]||{},r=0,i=this.length;if(e===t)return n.nodeType===1?n.innerHTML.replace(ht,""):t;if(typeof e=="string"&&!yt.test(e)&&(v.support.htmlSerialize||!wt.test(e))&&(v.support.leadingWhitespace||!pt.test(e))&&!Nt[(vt.exec(e)||["",""])[1].toLowerCase()]){e=e.replace(dt,"<$1>");try{for(;r1&&typeof f=="string"&&St.test(f))return this.each(function(){v(this).domManip(e,n,r)});if(v.isFunction(f))return this.each(function(i){var s=v(this);e[0]=f.call(this,i,n?s.html():t),s.domManip(e,n,r)});if(this[0]){i=v.buildFragment(e,this,l),o=i.fragment,s=o.firstChild,o.childNodes.length===1&&(o=s);if(s){n=n&&v.nodeName(s,"tr");for(u=i.cacheable||c-1;a0?this.clone(!0):this).get(),v(o[i])[t](r),s=s.concat(r);return this.pushStack(s,e,o.selector)}}),v.extend({clone:function(e,t,n){var r,i,s,o;v.support.html5Clone||v.isXMLDoc(e)||!wt.test("<"+e.nodeName+">")?o=e.cloneNode(!0):(kt.innerHTML=e.outerHTML,kt.removeChild(o=kt.firstChild));if((!v.support.noCloneEvent||!v.support.noCloneChecked)&&(e.nodeType===1||e.nodeType===11)&&!v.isXMLDoc(e)){Ot(e,o),r=Mt(e),i=Mt(o);for(s=0;r[s];++s)i[s]&&Ot(r[s],i[s])}if(t){At(e,o);if(n){r=Mt(e),i=Mt(o);for(s=0;r[s];++s)At(r[s],i[s])}}return r=i=null,o},clean:function(e,t,n,r){var s,o,u,a,f,l,c,h,p,d,m,g,y=t===i&&Ct,b=[];if(!t||typeof t.createDocumentFragment=="undefined")t=i;for(s=0;(u=e[s])!=null;s++){typeof u=="number"&&(u+="");if(!u)continue;if(typeof u=="string")if(!gt.test(u))u=t.createTextNode(u);else{y=y||lt(t),c=t.createElement("div"),y.appendChild(c),u=u.replace(dt,"<$1>"),a=(vt.exec(u)||["",""])[1].toLowerCase(),f=Nt[a]||Nt._default,l=f[0],c.innerHTML=f[1]+u+f[2];while(l--)c=c.lastChild;if(!v.support.tbody){h=mt.test(u),p=a==="table"&&!h?c.firstChild&&c.firstChild.childNodes:f[1]===""&&!h?c.childNodes:[];for(o=p.length-1;o>=0;--o)v.nodeName(p[o],"tbody")&&!p[o].childNodes.length&&p[o].parentNode.removeChild(p[o])}!v.support.leadingWhitespace&&pt.test(u)&&c.insertBefore(t.createTextNode(pt.exec(u)[0]),c.firstChild),u=c.childNodes,c.parentNode.removeChild(c)}u.nodeType?b.push(u):v.merge(b,u)}c&&(u=c=y=null);if(!v.support.appendChecked)for(s=0;(u=b[s])!=null;s++)v.nodeName(u,"input")?_t(u):typeof u.getElementsByTagName!="undefined"&&v.grep(u.getElementsByTagName("input"),_t);if(n){m=function(e){if(!e.type||xt.test(e.type))return r?r.push(e.parentNode?e.parentNode.removeChild(e):e):n.appendChild(e)};for(s=0;(u=b[s])!=null;s++)if(!v.nodeName(u,"script")||!m(u))n.appendChild(u),typeof u.getElementsByTagName!="undefined"&&(g=v.grep(v.merge([],u.getElementsByTagName("script")),m),b.splice.apply(b,[s+1,0].concat(g)),s+=g.length)}return b},cleanData:function(e,t){var n,r,i,s,o=0,u=v.expando,a=v.cache,f=v.support.deleteExpando,l=v.event.special;for(;(i=e[o])!=null;o++)if(t||v.acceptData(i)){r=i[u],n=r&&a[r];if(n){if(n.events)for(s in n.events)l[s]?v.event.remove(i,s):v.removeEvent(i,s,n.handle);a[r]&&(delete a[r],f?delete i[u]:i.removeAttribute?i.removeAttribute(u):i[u]=null,v.deletedIds.push(r))}}}}),function(){var e,t;v.uaMatch=function(e){e=e.toLowerCase();var t=/(chrome)[ \/]([\w.]+)/.exec(e)||/(webkit)[ \/]([\w.]+)/.exec(e)||/(opera)(?:.*version|)[ \/]([\w.]+)/.exec(e)||/(msie) ([\w.]+)/.exec(e)||e.indexOf("compatible")<0&&/(mozilla)(?:.*? rv:([\w.]+)|)/.exec(e)||[];return{browser:t[1]||"",version:t[2]||"0"}},e=v.uaMatch(o.userAgent),t={},e.browser&&(t[e.browser]=!0,t.version=e.version),t.chrome?t.webkit=!0:t.webkit&&(t.safari=!0),v.browser=t,v.sub=function(){function e(t,n){return new e.fn.init(t,n)}v.extend(!0,e,this),e.superclass=this,e.fn=e.prototype=this(),e.fn.constructor=e,e.sub=this.sub,e.fn.init=function(r,i){return i&&i instanceof v&&!(i instanceof e)&&(i=e(i)),v.fn.init.call(this,r,i,t)},e.fn.init.prototype=e.fn;var t=e(i);return e}}();var Dt,Pt,Ht,Bt=/alpha\([^)]*\)/i,jt=/opacity=([^)]*)/,Ft=/^(top|right|bottom|left)$/,It=/^(none|table(?!-c[ea]).+)/,qt=/^margin/,Rt=new RegExp("^("+m+")(.*)$","i"),Ut=new RegExp("^("+m+")(?!px)[a-z%]+$","i"),zt=new RegExp("^([-+])=("+m+")","i"),Wt={BODY:"block"},Xt={position:"absolute",visibility:"hidden",display:"block"},Vt={letterSpacing:0,fontWeight:400},$t=["Top","Right","Bottom","Left"],Jt=["Webkit","O","Moz","ms"],Kt=v.fn.toggle;v.fn.extend({css:function(e,n){return v.access(this,function(e,n,r){return r!==t?v.style(e,n,r):v.css(e,n)},e,n,arguments.length>1)},show:function(){return Yt(this,!0)},hide:function(){return Yt(this)},toggle:function(e,t){var n=typeof e=="boolean";return v.isFunction(e)&&v.isFunction(t)?Kt.apply(this,arguments):this.each(function(){(n?e:Gt(this))?v(this).show():v(this).hide()})}}),v.extend({cssHooks:{opacity:{get:function(e,t){if(t){var n=Dt(e,"opacity");return n===""?"1":n}}}},cssNumber:{fillOpacity:!0,fontWeight:!0,lineHeight:!0,opacity:!0,orphans:!0,widows:!0,zIndex:!0,zoom:!0},cssProps:{"float":v.support.cssFloat?"cssFloat":"styleFloat"},style:function(e,n,r,i){if(!e||e.nodeType===3||e.nodeType===8||!e.style)return;var s,o,u,a=v.camelCase(n),f=e.style;n=v.cssProps[a]||(v.cssProps[a]=Qt(f,a)),u=v.cssHooks[n]||v.cssHooks[a];if(r===t)return u&&"get"in u&&(s=u.get(e,!1,i))!==t?s:f[n];o=typeof r,o==="string"&&(s=zt.exec(r))&&(r=(s[1]+1)*s[2]+parseFloat(v.css(e,n)),o="number");if(r==null||o==="number"&&isNaN(r))return;o==="number"&&!v.cssNumber[a]&&(r+="px");if(!u||!("set"in u)||(r=u.set(e,r,i))!==t)try{f[n]=r}catch(l){}},css:function(e,n,r,i){var s,o,u,a=v.camelCase(n);return n=v.cssProps[a]||(v.cssProps[a]=Qt(e.style,a)),u=v.cssHooks[n]||v.cssHooks[a],u&&"get"in u&&(s=u.get(e,!0,i)),s===t&&(s=Dt(e,n)),s==="normal"&&n in Vt&&(s=Vt[n]),r||i!==t?(o=parseFloat(s),r||v.isNumeric(o)?o||0:s):s},swap:function(e,t,n){var r,i,s={};for(i in t)s[i]=e.style[i],e.style[i]=t[i];r=n.call(e);for(i in t)e.style[i]=s[i];return r}}),e.getComputedStyle?Dt=function(t,n){var r,i,s,o,u=e.getComputedStyle(t,null),a=t.style;return u&&(r=u.getPropertyValue(n)||u[n],r===""&&!v.contains(t.ownerDocument,t)&&(r=v.style(t,n)),Ut.test(r)&&qt.test(n)&&(i=a.width,s=a.minWidth,o=a.maxWidth,a.minWidth=a.maxWidth=a.width=r,r=u.width,a.width=i,a.minWidth=s,a.maxWidth=o)),r}:i.documentElement.currentStyle&&(Dt=function(e,t){var n,r,i=e.currentStyle&&e.currentStyle[t],s=e.style;return i==null&&s&&s[t]&&(i=s[t]),Ut.test(i)&&!Ft.test(t)&&(n=s.left,r=e.runtimeStyle&&e.runtimeStyle.left,r&&(e.runtimeStyle.left=e.currentStyle.left),s.left=t==="fontSize"?"1em":i,i=s.pixelLeft+"px",s.left=n,r&&(e.runtimeStyle.left=r)),i===""?"auto":i}),v.each(["height","width"],function(e,t){v.cssHooks[t]={get:function(e,n,r){if(n)return e.offsetWidth===0&&It.test(Dt(e,"display"))?v.swap(e,Xt,function(){return tn(e,t,r)}):tn(e,t,r)},set:function(e,n,r){return Zt(e,n,r?en(e,t,r,v.support.boxSizing&&v.css(e,"boxSizing")==="border-box"):0)}}}),v.support.opacity||(v.cssHooks.opacity={get:function(e,t){return jt.test((t&&e.currentStyle?e.currentStyle.filter:e.style.filter)||"")?.01*parseFloat(RegExp.$1)+"":t?"1":""},set:function(e,t){var n=e.style,r=e.currentStyle,i=v.isNumeric(t)?"alpha(opacity="+t*100+")":"",s=r&&r.filter||n.filter||"";n.zoom=1;if(t>=1&&v.trim(s.replace(Bt,""))===""&&n.removeAttribute){n.removeAttribute("filter");if(r&&!r.filter)return}n.filter=Bt.test(s)?s.replace(Bt,i):s+" "+i}}),v(function(){v.support.reliableMarginRight||(v.cssHooks.marginRight={get:function(e,t){return v.swap(e,{display:"inline-block"},function(){if(t)return Dt(e,"marginRight")})}}),!v.support.pixelPosition&&v.fn.position&&v.each(["top","left"],function(e,t){v.cssHooks[t]={get:function(e,n){if(n){var r=Dt(e,t);return Ut.test(r)?v(e).position()[t]+"px":r}}}})}),v.expr&&v.expr.filters&&(v.expr.filters.hidden=function(e){return e.offsetWidth===0&&e.offsetHeight===0||!v.support.reliableHiddenOffsets&&(e.style&&e.style.display||Dt(e,"display"))==="none"},v.expr.filters.visible=function(e){return!v.expr.filters.hidden(e)}),v.each({margin:"",padding:"",border:"Width"},function(e,t){v.cssHooks[e+t]={expand:function(n){var r,i=typeof n=="string"?n.split(" "):[n],s={};for(r=0;r<4;r++)s[e+$t[r]+t]=i[r]||i[r-2]||i[0];return s}},qt.test(e)||(v.cssHooks[e+t].set=Zt)});var rn=/%20/g,sn=/\[\]$/,on=/\r?\n/g,un=/^(?:color|date|datetime|datetime-local|email|hidden|month|number|password|range|search|tel|text|time|url|week)$/i,an=/^(?:select|textarea)/i;v.fn.extend({serialize:function(){return v.param(this.serializeArray())},serializeArray:function(){return this.map(function(){return this.elements?v.makeArray(this.elements):this}).filter(function(){return this.name&&!this.disabled&&(this.checked||an.test(this.nodeName)||un.test(this.type))}).map(function(e,t){var n=v(this).val();return n==null?null:v.isArray(n)?v.map(n,function(e,n){return{name:t.name,value:e.replace(on,"\r\n")}}):{name:t.name,value:n.replace(on,"\r\n")}}).get()}}),v.param=function(e,n){var r,i=[],s=function(e,t){t=v.isFunction(t)?t():t==null?"":t,i[i.length]=encodeURIComponent(e)+"="+encodeURIComponent(t)};n===t&&(n=v.ajaxSettings&&v.ajaxSettings.traditional);if(v.isArray(e)||e.jquery&&!v.isPlainObject(e))v.each(e,function(){s(this.name,this.value)});else for(r in e)fn(r,e[r],n,s);return i.join("&").replace(rn,"+")};var ln,cn,hn=/#.*$/,pn=/^(.*?):[ \t]*([^\r\n]*)\r?$/mg,dn=/^(?:about|app|app\-storage|.+\-extension|file|res|widget):$/,vn=/^(?:GET|HEAD)$/,mn=/^\/\//,gn=/\?/,yn=/)<[^<]*)*<\/script>/gi,bn=/([?&])_=[^&]*/,wn=/^([\w\+\.\-]+:)(?:\/\/([^\/?#:]*)(?::(\d+)|)|)/,En=v.fn.load,Sn={},xn={},Tn=["*/"]+["*"];try{cn=s.href}catch(Nn){cn=i.createElement("a"),cn.href="",cn=cn.href}ln=wn.exec(cn.toLowerCase())||[],v.fn.load=function(e,n,r){if(typeof e!="string"&&En)return En.apply(this,arguments);if(!this.length)return this;var i,s,o,u=this,a=e.indexOf(" ");return a>=0&&(i=e.slice(a,e.length),e=e.slice(0,a)),v.isFunction(n)?(r=n,n=t):n&&typeof n=="object"&&(s="POST"),v.ajax({url:e,type:s,dataType:"html",data:n,complete:function(e,t){r&&u.each(r,o||[e.responseText,t,e])}}).done(function(e){o=arguments,u.html(i?v("
").append(e.replace(yn,"")).find(i):e)}),this},v.each("ajaxStart ajaxStop ajaxComplete ajaxError ajaxSuccess ajaxSend".split(" "),function(e,t){v.fn[t]=function(e){return this.on(t,e)}}),v.each(["get","post"],function(e,n){v[n]=function(e,r,i,s){return v.isFunction(r)&&(s=s||i,i=r,r=t),v.ajax({type:n,url:e,data:r,success:i,dataType:s})}}),v.extend({getScript:function(e,n){return v.get(e,t,n,"script")},getJSON:function(e,t,n){return v.get(e,t,n,"json")},ajaxSetup:function(e,t){return t?Ln(e,v.ajaxSettings):(t=e,e=v.ajaxSettings),Ln(e,t),e},ajaxSettings:{url:cn,isLocal:dn.test(ln[1]),global:!0,type:"GET",contentType:"application/x-www-form-urlencoded; charset=UTF-8",processData:!0,async:!0,accepts:{xml:"application/xml, text/xml",html:"text/html",text:"text/plain",json:"application/json, text/javascript","*":Tn},contents:{xml:/xml/,html:/html/,json:/json/},responseFields:{xml:"responseXML",text:"responseText"},converters:{"* text":e.String,"text html":!0,"text json":v.parseJSON,"text xml":v.parseXML},flatOptions:{context:!0,url:!0}},ajaxPrefilter:Cn(Sn),ajaxTransport:Cn(xn),ajax:function(e,n){function T(e,n,s,a){var l,y,b,w,S,T=n;if(E===2)return;E=2,u&&clearTimeout(u),o=t,i=a||"",x.readyState=e>0?4:0,s&&(w=An(c,x,s));if(e>=200&&e<300||e===304)c.ifModified&&(S=x.getResponseHeader("Last-Modified"),S&&(v.lastModified[r]=S),S=x.getResponseHeader("Etag"),S&&(v.etag[r]=S)),e===304?(T="notmodified",l=!0):(l=On(c,w),T=l.state,y=l.data,b=l.error,l=!b);else{b=T;if(!T||e)T="error",e<0&&(e=0)}x.status=e,x.statusText=(n||T)+"",l?d.resolveWith(h,[y,T,x]):d.rejectWith(h,[x,T,b]),x.statusCode(g),g=t,f&&p.trigger("ajax"+(l?"Success":"Error"),[x,c,l?y:b]),m.fireWith(h,[x,T]),f&&(p.trigger("ajaxComplete",[x,c]),--v.active||v.event.trigger("ajaxStop"))}typeof e=="object"&&(n=e,e=t),n=n||{};var r,i,s,o,u,a,f,l,c=v.ajaxSetup({},n),h=c.context||c,p=h!==c&&(h.nodeType||h instanceof v)?v(h):v.event,d=v.Deferred(),m=v.Callbacks("once memory"),g=c.statusCode||{},b={},w={},E=0,S="canceled",x={readyState:0,setRequestHeader:function(e,t){if(!E){var n=e.toLowerCase();e=w[n]=w[n]||e,b[e]=t}return this},getAllResponseHeaders:function(){return E===2?i:null},getResponseHeader:function(e){var n;if(E===2){if(!s){s={};while(n=pn.exec(i))s[n[1].toLowerCase()]=n[2]}n=s[e.toLowerCase()]}return n===t?null:n},overrideMimeType:function(e){return E||(c.mimeType=e),this},abort:function(e){return e=e||S,o&&o.abort(e),T(0,e),this}};d.promise(x),x.success=x.done,x.error=x.fail,x.complete=m.add,x.statusCode=function(e){if(e){var t;if(E<2)for(t in e)g[t]=[g[t],e[t]];else t=e[x.status],x.always(t)}return this},c.url=((e||c.url)+"").replace(hn,"").replace(mn,ln[1]+"//"),c.dataTypes=v.trim(c.dataType||"*").toLowerCase().split(y),c.crossDomain==null&&(a=wn.exec(c.url.toLowerCase()),c.crossDomain=!(!a||a[1]===ln[1]&&a[2]===ln[2]&&(a[3]||(a[1]==="http:"?80:443))==(ln[3]||(ln[1]==="http:"?80:443)))),c.data&&c.processData&&typeof c.data!="string"&&(c.data=v.param(c.data,c.traditional)),kn(Sn,c,n,x);if(E===2)return x;f=c.global,c.type=c.type.toUpperCase(),c.hasContent=!vn.test(c.type),f&&v.active++===0&&v.event.trigger("ajaxStart");if(!c.hasContent){c.data&&(c.url+=(gn.test(c.url)?"&":"?")+c.data,delete c.data),r=c.url;if(c.cache===!1){var N=v.now(),C=c.url.replace(bn,"$1_="+N);c.url=C+(C===c.url?(gn.test(c.url)?"&":"?")+"_="+N:"")}}(c.data&&c.hasContent&&c.contentType!==!1||n.contentType)&&x.setRequestHeader("Content-Type",c.contentType),c.ifModified&&(r=r||c.url,v.lastModified[r]&&x.setRequestHeader("If-Modified-Since",v.lastModified[r]),v.etag[r]&&x.setRequestHeader("If-None-Match",v.etag[r])),x.setRequestHeader("Accept",c.dataTypes[0]&&c.accepts[c.dataTypes[0]]?c.accepts[c.dataTypes[0]]+(c.dataTypes[0]!=="*"?", "+Tn+"; q=0.01":""):c.accepts["*"]);for(l in c.headers)x.setRequestHeader(l,c.headers[l]);if(!c.beforeSend||c.beforeSend.call(h,x,c)!==!1&&E!==2){S="abort";for(l in{success:1,error:1,complete:1})x[l](c[l]);o=kn(xn,c,n,x);if(!o)T(-1,"No Transport");else{x.readyState=1,f&&p.trigger("ajaxSend",[x,c]),c.async&&c.timeout>0&&(u=setTimeout(function(){x.abort("timeout")},c.timeout));try{E=1,o.send(b,T)}catch(k){if(!(E<2))throw k;T(-1,k)}}return x}return x.abort()},active:0,lastModified:{},etag:{}});var Mn=[],_n=/\?/,Dn=/(=)\?(?=&|$)|\?\?/,Pn=v.now();v.ajaxSetup({jsonp:"callback",jsonpCallback:function(){var e=Mn.pop()||v.expando+"_"+Pn++;return this[e]=!0,e}}),v.ajaxPrefilter("json jsonp",function(n,r,i){var s,o,u,a=n.data,f=n.url,l=n.jsonp!==!1,c=l&&Dn.test(f),h=l&&!c&&typeof a=="string"&&!(n.contentType||"").indexOf("application/x-www-form-urlencoded")&&Dn.test(a);if(n.dataTypes[0]==="jsonp"||c||h)return s=n.jsonpCallback=v.isFunction(n.jsonpCallback)?n.jsonpCallback():n.jsonpCallback,o=e[s],c?n.url=f.replace(Dn,"$1"+s):h?n.data=a.replace(Dn,"$1"+s):l&&(n.url+=(_n.test(f)?"&":"?")+n.jsonp+"="+s),n.converters["script json"]=function(){return u||v.error(s+" was not called"),u[0]},n.dataTypes[0]="json",e[s]=function(){u=arguments},i.always(function(){e[s]=o,n[s]&&(n.jsonpCallback=r.jsonpCallback,Mn.push(s)),u&&v.isFunction(o)&&o(u[0]),u=o=t}),"script"}),v.ajaxSetup({accepts:{script:"text/javascript, application/javascript, application/ecmascript, application/x-ecmascript"},contents:{script:/javascript|ecmascript/},converters:{"text script":function(e){return v.globalEval(e),e}}}),v.ajaxPrefilter("script",function(e){e.cache===t&&(e.cache=!1),e.crossDomain&&(e.type="GET",e.global=!1)}),v.ajaxTransport("script",function(e){if(e.crossDomain){var n,r=i.head||i.getElementsByTagName("head")[0]||i.documentElement;return{send:function(s,o){n=i.createElement("script"),n.async="async",e.scriptCharset&&(n.charset=e.scriptCharset),n.src=e.url,n.onload=n.onreadystatechange=function(e,i){if(i||!n.readyState||/loaded|complete/.test(n.readyState))n.onload=n.onreadystatechange=null,r&&n.parentNode&&r.removeChild(n),n=t,i||o(200,"success")},r.insertBefore(n,r.firstChild)},abort:function(){n&&n.onload(0,1)}}}});var Hn,Bn=e.ActiveXObject?function(){for(var e in Hn)Hn[e](0,1)}:!1,jn=0;v.ajaxSettings.xhr=e.ActiveXObject?function(){return!this.isLocal&&Fn()||In()}:Fn,function(e){v.extend(v.support,{ajax:!!e,cors:!!e&&"withCredentials"in e})}(v.ajaxSettings.xhr()),v.support.ajax&&v.ajaxTransport(function(n){if(!n.crossDomain||v.support.cors){var r;return{send:function(i,s){var o,u,a=n.xhr();n.username?a.open(n.type,n.url,n.async,n.username,n.password):a.open(n.type,n.url,n.async);if(n.xhrFields)for(u in n.xhrFields)a[u]=n.xhrFields[u];n.mimeType&&a.overrideMimeType&&a.overrideMimeType(n.mimeType),!n.crossDomain&&!i["X-Requested-With"]&&(i["X-Requested-With"]="XMLHttpRequest");try{for(u in i)a.setRequestHeader(u,i[u])}catch(f){}a.send(n.hasContent&&n.data||null),r=function(e,i){var u,f,l,c,h;try{if(r&&(i||a.readyState===4)){r=t,o&&(a.onreadystatechange=v.noop,Bn&&delete Hn[o]);if(i)a.readyState!==4&&a.abort();else{u=a.status,l=a.getAllResponseHeaders(),c={},h=a.responseXML,h&&h.documentElement&&(c.xml=h);try{c.text=a.responseText}catch(p){}try{f=a.statusText}catch(p){f=""}!u&&n.isLocal&&!n.crossDomain?u=c.text?200:404:u===1223&&(u=204)}}}catch(d){i||s(-1,d)}c&&s(u,f,c,l)},n.async?a.readyState===4?setTimeout(r,0):(o=++jn,Bn&&(Hn||(Hn={},v(e).unload(Bn)),Hn[o]=r),a.onreadystatechange=r):r()},abort:function(){r&&r(0,1)}}}});var qn,Rn,Un=/^(?:toggle|show|hide)$/,zn=new RegExp("^(?:([-+])=|)("+m+")([a-z%]*)$","i"),Wn=/queueHooks$/,Xn=[Gn],Vn={"*":[function(e,t){var n,r,i=this.createTween(e,t),s=zn.exec(t),o=i.cur(),u=+o||0,a=1,f=20;if(s){n=+s[2],r=s[3]||(v.cssNumber[e]?"":"px");if(r!=="px"&&u){u=v.css(i.elem,e,!0)||n||1;do a=a||".5",u/=a,v.style(i.elem,e,u+r);while(a!==(a=i.cur()/o)&&a!==1&&--f)}i.unit=r,i.start=u,i.end=s[1]?u+(s[1]+1)*n:n}return i}]};v.Animation=v.extend(Kn,{tweener:function(e,t){v.isFunction(e)?(t=e,e=["*"]):e=e.split(" ");var n,r=0,i=e.length;for(;r-1,f={},l={},c,h;a?(l=i.position(),c=l.top,h=l.left):(c=parseFloat(o)||0,h=parseFloat(u)||0),v.isFunction(t)&&(t=t.call(e,n,s)),t.top!=null&&(f.top=t.top-s.top+c),t.left!=null&&(f.left=t.left-s.left+h),"using"in t?t.using.call(e,f):i.css(f)}},v.fn.extend({position:function(){if(!this[0])return;var e=this[0],t=this.offsetParent(),n=this.offset(),r=er.test(t[0].nodeName)?{top:0,left:0}:t.offset();return n.top-=parseFloat(v.css(e,"marginTop"))||0,n.left-=parseFloat(v.css(e,"marginLeft"))||0,r.top+=parseFloat(v.css(t[0],"borderTopWidth"))||0,r.left+=parseFloat(v.css(t[0],"borderLeftWidth"))||0,{top:n.top-r.top,left:n.left-r.left}},offsetParent:function(){return this.map(function(){var e=this.offsetParent||i.body;while(e&&!er.test(e.nodeName)&&v.css(e,"position")==="static")e=e.offsetParent;return e||i.body})}}),v.each({scrollLeft:"pageXOffset",scrollTop:"pageYOffset"},function(e,n){var r=/Y/.test(n);v.fn[e]=function(i){return v.access(this,function(e,i,s){var o=tr(e);if(s===t)return o?n in o?o[n]:o.document.documentElement[i]:e[i];o?o.scrollTo(r?v(o).scrollLeft():s,r?s:v(o).scrollTop()):e[i]=s},e,i,arguments.length,null)}}),v.each({Height:"height",Width:"width"},function(e,n){v.each({padding:"inner"+e,content:n,"":"outer"+e},function(r,i){v.fn[i]=function(i,s){var o=arguments.length&&(r||typeof i!="boolean"),u=r||(i===!0||s===!0?"margin":"border");return v.access(this,function(n,r,i){var s;return v.isWindow(n)?n.document.documentElement["client"+e]:n.nodeType===9?(s=n.documentElement,Math.max(n.body["scroll"+e],s["scroll"+e],n.body["offset"+e],s["offset"+e],s["client"+e])):i===t?v.css(n,r,i,u):v.style(n,r,i,u)},n,o?i:t,o,null)}})}),e.jQuery=e.$=v,typeof define=="function"&&define.amd&&define.amd.ajQuery&&define("jquery",[],function(){return v})})(window); \ No newline at end of file diff --git a/qrlocal/qrcodeborder.js b/qrlocal/qrcodeborder.js new file mode 100644 index 00000000..4dcd0c27 --- /dev/null +++ b/qrlocal/qrcodeborder.js @@ -0,0 +1,624 @@ +/** + * @fileoverview + * - Using the 'QRCode for Javascript library' + * - Fixed dataset of 'QRCode for Javascript library' for support full-spec. + * - this library has no dependencies. + * + * @author davidshimjs + * @see http://www.d-project.com/ + * @see http://jeromeetienne.github.com/jquery-qrcode/ + */ +var QRCode; + +(function () { + //--------------------------------------------------------------------- + // QRCode for JavaScript + // + // Copyright (c) 2009 Kazuhiko Arase + // + // URL: http://www.d-project.com/ + // + // Licensed under the MIT license: + // http://www.opensource.org/licenses/mit-license.php + // + // The word "QR Code" is registered trademark of + // DENSO WAVE INCORPORATED + // http://www.denso-wave.com/qrcode/faqpatent-e.html + // + //--------------------------------------------------------------------- + function QR8bitByte(data) { + this.mode = QRMode.MODE_8BIT_BYTE; + this.data = data; + this.parsedData = []; + + // Added to support UTF-8 Characters + for (var i = 0, l = this.data.length; i < l; i++) { + var byteArray = []; + var code = this.data.charCodeAt(i); + + if (code > 0x10000) { + byteArray[0] = 0xF0 | ((code & 0x1C0000) >>> 18); + byteArray[1] = 0x80 | ((code & 0x3F000) >>> 12); + byteArray[2] = 0x80 | ((code & 0xFC0) >>> 6); + byteArray[3] = 0x80 | (code & 0x3F); + } else if (code > 0x800) { + byteArray[0] = 0xE0 | ((code & 0xF000) >>> 12); + byteArray[1] = 0x80 | ((code & 0xFC0) >>> 6); + byteArray[2] = 0x80 | (code & 0x3F); + } else if (code > 0x80) { + byteArray[0] = 0xC0 | ((code & 0x7C0) >>> 6); + byteArray[1] = 0x80 | (code & 0x3F); + } else { + byteArray[0] = code; + } + + this.parsedData.push(byteArray); + } + + this.parsedData = Array.prototype.concat.apply([], this.parsedData); + + if (this.parsedData.length != this.data.length) { + this.parsedData.unshift(191); + this.parsedData.unshift(187); + this.parsedData.unshift(239); + } + } + + QR8bitByte.prototype = { + getLength: function (buffer) { + return this.parsedData.length; + }, + write: function (buffer) { + for (var i = 0, l = this.parsedData.length; i < l; i++) { + buffer.put(this.parsedData[i], 8); + } + } + }; + + function QRCodeModel(typeNumber, errorCorrectLevel) { + this.typeNumber = typeNumber; + this.errorCorrectLevel = errorCorrectLevel; + this.modules = null; + this.moduleCount = 0; + this.dataCache = null; + this.dataList = []; + } + + QRCodeModel.prototype={addData:function(data){var newData=new QR8bitByte(data);this.dataList.push(newData);this.dataCache=null;},isDark:function(row,col){if(row<0||this.moduleCount<=row||col<0||this.moduleCount<=col){throw new Error(row+","+col);} + return this.modules[row][col];},getModuleCount:function(){return this.moduleCount;},make:function(){this.makeImpl(false,this.getBestMaskPattern());},makeImpl:function(test,maskPattern){this.moduleCount=this.typeNumber*4+17;this.modules=new Array(this.moduleCount);for(var row=0;row=7){this.setupTypeNumber(test);} + if(this.dataCache==null){this.dataCache=QRCodeModel.createData(this.typeNumber,this.errorCorrectLevel,this.dataList);} + this.mapData(this.dataCache,maskPattern);},setupPositionProbePattern:function(row,col){for(var r=-1;r<=7;r++){if(row+r<=-1||this.moduleCount<=row+r)continue;for(var c=-1;c<=7;c++){if(col+c<=-1||this.moduleCount<=col+c)continue;if((0<=r&&r<=6&&(c==0||c==6))||(0<=c&&c<=6&&(r==0||r==6))||(2<=r&&r<=4&&2<=c&&c<=4)){this.modules[row+r][col+c]=true;}else{this.modules[row+r][col+c]=false;}}}},getBestMaskPattern:function(){var minLostPoint=0;var pattern=0;for(var i=0;i<8;i++){this.makeImpl(true,i);var lostPoint=QRUtil.getLostPoint(this);if(i==0||minLostPoint>lostPoint){minLostPoint=lostPoint;pattern=i;}} + return pattern;},createMovieClip:function(target_mc,instance_name,depth){var qr_mc=target_mc.createEmptyMovieClip(instance_name,depth);var cs=1;this.make();for(var row=0;row>i)&1)==1);this.modules[Math.floor(i/3)][i%3+this.moduleCount-8-3]=mod;} + for(var i=0;i<18;i++){var mod=(!test&&((bits>>i)&1)==1);this.modules[i%3+this.moduleCount-8-3][Math.floor(i/3)]=mod;}},setupTypeInfo:function(test,maskPattern){var data=(this.errorCorrectLevel<<3)|maskPattern;var bits=QRUtil.getBCHTypeInfo(data);for(var i=0;i<15;i++){var mod=(!test&&((bits>>i)&1)==1);if(i<6){this.modules[i][8]=mod;}else if(i<8){this.modules[i+1][8]=mod;}else{this.modules[this.moduleCount-15+i][8]=mod;}} + for(var i=0;i<15;i++){var mod=(!test&&((bits>>i)&1)==1);if(i<8){this.modules[8][this.moduleCount-i-1]=mod;}else if(i<9){this.modules[8][15-i-1+1]=mod;}else{this.modules[8][15-i-1]=mod;}} + this.modules[this.moduleCount-8][8]=(!test);},mapData:function(data,maskPattern){var inc=-1;var row=this.moduleCount-1;var bitIndex=7;var byteIndex=0;for(var col=this.moduleCount-1;col>0;col-=2){if(col==6)col--;while(true){for(var c=0;c<2;c++){if(this.modules[row][col-c]==null){var dark=false;if(byteIndex>>bitIndex)&1)==1);} + var mask=QRUtil.getMask(maskPattern,row,col-c);if(mask){dark=!dark;} + this.modules[row][col-c]=dark;bitIndex--;if(bitIndex==-1){byteIndex++;bitIndex=7;}}} + row+=inc;if(row<0||this.moduleCount<=row){row-=inc;inc=-inc;break;}}}}};QRCodeModel.PAD0=0xEC;QRCodeModel.PAD1=0x11;QRCodeModel.createData=function(typeNumber,errorCorrectLevel,dataList){var rsBlocks=QRRSBlock.getRSBlocks(typeNumber,errorCorrectLevel);var buffer=new QRBitBuffer();for(var i=0;itotalDataCount*8){throw new Error("code length overflow. (" + +buffer.getLengthInBits() + +">" + +totalDataCount*8 + +")");} + if(buffer.getLengthInBits()+4<=totalDataCount*8){buffer.put(0,4);} + while(buffer.getLengthInBits()%8!=0){buffer.putBit(false);} + while(true){if(buffer.getLengthInBits()>=totalDataCount*8){break;} + buffer.put(QRCodeModel.PAD0,8);if(buffer.getLengthInBits()>=totalDataCount*8){break;} + buffer.put(QRCodeModel.PAD1,8);} + return QRCodeModel.createBytes(buffer,rsBlocks);};QRCodeModel.createBytes=function(buffer,rsBlocks){var offset=0;var maxDcCount=0;var maxEcCount=0;var dcdata=new Array(rsBlocks.length);var ecdata=new Array(rsBlocks.length);for(var r=0;r=0)?modPoly.get(modIndex):0;}} + var totalCodeCount=0;for(var i=0;i=0){d^=(QRUtil.G15<<(QRUtil.getBCHDigit(d)-QRUtil.getBCHDigit(QRUtil.G15)));} + return((data<<10)|d)^QRUtil.G15_MASK;},getBCHTypeNumber:function(data){var d=data<<12;while(QRUtil.getBCHDigit(d)-QRUtil.getBCHDigit(QRUtil.G18)>=0){d^=(QRUtil.G18<<(QRUtil.getBCHDigit(d)-QRUtil.getBCHDigit(QRUtil.G18)));} + return(data<<12)|d;},getBCHDigit:function(data){var digit=0;while(data!=0){digit++;data>>>=1;} + return digit;},getPatternPosition:function(typeNumber){return QRUtil.PATTERN_POSITION_TABLE[typeNumber-1];},getMask:function(maskPattern,i,j){switch(maskPattern){case QRMaskPattern.PATTERN000:return(i+j)%2==0;case QRMaskPattern.PATTERN001:return i%2==0;case QRMaskPattern.PATTERN010:return j%3==0;case QRMaskPattern.PATTERN011:return(i+j)%3==0;case QRMaskPattern.PATTERN100:return(Math.floor(i/2)+Math.floor(j/3))%2==0;case QRMaskPattern.PATTERN101:return(i*j)%2+(i*j)%3==0;case QRMaskPattern.PATTERN110:return((i*j)%2+(i*j)%3)%2==0;case QRMaskPattern.PATTERN111:return((i*j)%3+(i+j)%2)%2==0;default:throw new Error("bad maskPattern:"+maskPattern);}},getErrorCorrectPolynomial:function(errorCorrectLength){var a=new QRPolynomial([1],0);for(var i=0;i5){lostPoint+=(3+sameCount-5);}}} + for(var row=0;row=256){n-=255;} + return QRMath.EXP_TABLE[n];},EXP_TABLE:new Array(256),LOG_TABLE:new Array(256)};for(var i=0;i<8;i++){QRMath.EXP_TABLE[i]=1<>>(7-index%8))&1)==1;},put:function(num,length){for(var i=0;i>>(length-i-1))&1)==1);}},getLengthInBits:function(){return this.length;},putBit:function(bit){var bufIndex=Math.floor(this.length/8);if(this.buffer.length<=bufIndex){this.buffer.push(0);} + if(bit){this.buffer[bufIndex]|=(0x80>>>(this.length%8));} + this.length++;}};var QRCodeLimitLength=[[17,14,11,7],[32,26,20,14],[53,42,32,24],[78,62,46,34],[106,84,60,44],[134,106,74,58],[154,122,86,64],[192,152,108,84],[230,180,130,98],[271,213,151,119],[321,251,177,137],[367,287,203,155],[425,331,241,177],[458,362,258,194],[520,412,292,220],[586,450,322,250],[644,504,364,280],[718,560,394,310],[792,624,442,338],[858,666,482,382],[929,711,509,403],[1003,779,565,439],[1091,857,611,461],[1171,911,661,511],[1273,997,715,535],[1367,1059,751,593],[1465,1125,805,625],[1528,1190,868,658],[1628,1264,908,698],[1732,1370,982,742],[1840,1452,1030,790],[1952,1538,1112,842],[2068,1628,1168,898],[2188,1722,1228,958],[2303,1809,1283,983],[2431,1911,1351,1051],[2563,1989,1423,1093],[2699,2099,1499,1139],[2809,2213,1579,1219],[2953,2331,1663,1273]]; + + function _isSupportCanvas() { + return typeof CanvasRenderingContext2D != "undefined"; + } + + // android 2.x doesn't support Data-URI spec + function _getAndroid() { + var android = false; + var sAgent = navigator.userAgent; + + if (/android/i.test(sAgent)) { // android + android = true; + var aMat = sAgent.toString().match(/android ([0-9]\.[0-9])/i); + + if (aMat && aMat[1]) { + android = parseFloat(aMat[1]); + } + } + + return android; + } + + var svgDrawer = (function() { + + var Drawing = function (el, htOption) { + this._el = el; + this._htOption = htOption; + }; + + Drawing.prototype.draw = function (oQRCode) { + var _htOption = this._htOption; + var _el = this._el; + var nCount = oQRCode.getModuleCount()+4; + var nWidth = Math.floor(_htOption.width / nCount); + var nHeight = Math.floor(_htOption.height / nCount); + + this.clear(); + + function makeSVG(tag, attrs) { + var el = document.createElementNS('http://www.w3.org/2000/svg', tag); + for (var k in attrs) + if (attrs.hasOwnProperty(k)) el.setAttribute(k, attrs[k]); + return el; + } + + var svg = makeSVG("svg" , {'viewBox': '0 0 ' + String(nCount) + " " + String(nCount), 'width': '100%', 'height': '100%', 'fill': _htOption.colorLight}); + svg.setAttributeNS("http://www.w3.org/2000/xmlns/", "xmlns:xlink", "http://www.w3.org/1999/xlink"); + _el.appendChild(svg); + + svg.appendChild(makeSVG("rect", {"fill": _htOption.colorLight, "width": "100%", "height": "100%"})); + svg.appendChild(makeSVG("rect", {"fill": _htOption.colorDark, "width": "1", "height": "1", "id": "template"})); + + for (var row = 0; row < nCount; row++) { + for (var col = 0; col < nCount; col++) { + if(row >= 2 && row < nCount-2 && col >= 2 && col < nCount-2) { + if (oQRCode.isDark(row-2, col-2)) { + var child = makeSVG("use", {"x": String(col), "y": String(row)}); + child.setAttributeNS("http://www.w3.org/1999/xlink", "href", "#template") + svg.appendChild(child); + } + } + } + } + }; + Drawing.prototype.clear = function () { + while (this._el.hasChildNodes()) + this._el.removeChild(this._el.lastChild); + }; + return Drawing; + })(); + + var useSVG = document.documentElement.tagName.toLowerCase() === "svg"; + + // Drawing in DOM by using Table tag + var Drawing = useSVG ? svgDrawer : !_isSupportCanvas() ? (function () { + var Drawing = function (el, htOption) { + this._el = el; + this._htOption = htOption; + }; + + /** + * Draw the QRCode + * + * @param {QRCode} oQRCode + */ + Drawing.prototype.draw = function (oQRCode) { + var _htOption = this._htOption; + var _el = this._el; + var nCount = oQRCode.getModuleCount()+4; + var nWidth = Math.floor(_htOption.width / nCount); + var nHeight = Math.floor(_htOption.height / nCount); + var aHTML = ['
']; + + for (var row = 0; row < nCount; row++) { + aHTML.push(''); + + for (var col = 0; col < nCount; col++) { + + if(row >= 2 && row < nCount-2 && col >= 2 && col < nCount-2) + aHTML.push(''); + else + aHTML.push(''); + } + + aHTML.push(''); + } + + aHTML.push('
'); + _el.innerHTML = aHTML.join(''); + + // Fix the margin values as real size. + var elTable = _el.childNodes[0]; + var nLeftMarginTable = (_htOption.width - elTable.offsetWidth) / 2; + var nTopMarginTable = (_htOption.height - elTable.offsetHeight) / 2; + + if (nLeftMarginTable > 0 && nTopMarginTable > 0) { + elTable.style.margin = nTopMarginTable + "px " + nLeftMarginTable + "px"; + } + }; + + /** + * Clear the QRCode + */ + Drawing.prototype.clear = function () { + this._el.innerHTML = ''; + }; + + return Drawing; + })() : (function () { // Drawing in Canvas + function _onMakeImage() { + this._elImage.src = this._elCanvas.toDataURL("image/png"); + this._elImage.style.display = "block"; + this._elCanvas.style.display = "none"; + } + + // Android 2.1 bug workaround + // http://code.google.com/p/android/issues/detail?id=5141 + if (this._android && this._android <= 2.1) { + var factor = 1 / window.devicePixelRatio; + var drawImage = CanvasRenderingContext2D.prototype.drawImage; + CanvasRenderingContext2D.prototype.drawImage = function (image, sx, sy, sw, sh, dx, dy, dw, dh) { + if (("nodeName" in image) && /img/i.test(image.nodeName)) { + for (var i = arguments.length - 1; i >= 1; i--) { + arguments[i] = arguments[i] * factor; + } + } else if (typeof dw == "undefined") { + arguments[1] *= factor; + arguments[2] *= factor; + arguments[3] *= factor; + arguments[4] *= factor; + } + + drawImage.apply(this, arguments); + }; + } + + /** + * Check whether the user's browser supports Data URI or not + * + * @private + * @param {Function} fSuccess Occurs if it supports Data URI + * @param {Function} fFail Occurs if it doesn't support Data URI + */ + function _safeSetDataURI(fSuccess, fFail) { + var self = this; + self._fFail = fFail; + self._fSuccess = fSuccess; + + // Check it just once + if (self._bSupportDataURI === null) { + var el = document.createElement("img"); + var fOnError = function() { + self._bSupportDataURI = false; + + if (self._fFail) { + self._fFail.call(self); + } + }; + var fOnSuccess = function() { + self._bSupportDataURI = true; + + if (self._fSuccess) { + self._fSuccess.call(self); + } + }; + + el.onabort = fOnError; + el.onerror = fOnError; + el.onload = fOnSuccess; + el.src = "data:image/gif;base64,iVBORw0KGgoAAAANSUhEUgAAAAUAAAAFCAYAAACNbyblAAAAHElEQVQI12P4//8/w38GIAXDIBKE0DHxgljNBAAO9TXL0Y4OHwAAAABJRU5ErkJggg=="; // the Image contains 1px data. + return; + } else if (self._bSupportDataURI === true && self._fSuccess) { + self._fSuccess.call(self); + } else if (self._bSupportDataURI === false && self._fFail) { + self._fFail.call(self); + } + }; + + /** + * Drawing QRCode by using canvas + * + * @constructor + * @param {HTMLElement} el + * @param {Object} htOption QRCode Options + */ + var Drawing = function (el, htOption) { + this._bIsPainted = false; + this._android = _getAndroid(); + + this._htOption = htOption; + this._elCanvas = document.createElement("canvas"); + this._elCanvas.width = htOption.width; + this._elCanvas.height = htOption.height; + el.appendChild(this._elCanvas); + this._el = el; + this._oContext = this._elCanvas.getContext("2d"); + this._bIsPainted = false; + this._elImage = document.createElement("img"); + this._elImage.alt = "Scan me!"; + this._elImage.style.display = "none"; + this._el.appendChild(this._elImage); + this._bSupportDataURI = null; + }; + + /** + * Draw the QRCode + * + * @param {QRCode} oQRCode + */ + Drawing.prototype.draw = function (oQRCode) { + var _elImage = this._elImage; + var _oContext = this._oContext; + var _htOption = this._htOption; + + var nCount = oQRCode.getModuleCount()+4; + var nWidth = _htOption.width / nCount; + var nHeight = _htOption.height / nCount; + var nRoundedWidth = Math.round(nWidth); + var nRoundedHeight = Math.round(nHeight); + + _elImage.style.display = "none"; + this.clear(); + + for (var row = 0; row < nCount; row++) { + for (var col = 0; col < nCount; col++) { + + var bIsDark = 0; + + if(row >= 2 && row < nCount-2 && col >= 2 && col < nCount-2) + bIsDark = oQRCode.isDark(row-2, col-2); + var nLeft = col * nWidth; + var nTop = row * nHeight; + _oContext.strokeStyle = bIsDark ? _htOption.colorDark : _htOption.colorLight; + _oContext.lineWidth = 1; + _oContext.fillStyle = bIsDark ? _htOption.colorDark : _htOption.colorLight; + _oContext.fillRect(nLeft, nTop, nWidth, nHeight); + + // 안티 앨리어싱 방지 처리 + _oContext.strokeRect( + Math.floor(nLeft) + 0.5, + Math.floor(nTop) + 0.5, + nRoundedWidth, + nRoundedHeight + ); + + _oContext.strokeRect( + Math.ceil(nLeft) - 0.5, + Math.ceil(nTop) - 0.5, + nRoundedWidth, + nRoundedHeight + ); + } + } + + this._bIsPainted = true; + }; + + /** + * Make the image from Canvas if the browser supports Data URI. + */ + Drawing.prototype.makeImage = function () { + if (this._bIsPainted) { + _safeSetDataURI.call(this, _onMakeImage); + } + }; + + /** + * Return whether the QRCode is painted or not + * + * @return {Boolean} + */ + Drawing.prototype.isPainted = function () { + return this._bIsPainted; + }; + + /** + * Clear the QRCode + */ + Drawing.prototype.clear = function () { + this._oContext.clearRect(0, 0, this._elCanvas.width, this._elCanvas.height); + this._bIsPainted = false; + }; + + /** + * @private + * @param {Number} nNumber + */ + Drawing.prototype.round = function (nNumber) { + if (!nNumber) { + return nNumber; + } + + return Math.floor(nNumber * 1000) / 1000; + }; + + return Drawing; + })(); + + /** + * Get the type by string length + * + * @private + * @param {String} sText + * @param {Number} nCorrectLevel + * @return {Number} type + */ + function _getTypeNumber(sText, nCorrectLevel) { + var nType = 1; + var length = _getUTF8Length(sText); + + for (var i = 0, len = QRCodeLimitLength.length; i <= len; i++) { + var nLimit = 0; + + switch (nCorrectLevel) { + case QRErrorCorrectLevel.L : + nLimit = QRCodeLimitLength[i][0]; + break; + case QRErrorCorrectLevel.M : + nLimit = QRCodeLimitLength[i][1]; + break; + case QRErrorCorrectLevel.Q : + nLimit = QRCodeLimitLength[i][2]; + break; + case QRErrorCorrectLevel.H : + nLimit = QRCodeLimitLength[i][3]; + break; + } + + if (length <= nLimit) { + break; + } else { + nType++; + } + } + + if (nType > QRCodeLimitLength.length) { + throw new Error("Too long data"); + } + + return nType; + } + + function _getUTF8Length(sText) { + var replacedText = encodeURI(sText).toString().replace(/\%[0-9a-fA-F]{2}/g, 'a'); + return replacedText.length + (replacedText.length != sText ? 3 : 0); + } + + /** + * @class QRCode + * @constructor + * @example + * new QRCode(document.getElementById("test"), "http://jindo.dev.naver.com/collie"); + * + * @example + * var oQRCode = new QRCode("test", { + * text : "http://naver.com", + * width : 128, + * height : 128 + * }); + * + * oQRCode.clear(); // Clear the QRCode. + * oQRCode.makeCode("http://map.naver.com"); // Re-create the QRCode. + * + * @param {HTMLElement|String} el target element or 'id' attribute of element. + * @param {Object|String} vOption + * @param {String} vOption.text QRCode link data + * @param {Number} [vOption.width=256] + * @param {Number} [vOption.height=256] + * @param {String} [vOption.colorDark="#000000"] + * @param {String} [vOption.colorLight="#ffffff"] + * @param {QRCode.CorrectLevel} [vOption.correctLevel=QRCode.CorrectLevel.H] [L|M|Q|H] + */ + QRCode = function (el, vOption) { + this._htOption = { + width : 256, + height : 256, + typeNumber : 4, + colorDark : "#000000", + colorLight : "#ffffff", + correctLevel : QRErrorCorrectLevel.H + }; + + if (typeof vOption === 'string') { + vOption = { + text : vOption + }; + } + + // Overwrites options + if (vOption) { + for (var i in vOption) { + this._htOption[i] = vOption[i]; + } + } + + if (typeof el == "string") { + el = document.getElementById(el); + } + + if (this._htOption.useSVG) { + Drawing = svgDrawer; + } + + this._android = _getAndroid(); + this._el = el; + this._oQRCode = null; + this._oDrawing = new Drawing(this._el, this._htOption); + + if (this._htOption.text) { + this.makeCode(this._htOption.text); + } + }; + + /** + * Make the QRCode + * + * @param {String} sText link data + */ + QRCode.prototype.makeCode = function (sText) { + this._oQRCode = new QRCodeModel(_getTypeNumber(sText, this._htOption.correctLevel), this._htOption.correctLevel); + this._oQRCode.addData(sText); + this._oQRCode.make(); + this._el.title = ""; + this._oDrawing.draw(this._oQRCode); + this.makeImage(); + }; + + /** + * Make the Image from Canvas element + * - It occurs automatically + * - Android below 3 doesn't support Data-URI spec. + * + * @private + */ + QRCode.prototype.makeImage = function () { + if (typeof this._oDrawing.makeImage == "function" && (!this._android || this._android >= 3)) { + this._oDrawing.makeImage(); + } + }; + + /** + * Clear the QRCode + */ + QRCode.prototype.clear = function () { + this._oDrawing.clear(); + }; + + /** + * @name QRCode.CorrectLevel + */ + QRCode.CorrectLevel = QRErrorCorrectLevel; +})(); diff --git a/qrlocal/style.css b/qrlocal/style.css new file mode 100644 index 00000000..74108247 --- /dev/null +++ b/qrlocal/style.css @@ -0,0 +1 @@ +/*! normalize.css v3.0.2 | MIT License | git.io/normalize */@import url("https://fonts.googleapis.com/css?family=Open+Sans:400,700");html{font-family:sans-serif;-ms-text-size-adjust:100%;-webkit-text-size-adjust:100%}body{margin:0}article,aside,details,figcaption,figure,footer,header,hgroup,main,menu,nav,section,summary{display:block}audio,canvas,progress,video{display:inline-block;vertical-align:baseline}audio:not([controls]){display:none;height:0}[hidden],template{display:none}a{background-color:transparent}a:active,a:hover{outline:0}abbr[title]{border-bottom:1px dotted}b,strong{font-weight:bold}dfn{font-style:italic}h1{font-size:2em;margin:0.67em 0}mark{background:#ff0;color:#000}small{font-size:80%}sub,sup{font-size:75%;line-height:0;position:relative;vertical-align:baseline}sup{top:-0.5em}sub{bottom:-0.25em}img{border:0}svg:not(:root){overflow:hidden}figure{margin:1em 40px}hr{box-sizing:content-box;height:0}pre{overflow:auto}code,kbd,pre,samp{font-family:monospace, monospace;font-size:1em}button,input,optgroup,select,textarea{color:inherit;font:inherit;margin:0}button{overflow:visible}button,select{text-transform:none}button,html input[type="button"],input[type="reset"],input[type="submit"]{-webkit-appearance:button;cursor:pointer}button[disabled],html input[disabled]{cursor:default}button::-moz-focus-inner,input::-moz-focus-inner{border:0;padding:0}input{line-height:normal}input[type="checkbox"],input[type="radio"]{box-sizing:border-box;padding:0}input[type="number"]::-webkit-inner-spin-button,input[type="number"]::-webkit-outer-spin-button{height:auto}input[type="search"]{-webkit-appearance:textfield;box-sizing:content-box}input[type="search"]::-webkit-search-cancel-button,input[type="search"]::-webkit-search-decoration{-webkit-appearance:none}fieldset{border:1px solid #c0c0c0;margin:0 2px;padding:0.35em 0.625em 0.75em}legend{border:0;padding:0}textarea{overflow:auto}optgroup{font-weight:bold}table{border-collapse:collapse;border-spacing:0}td,th{padding:0}.highlight table td{padding:5px}.highlight table pre{margin:0}.highlight .cm{color:#999988;font-style:italic}.highlight .cp{color:#999999;font-weight:bold}.highlight .c1{color:#999988;font-style:italic}.highlight .cs{color:#999999;font-weight:bold;font-style:italic}.highlight .c,.highlight .cd{color:#999988;font-style:italic}.highlight .err{color:#a61717;background-color:#e3d2d2}.highlight .gd{color:#000000;background-color:#ffdddd}.highlight .ge{color:#000000;font-style:italic}.highlight .gr{color:#aa0000}.highlight .gh{color:#999999}.highlight .gi{color:#000000;background-color:#ddffdd}.highlight .go{color:#888888}.highlight .gp{color:#555555}.highlight .gs{font-weight:bold}.highlight .gu{color:#aaaaaa}.highlight .gt{color:#aa0000}.highlight .kc{color:#000000;font-weight:bold}.highlight .kd{color:#000000;font-weight:bold}.highlight .kn{color:#000000;font-weight:bold}.highlight .kp{color:#000000;font-weight:bold}.highlight .kr{color:#000000;font-weight:bold}.highlight .kt{color:#445588;font-weight:bold}.highlight .k,.highlight .kv{color:#000000;font-weight:bold}.highlight .mf{color:#009999}.highlight .mh{color:#009999}.highlight .il{color:#009999}.highlight .mi{color:#009999}.highlight .mo{color:#009999}.highlight .m,.highlight .mb,.highlight .mx{color:#009999}.highlight .sb{color:#d14}.highlight .sc{color:#d14}.highlight .sd{color:#d14}.highlight .s2{color:#d14}.highlight .se{color:#d14}.highlight .sh{color:#d14}.highlight .si{color:#d14}.highlight .sx{color:#d14}.highlight .sr{color:#009926}.highlight .s1{color:#d14}.highlight .ss{color:#990073}.highlight .s{color:#d14}.highlight .na{color:#008080}.highlight .bp{color:#999999}.highlight .nb{color:#0086B3}.highlight .nc{color:#445588;font-weight:bold}.highlight .no{color:#008080}.highlight .nd{color:#3c5d5d;font-weight:bold}.highlight .ni{color:#800080}.highlight .ne{color:#990000;font-weight:bold}.highlight .nf{color:#990000;font-weight:bold}.highlight .nl{color:#990000;font-weight:bold}.highlight .nn{color:#555555}.highlight .nt{color:#000080}.highlight .vc{color:#008080}.highlight .vg{color:#008080}.highlight .vi{color:#008080}.highlight .nv{color:#008080}.highlight .ow{color:#000000;font-weight:bold}.highlight .o{color:#000000;font-weight:bold}.highlight .w{color:#bbbbbb}.highlight{background-color:#f8f8f8}*{box-sizing:border-box}body{padding:0;margin:0;font-family:"Open Sans", "Helvetica Neue", Helvetica, Arial, sans-serif;font-size:16px;line-height:1.5;color:#606c71}#skip-to-content{height:1px;width:1px;position:absolute;overflow:hidden;top:-10px}#skip-to-content:focus{position:fixed;top:10px;left:10px;height:auto;width:auto;background:#e19447;outline:thick solid #e19447}a{color:#1e6bb8;text-decoration:none}a:hover{text-decoration:underline}.btn{display:inline-block;margin-bottom:1rem;color:rgba(255,255,255,0.7);background-color:rgba(255,255,255,0.08);border-color:rgba(255,255,255,0.2);border-style:solid;border-width:1px;border-radius:0.3rem;transition:color 0.2s, background-color 0.2s, border-color 0.2s}.btn:hover{color:rgba(255,255,255,0.8);text-decoration:none;background-color:rgba(255,255,255,0.2);border-color:rgba(255,255,255,0.3)}.btn+.btn{margin-left:1rem}@media screen and (min-width: 64em){.btn{padding:0.75rem 1rem}}@media screen and (min-width: 42em) and (max-width: 64em){.btn{padding:0.6rem 0.9rem;font-size:0.9rem}}@media screen and (max-width: 42em){.btn{display:block;width:100%;padding:0.75rem;font-size:0.9rem}.btn+.btn{margin-top:1rem;margin-left:0}}.page-header{color:#fff;width:100%;background-color:#2d2d2d;background-image:linear-gradient(120deg, #2d2d2d, #2d2d2d)}@media screen and (min-width: 64em){.page-header{padding:2rem 3rem}}@media screen and (min-width: 42em) and (max-width: 64em){.page-header{padding:1.5rem 2rem}}@media screen and (max-width: 42em){.page-header{padding:0.5rem 1rem}}.page-header a{display:inline-block;padding-right:2.0rem}.page-header span{display:inline-block}.project-name{margin-top:0;margin-bottom:0.1rem}@media screen and (min-width: 64em){.project-name{font-size:1.75rem}}@media screen and (min-width: 42em) and (max-width: 64em){.project-name{font-size:1.25rem}}@media screen and (max-width: 42em){.project-name{font-size:1.0rem}}.project-tagline{margin-bottom:1rem;font-weight:normal;opacity:0.7}@media screen and (min-width: 64em){.project-tagline{font-size:1.00rem}}@media screen and (min-width: 42em) and (max-width: 64em){.project-tagline{font-size:1.00rem}}@media screen and (max-width: 42em){.project-tagline{font-size:0.75rem}}.main-content{word-wrap:break-word}.main-content :first-child{margin-top:0}@media screen and (min-width: 64em){.main-content{max-width:64rem;padding:2rem 6rem;margin:0 auto;font-size:1.1rem}}@media screen and (min-width: 42em) and (max-width: 64em){.main-content{padding:2rem 4rem;font-size:1.1rem}}@media screen and (max-width: 42em){.main-content{padding:2rem 1rem;font-size:1rem}}.main-content kbd{background-color:#fafbfc;border:1px solid #c6cbd1;border-bottom-color:#959da5;border-radius:3px;box-shadow:inset 0 -1px 0 #959da5;color:#444d56;display:inline-block;font-size:11px;line-height:10px;padding:3px 5px;vertical-align:middle}.main-content img{max-width:100%}.main-content h1,.main-content h2,.main-content h3,.main-content h4,.main-content h5,.main-content h6{margin-top:2rem;margin-bottom:1rem;font-weight:normal;color:#03529f}.main-content p{margin-bottom:1em}.main-content code{padding:2px 4px;font-family:Consolas, "Liberation Mono", Menlo, Courier, monospace;font-size:0.9rem;color:#567482;background-color:#f3f6fa;border-radius:0.3rem}.main-content pre{padding:0.8rem;margin-top:0;margin-bottom:1rem;font:1rem Consolas, "Liberation Mono", Menlo, Courier, monospace;color:#567482;word-wrap:normal;background-color:#f3f6fa;border:solid 1px #dce6f0;border-radius:0.3rem}.main-content pre>code{padding:0;margin:0;font-size:0.9rem;color:#567482;word-break:normal;white-space:pre;background:transparent;border:0}.main-content .highlight{margin-bottom:1rem}.main-content .highlight pre{margin-bottom:0;word-break:normal}.main-content .highlight pre,.main-content pre{padding:0.8rem;overflow:auto;font-size:0.9rem;line-height:1.45;border-radius:0.3rem;-webkit-overflow-scrolling:touch}.main-content pre code,.main-content pre tt{display:inline;max-width:initial;padding:0;margin:0;overflow:initial;line-height:inherit;word-wrap:normal;background-color:transparent;border:0}.main-content pre code:before,.main-content pre code:after,.main-content pre tt:before,.main-content pre tt:after{content:normal}.main-content ul,.main-content ol{margin-top:0}.main-content blockquote{padding:0 1rem;margin-left:0;color:#819198;border-left:0.3rem solid #dce6f0}.main-content blockquote>:first-child{margin-top:0}.main-content blockquote>:last-child{margin-bottom:0}.main-content table{display:block;width:100%;overflow:auto;word-break:normal;word-break:keep-all;-webkit-overflow-scrolling:touch}.main-content table th{font-weight:bold}.main-content table th,.main-content table td{padding:0.5rem 1rem;border:1px solid #e9ebec}.main-content dl{padding:0}.main-content dl dt{padding:0;margin-top:1rem;font-size:1rem;font-weight:bold}.main-content dl dd{padding:0;margin-bottom:1rem}.main-content hr{height:2px;padding:0;margin:1rem 0;background-color:#eff0f1;border:0}.site-footer{padding-top:2rem;margin-top:2rem;border-top:solid 1px #eff0f1}@media screen and (min-width: 64em){.site-footer{font-size:1rem}}@media screen and (min-width: 42em) and (max-width: 64em){.site-footer{font-size:1rem}}@media screen and (max-width: 42em){.site-footer{font-size:0.9rem}}.site-footer-owner{display:block;font-weight:bold}.site-footer-credits{color:#819198}