diff --git a/bindings/python/IK/src/InverseKinematics.cpp b/bindings/python/IK/src/InverseKinematics.cpp index 8bf3051..cf652d2 100644 --- a/bindings/python/IK/src/InverseKinematics.cpp +++ b/bindings/python/IK/src/InverseKinematics.cpp @@ -31,7 +31,9 @@ void CreateInverseKinematics(pybind11::module& module) py::class_(module, "nodeData") .def(py::init<>()) .def_readwrite("I_R_IMU", &nodeData::I_R_IMU) - .def_readwrite("I_omega_IMU", &nodeData::I_omega_IMU); + .def_readwrite("I_omega_IMU", &nodeData::I_omega_IMU) + .def_readwrite("I_position", &nodeData::I_position) + .def_readwrite("I_linearVelocity", &nodeData::I_linearVelocity); py::class_(module, "HumanIK") .def(py::init()) @@ -54,14 +56,16 @@ void CreateInverseKinematics(pybind11::module& module) .def("setDt", &HumanIK::setDt, py::arg("dt")) .def("getDt", &HumanIK::getDt) .def("getDoFsNumber", &HumanIK::getDoFsNumber) + .def("updatePoseTask", &HumanIK::updatePoseTask, py::arg("node"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU")) .def("updateOrientationTask", &HumanIK::updateOrientationTask, py::arg("node"), py::arg("I_R_IMU"), py::arg("I_omega_IMU")) .def("updateGravityTask", &HumanIK::updateGravityTask, py::arg("node"), py::arg("I_R_IMU")) - .def("updateFloorContactTask", &HumanIK::updateFloorContactTask, py::arg("node"), py::arg("verticalForce"), py::arg("linkHeight")) + .def("updateFloorContactTask", &HumanIK::updateFloorContactTask, py::arg("node"), py::arg("verticalForce"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"), py::arg("linkHeight")) .def("clearCalibrationMatrices", &HumanIK::clearCalibrationMatrices) .def("calibrateWorldYaw", &HumanIK::calibrateWorldYaw, py::arg("nodeStruct")) .def("calibrateAllWithWorld", &HumanIK::calibrateAllWithWorld, py::arg("nodeStruct"), py::arg("frameName")) + .def("updatePoseTasks", &HumanIK::updatePoseTasks, py::arg("nodeStruct")) .def("updateOrientationGravityTasks", &HumanIK::updateOrientationAndGravityTasks, py::arg("nodeStruct")) - .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("linkHeight")) + .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"), py::arg("linkHeight")) .def("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask) .def("updateJointConstraintsTask", &HumanIK::updateJointConstraintsTask) .def("advance", &HumanIK::advance) diff --git a/examples/test_IK.py b/examples/test_IK.py new file mode 100644 index 0000000..484025a --- /dev/null +++ b/examples/test_IK.py @@ -0,0 +1,125 @@ +import biomechanical_analysis_framework as baf +import bipedal_locomotion_framework.bindings as blf +import idyntree.swig as idyn +import icub_models +import numpy as np +import resolve_robotics_uri_py +import pathlib +import h5py +import manifpy as manif + +def get_kindyn(): + + joints_list = ["neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", + "l_hip_pitch", "l_hip_roll", "l_hip_yaw","l_knee", "l_ankle_pitch", "l_ankle_roll", + "r_hip_pitch", "r_hip_roll", "r_hip_yaw","r_knee", "r_ankle_pitch", "r_ankle_roll"] + + model_loader = idyn.ModelLoader() + # assert model_loader.loadReducedModelFromFile(str(icub_models.get_model_file("ergoCubSN001")), joints_list) + + urdf_path = str(resolve_robotics_uri_py.resolve_robotics_uri("package://ergoCub/robots/ergoCubSN001/model.urdf")) + assert model_loader.loadReducedModelFromFile(str(urdf_path), joints_list) + + # create KinDynComputationsDescriptor + kindyn = idyn.KinDynComputations() + assert kindyn.loadRobotModel(model_loader.model()) + + return joints_list, kindyn + +def normalize_quat(quat): + norm = np.linalg.norm(quat) + return [quat[1] / norm, quat[2] / norm, quat[3] / norm, quat[0] / norm] + +print("test_IK") + +joints_list, kindyn = get_kindyn() + +# create KinDynComputationsDescriptor +assert kindyn.getNrOfDegreesOfFreedom() == len(joints_list) + +# Set the joint positions to random values +joint_values = [np.random.uniform(-0.5, 0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] +assert kindyn.setJointPos(joint_values) + +# Set the robot state +updated_world_T_base = np.array([[1., 0., 0., 0.],[0., 0., -1., 0.],[0., 1., 0., 0.],[0., 0., 0., 1.]]) +updated_s = [np.random.uniform(-0.5,0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] +updated_base_velocity = [np.random.uniform(-0.5,0.5) for _ in range(6)] +updated_s_dot = [np.random.uniform(-0.5,0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] +updated_world_gravity = [np.random.uniform(-0.5,0.5) for _ in range(3)] +assert kindyn.setRobotState(updated_world_T_base,updated_s,updated_base_velocity, + updated_s_dot,updated_world_gravity) + +humanIK = baf.ik.HumanIK() +# empty_handler = blf.parameters_handler.TomlParametersHandler() + +# Set the parameters from toml file +qp_ik_params = blf.parameters_handler.TomlParametersHandler() +toml = pathlib.Path("test_IK.toml").expanduser() +assert toml.is_file() +ok = qp_ik_params.set_from_file(str(toml)) +# qp_ik_params.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + +humanIK.initialize(qp_ik_params, kindyn) + +# Create the node struct (dict) +# Read in the data +mocap_filename = "/home/evelyd/pi_learning_for_collaborative_carrying/datasets/collaborative_payload_carrying/leader_backward/follower.mat" +mocap_data = h5py.File(mocap_filename, 'r') +mocap_data_cleaned = mocap_data['robot_logger_device'] + +# Cut the data at the start time +# Get the index of the timestamp closest to the start time +start_time = 12.59 +zeroed_timestamps = np.squeeze(mocap_data_cleaned['shoe1']['FT']['timestamps'][:] - mocap_data_cleaned['shoe1']['FT']['timestamps'][0]) +start_time_index = np.argmin(np.abs(zeroed_timestamps - start_time)) + +# Assign the data for the SO3 and Gravity tasks into a struct +# Create the list of nodes in order of the toml file +pose_nodes = [3] +orientation_nodes = [6, 7, 8, 5, 4, 11, 12, 9, 10] +floor_contact_nodes = [1, 2] + +node_struct = {} +for node in orientation_nodes + floor_contact_nodes: + # Define time series of rotations for this node + I_R_IMU = [manif.SO3(quaternion=normalize_quat([quat[1], quat[2], quat[3], quat[0]])) for quat in np.squeeze(mocap_data_cleaned['node' + str(node)]['orientation']['data'][start_time_index:])] + # Define time series of angular velocities for this node + I_omega_IMU = [manif.SO3Tangent(omega) for omega in np.squeeze(mocap_data_cleaned['node' + str(node)]['angVel']['data'][start_time_index:])] + # Assign these values to the node struct + # node_struct[node] = {'I_R_IMU': I_R_IMU, 'I_omega_IMU': I_omega_IMU} + nodeData = baf.ik.nodeData() + nodeData.I_R_IMU = I_R_IMU[0] + nodeData.I_omega_IMU = I_omega_IMU[0] + nodeData.I_position = [0., 0., 0.] + nodeData.I_linearVelocity = [0., 0., 0.] + node_struct[node] = nodeData + +pose_node_struct = {} +for node in pose_nodes: + # Define time series of rotations for this node + I_R_IMU = [manif.SO3(quaternion=normalize_quat([quat[1], quat[2], quat[3], quat[0]])) for quat in np.squeeze(mocap_data_cleaned['node' + str(node)]['orientation']['data'][start_time_index:])] + # Define time series of angular velocities for this node + I_omega_IMU = [manif.SO3Tangent(omega) for omega in np.squeeze(mocap_data_cleaned['node' + str(node)]['angVel']['data'][start_time_index:])] + # Assign these values to the node struct + # node_struct[node] = {'I_R_IMU': I_R_IMU, 'I_omega_IMU': I_omega_IMU} + nodeData = baf.ik.nodeData() + nodeData.I_R_IMU = I_R_IMU[0] + nodeData.I_omega_IMU = I_omega_IMU[0] + nodeData.I_position = [0., 0., 0.] + nodeData.I_linearVelocity = [0., 0., 0.] + pose_node_struct[node] = nodeData + +# Put robot in T pose +calib_joint_positions = np.array([0.]*len(joints_list)) +calib_joint_positions[joints_list.index("l_shoulder_roll")] = np.pi/2 +calib_joint_positions[joints_list.index("r_shoulder_roll")] = np.pi/2 + +humanIK.calibrateWorldYaw(node_struct) +humanIK.calibrateAllWithWorld(node_struct, "l_sole") + +humanIK.updatePoseTask(3, pose_node_struct[3].I_position, pose_node_struct[3].I_R_IMU, pose_node_struct[3].I_linearVelocity, pose_node_struct[3].I_omega_IMU) +humanIK.updatePoseTasks(pose_node_struct) \ No newline at end of file diff --git a/examples/test_IK.toml b/examples/test_IK.toml new file mode 100644 index 0000000..0f830ed --- /dev/null +++ b/examples/test_IK.toml @@ -0,0 +1,231 @@ +tasks = [ + "PELVIS_TASK", + "T8_TASK", + "RIGHT_UPPER_ARM_TASK", + "RIGHT_FORE_ARM_TASK", + "LEFT_UPPER_ARM_TASK", + "LEFT_FORE_ARM_TASK", + "RIGHT_UPPER_LEG_TASK", + "RIGHT_LOWER_LEG_TASK", + "LEFT_UPPER_LEG_TASK", + "LEFT_LOWER_LEG_TASK", + "RIGHT_TOE_TASK", + "LEFT_TOE_TASK", + "RIGHT_FOOT_TASK", + "LEFT_FOOT_TASK", + "JOINT_LIMITS_TASK", + "JOINT_REG_TASK", + "JOINT_VEL_LIMITS_TASK" +] + +[IK] +verbosity = false +robot_velocity_variable_name = "robot_velocity" + +[PELVIS_TASK] +type = "SE3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 #remove later +frame_name = "root_link" +kp_linear = 20.0 +kp_angular = 20.0 +node_number = 3 +weight = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] +rotation_matrix = [0.0, 1.0, 0.0, + 0.0, 0.0, -1.0, + -1.0, 0.0, 0.0] + +[T8_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "chest" +kp_angular = 20.0 +node_number = 6 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0] + +[RIGHT_UPPER_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_upper_arm" +kp_angular = 20.0 +node_number = 7 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[RIGHT_FORE_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_forearm" +kp_angular = 20.0 +node_number = 8 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[LEFT_UPPER_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_upper_arm" +kp_angular = 20.0 +node_number = 5 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + +[LEFT_FORE_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_forearm" +kp_angular = 20.0 +node_number = 4 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + +[RIGHT_UPPER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_upper_leg" +kp_angular = 20.0 +node_number = 11 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[RIGHT_LOWER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_lower_leg" +kp_angular = 20.0 +node_number = 12 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[LEFT_UPPER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_upper_leg" +kp_angular = 20.0 +node_number = 9 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + +[LEFT_LOWER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_lower_leg" +kp_angular = 20.0 +node_number = 10 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + + +[RIGHT_FOOT_TASK] +type = "GravityTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +target_frame_name = "r_sole" +kp = 20.0 +node_number = 2 +weight = [1.0, 1.0] +rotation_matrix = [0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0] + +[LEFT_FOOT_TASK] +type = "GravityTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +target_frame_name = "l_sole" +kp = 20.0 +node_number = 1 +weight = [1.0, 1.0] +rotation_matrix = [-1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0] + +[RIGHT_TOE_TASK] +type = "FloorContactTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_foot_front" +kp_linear = 60.0 +node_number = 2 +mask = [true, true, true] +weight = [1.0, 1.0, 1.0] +vertical_force_threshold = 60.0 + +[LEFT_TOE_TASK] +type = "FloorContactTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_foot_front" +kp_linear = 60.0 +node_number = 1 +mask = [true, true, true] +weight = [1.0, 1.0, 1.0] +vertical_force_threshold = 60.0 + +[JOINT_LIMITS_TASK] +type = "JointConstraintTask" +robot_velocity_variable_name = "robot_velocity" +priority = 0 +use_model_limits = false +sampling_time = 0.017 +k_limits = 1.0 +joints_list = ["l_hip_roll", "r_hip_roll", "l_hip_yaw", "r_hip_yaw"] +upper_bounds = [1.9, 1.9, 0.75, 0.75] +lower_bounds = [0.07, 0.07, -0.25, -0.25] + +[JOINT_REG_TASK] +type = "JointRegularizationTask" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +robot_velocity_variable_name = "robot_velocity" +weight = 0.000001 + + +[JOINT_VEL_LIMITS_TASK] +type = "JointVelocityLimitsTask" +robot_velocity_variable_name = "robot_velocity" +priority = 0 +upper_limit = 5.0 +lower_limit = -5.0 \ No newline at end of file diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index 31ad9a0..f67f31f 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -17,8 +17,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -36,6 +36,8 @@ struct nodeData { manif::SO3d I_R_IMU; manif::SO3Tangentd I_omega_IMU = manif::SO3d::Tangent::Zero(); + Eigen::Vector3d I_position = Eigen::Vector3d::Zero(); + Eigen::Vector3d I_linearVelocity = Eigen::Vector3d::Zero(); }; // clang-format off @@ -46,6 +48,15 @@ struct nodeData class HumanIK { private: + /** + * initialize the SE3 task + * @param taskName name of the task + * @param handler pointer to the parameters handler + * @return true if the SE3 task is initialized correctly + */ + bool initializePoseTask(const std::string& taskName, + const std::shared_ptr taskHandler); + /** * initialize the SO3 task * @param taskName name of the task @@ -65,10 +76,10 @@ class HumanIK const std::shared_ptr taskHandler); /** - * initialize the R3 task + * initialize the SE3 task * @param taskName name of the task * @param handler pointer to the parameters handler - * @return true if the R3 task is initialized correctly + * @return true if the SE3 task is initialized correctly */ bool initializeFloorContactTask(const std::string& taskName, const std::shared_ptr taskHandler); @@ -117,6 +128,7 @@ class HumanIK Eigen::VectorXd m_jointPositions; /** Position of the joints */ Eigen::VectorXd m_jointVelocities; /** Velocity of the joints */ Eigen::Matrix4d m_basePose; /** SO3 pose of the base */ + bool m_useMeasuredBasePose; /** Flag to show that the vive tracker is being used to provide base pose info */ Eigen::Matrix m_baseVelocity; /** Vector containing the linear and angular velocity of the base */ Eigen::Vector3d m_gravity; /** Gravity vector */ @@ -126,6 +138,21 @@ class HumanIK manif::SO3Tangentd I_omega_link; /** angular velocity of the link in the inertial frame */ Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */ + Eigen::Matrix4d m_initialBasePose; /** Initial SO3 pose of the base */ + + /** + * Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the + * rotation matrix between the IMU and the link + */ + struct PoseTaskStruct + { + std::shared_ptr task; + manif::SE3d IMU_H_link; // Transformation matrix from the IMU to related link + manif::SE3d IMU_H_link_init; // Initial value of the transformation matrix from the IMU to related link, set through config + // file + Eigen::VectorXd weight; // Weight of the task, length depends on mask + std::string frameName; // Name of the frame in which the task is expressed + }; /** * Struct containing the SO3 task from the BipedalLocomotion IK, the node number and the @@ -165,16 +192,18 @@ class HumanIK }; /** - * Struct containing the R3 task from the BipedalLocomotion IK, the node number and the + * Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the * multiple state weight provider */ struct FloorContactTaskStruct { - std::shared_ptr task; - Eigen::Vector3d weight; + std::shared_ptr task; + Eigen::VectorXd weight; int nodeNumber; bool footInContact{false}; - Eigen::Vector3d setPointPosition; + manif::SE3d IMU_H_link; // Transformation matrix from the IMU to related link + manif::SE3d IMU_H_link_init; // Initial value of the transformation matrix from the IMU to related link, set through config + // file std::string taskName; std::string frameName; double verticalForceThreshold; @@ -192,6 +221,11 @@ class HumanIK manif::SO3d calib_W_R_link = manif::SO3d::Identity(); /** calibration matrix between the world and the link */ + std::unordered_map m_PoseTasks; /** unordered map of type + PoseTaskStruct, each + element referring to a + node*/ + std::unordered_map m_OrientationTasks; /** unordered map of type OrientationTaskStruct, each element referring to a @@ -240,7 +274,21 @@ class HumanIK * For **each** task listed in the parameter `tasks` the user must specify all the parameters * required by the task itself but `robot_velocity_variable_name` since is already specified in * the `IK` group. Moreover, each task requires a parameter `type` that identifies the type of - * task. Up to now, only the "SO3Task" is implemented. + * task. + * + * * The "SE3Task" requires the following parameters: + * | Group | Parameter Name | Type | Description | Mandatory | + * |:---------:|:------------------------------:|:---------------:|:---------------------------------------------------------------------------------------:|:---------:| + * | `SE3Task` | `type` | `string` | Type of the task. The value to be set is `SE3Task` | Yes | + * | `SE3Task` | `robot_velocity_variable_name` | `string` |Name of the variable contained in `VariablesHandler` describing the generalized robot velocity| Yes | + * | `SE3Task` | `node_number` | `int` | Node number of the task. The node number must be unique. | Yes | + * | `SE3Task` | `rotation_matrix` | `vector`| Rotation matrix between the IMU and the link. By default it set to identity. | No | + * | `SE3Task` | `frame_name` | `string` | Name of the frame in which the task is expressed. | Yes | + * * | `SE3Task` | `kp_linear` | `double` | Value of the gain of the linear velocity feedback. | Yes | + * | `SE3Task` | `kp_angular` | `double` | Value of the gain of the angular velocity feedback. | Yes | + * | `SE3Task` | `weight` | `vector`| Weight of the task. Default value is (1.0, 1.0, 1.0) | yes | + * `SE3Task` is a placeholder for the name of the task contained in the `tasks` list. + * * The "SO3Task" requires the following parameters: * | Group | Parameter Name | Type | Description | Mandatory | * |:---------:|:------------------------------:|:---------------:|:---------------------------------------------------------------------------------------:|:---------:| @@ -296,23 +344,35 @@ class HumanIK * @note The following `ini` file presents an example of the configuration that can be used to * build the HumanIK class. * ~~~~~{.ini} - * tasks ("PELVIS_TASK", "GRAVITY_TASK", "RIGHT_TOE_TASK", "JOINT_LIMITS_TASK", "JOINT_REG_TASK") + * tasks ("PELVIS_TASK", "T8_TASK", "GRAVITY_TASK", "RIGHT_TOE_TASK", "JOINT_LIMITS_TASK", "JOINT_REG_TASK") * * [IK] * robot_velocity_variable_name "robot_velocity" * verbosity false * * [PELVIS_TASK] - * type "SO3Task" + * type "SE3Task" * robot_velocity_variable_name "robot_velocity" * frame_name "Pelvis" - * kp_angular 5.0 + * kp_linear 1.0 + * kp_angular 1.0 * node_number 3 * weight (1.0, 1.0, 1.0) * rotation_matrix (0.0, 1.0, 0.0, * 0.0, 0.0, -1.0, * -1.0, 0.0, 0.0) * + * [T8_TASK] + * type "SO3Task" + * robot_velocity_variable_name "robot_velocity" + * frame_name "T8" + * kp_angular 20.0 + * node_number 6 + * weight (1.0 1.0 1.0) + * rotation_matrix (0.0, 1.0, 0.0, + * 0.0, 0.0, 1.0, + * 1.0, 0.0, 0.0) + * * [GRAVITY_TASK] * type "GravityTask" * robot_velocity_variable_name "robot_velocity" @@ -376,6 +436,18 @@ class HumanIK * @return true if the orientation setpoint is set correctly */ bool + updatePoseTask(const std::string frameName, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero()); + + /** + * set the orientation and the angular velocity for a given node of a SO3 task + * @param node node number + * @param I_position position of the sensor in the world frame + * @param I_R_IMU orientation of the IMU + * @param I_linearVelocity linear velocity of the sensor in the world frame + * @param I_omega_IMU angular velocity of the IMU + * @return true if the orientation setpoint is set correctly + */ + bool updateOrientationTask(const int node, const manif::SO3d& I_R_IMU, const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero()); /** @@ -393,7 +465,7 @@ class HumanIK * @param verticalForce vertical force * @return true if the orientation setpoint is set correctly */ - bool updateFloorContactTask(const int node, const double verticalForce, const double linkHeight = 0.0); + bool updateFloorContactTask(const int node, const double verticalForce, const Eigen::Vector3d& I_position = Eigen::Vector3d::Zero(), const manif::SO3d& I_R_IMU = manif::SO3d::Identity(), const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero(), const double linkHeight = 0.0); /** * set the setpoint for the joint regularization task. @@ -411,6 +483,14 @@ class HumanIK */ bool updateJointConstraintsTask(); + /** + * update the pose for all the nodes of the SE3 tasks + * @param nodeStruct unordered map containing the struct node data + * containing the position, orientation, linear and angular velocity of an IMU, associated to the node number + * @return true if the calibration matrix is set correctly + */ + bool updatePoseTasks(const std::unordered_map& nodeStruct); + /** * update the orientation for all the nodes of the SO3 and gravity tasks * @param nodeStruct unordered map containing the struct node data (see @@ -425,7 +505,7 @@ class HumanIK * @param footInContact unordered map containing the node number and the vertical force * @return true if the calibration matrix is set correctly */ - bool updateFloorContactTasks(const std::unordered_map>& wrenchMap, const double linkHeight = 0.0); + bool updateFloorContactTasks(const std::unordered_map>& wrenchMap, const Eigen::Vector3d& I_position = Eigen::Vector3d::Zero(), const manif::SO3d& I_R_IMU = manif::SO3d::Identity(), const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero(), const double linkHeight = 0.0); /** * clear the calibration matrices W_R_WIMU and IMU_R_link of all the orientation and gravity tasks diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index 41cb108..76d7669 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -46,6 +46,9 @@ bool HumanIK::initialize(std::weak_ptr(); m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3, 3>()), m_jointPositions}); @@ -70,6 +73,15 @@ bool HumanIK::initialize(std::weak_ptrgetParameter("use_measured_base_pose", useMeasuredBasePose)) + { + BiomechanicalAnalysis::log()->error("{} Parameter use_measured_base_pose is missing", logPrefix); + return false; + } + m_useMeasuredBasePose = useMeasuredBasePose; + bool ok = m_qpIK.initialize(ptr->getGroup("IK")); auto group = ptr->getGroup("IK").lock(); std::string variable; @@ -116,8 +128,18 @@ bool HumanIK::initialize(std::weak_ptrerror("{} Parameter task_type of the {} task is missing", logPrefix, task); return false; } - // Initialize SO3 task - if (taskType == "SO3Task") + + // Initialize SE3 task + if (taskType == "SE3Task") + { + if (!initializePoseTask(task, taskHandler)) + { + BiomechanicalAnalysis::log()->error("{} Error in the initialization of the {} task", logPrefix, task); + return false; + } + // Initialize SO3Task + } + else if (taskType == "SO3Task") { if (!initializeOrientationTask(task, taskHandler)) { @@ -196,6 +218,40 @@ int HumanIK::getDoFsNumber() const return m_nrDoFs; } +bool HumanIK::updatePoseTask(const std::string frameName, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU) +{ + + // Check if the frame name is valid + if (m_PoseTasks.find(frameName) == m_PoseTasks.end()) + { + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTask] Invalid frame name {}.", frameName); + return false; + } + + // Compute the rotation matrix from the world to the link frame as: + // W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link + manif::SE3d I_H_IMU(I_position, I_R_IMU); + + // Create the transformation matrix from the world to the link frame + manif::SE3d I_H_link = I_H_IMU * m_PoseTasks[frameName].IMU_H_link; + + // Compute the linear velocity in the link frame (left trivialized) + // I_linearVelocity_link = link_R_W * W_R_WIMU * WIMU_linearVelocity + Eigen::Vector3d I_linearVelocity_link = I_H_link.rotation().transpose() * I_linearVelocity; + + // Compute the angular velocity of the link frame (right trivialized) + Eigen::Vector3d I_omega_link = I_omega_IMU.coeffs(); + + Eigen::VectorXd mixedVelocityVector(6); + mixedVelocityVector << I_linearVelocity_link, I_omega_link; + + // Create mixed velocity vector + manif::SE3d::Tangent mixedVelocity(mixedVelocityVector); + + // Set the setpoint for the pose task of the node + return m_PoseTasks[frameName].task->setSetPoint(I_H_link, mixedVelocity); +} + bool HumanIK::updateOrientationTask(const int node, const manif::SO3d& I_R_IMU, const manif::SO3Tangentd& I_omega_IMU) { // Check if the node number is valid @@ -232,7 +288,7 @@ bool HumanIK::updateGravityTask(const int node, const manif::SO3d& I_R_IMU) return m_GravityTasks[node].task->setSetPoint((I_R_link.rotation().transpose().rightCols(1))); } -bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, const double linkHeight) +bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU, const double linkHeight) { bool ok{true}; // check if the node number is valid @@ -247,38 +303,39 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, Eigen::VectorXd jointPositions; jointPositions.resize(this->getDoFsNumber()); jointPositions.setZero(); - Eigen::Matrix4d basePose; - basePose.setIdentity(); Eigen::VectorXd baseVelocity; baseVelocity.resize(6); baseVelocity.setZero(); - m_kinDyn->setRobotState(basePose, jointPositions, baseVelocity, m_jointVelocities, m_gravity); + m_kinDyn->setRobotState(m_basePose, jointPositions, baseVelocity, m_jointVelocities, m_gravity); } - // if the vertical force is greater than the threshold and if the foot is not yet in contact, - // set the weight of the associated task to the weight of the task and set the set point of the - // task to the position of the frame computed with the legged odometry - if (verticalForce > m_FloorContactTasks[node].verticalForceThreshold && !m_FloorContactTasks[node].footInContact) - { - m_qpIK.setTaskWeight(m_FloorContactTasks[node].taskName, m_FloorContactTasks[node].weight); - m_FloorContactTasks[node].footInContact = true; - m_FloorContactTasks[node].setPointPosition - = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_FloorContactTasks[node].frameName).getPosition()); - m_FloorContactTasks[node].setPointPosition(2) = linkHeight; - } else if (verticalForce < m_FloorContactTasks[node].verticalForceThreshold && m_FloorContactTasks[node].footInContact) - { - // if the foot is not more in contact, set the weight of the associated task to zero - m_qpIK.setTaskWeight(m_FloorContactTasks[node].taskName, Eigen::Vector3d::Zero()); - m_FloorContactTasks[node].footInContact = false; - } + // Compute the rotation matrix from the world to the link frame as: + // W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link + manif::SE3d I_H_IMU(I_position, I_R_IMU); - // if the foot is in contact, set the set point of the task - if (m_FloorContactTasks[node].footInContact) - { - ok = m_FloorContactTasks[node].task->setSetPoint(m_FloorContactTasks[node].setPointPosition); + // Create the transformation matrix from the world to the link frame + manif::SE3d I_H_link = I_H_IMU * m_FloorContactTasks[node].IMU_H_link; + + // Compute the linear velocity in the link frame (left trivialized) + // I_linearVelocity_link = link_R_W * W_R_WIMU * WIMU_linearVelocity + Eigen::Vector3d I_linearVelocity_link = I_H_link.rotation().transpose() * I_linearVelocity; + + // Compute the angular velocity of the link frame (right trivialized) + Eigen::Vector3d I_omega_link = I_omega_IMU.coeffs(); + + Eigen::VectorXd mixedVelocityVector(6); + mixedVelocityVector << I_linearVelocity_link, I_omega_link; + + // Create mixed velocity vector + manif::SE3d::Tangent mixedVelocity(mixedVelocityVector); + + if (verticalForce > m_FloorContactTasks[node].verticalForceThreshold) + { // if the vertical force is greater than the threshold, set the foot height to 0 + I_H_link.translation(Eigen::Vector3d(I_H_link.translation().x(), I_H_link.translation().y(), linkHeight)); } - return ok; + // then set the set point for the task regardless of whether the foot is in contact + return m_FloorContactTasks[node].task->setSetPoint(I_H_link, mixedVelocity); } bool HumanIK::updateJointRegularizationTask() @@ -305,6 +362,33 @@ bool HumanIK::updateJointConstraintsTask() return m_jointConstraintsTask->update(); } +bool HumanIK::updatePoseTasks(const std::unordered_map& nodeStruct) +{ + // Update the pose tasks + for (const auto& [frameName, data] : nodeStruct) + { + if (m_PoseTasks.find(frameName) != m_PoseTasks.end()) + { + if (!updatePoseTask(frameName, data.I_position, data.I_R_IMU, data.I_linearVelocity, data.I_omega_IMU)) + { + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTasks] " + "Error in updating the pose task of " + "frame {}", + frameName); + return false; + } + } else + { + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTasks] " + "Invalid frame name {}.", + frameName); + return false; + } + } + + return true; +} + bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map& nodeStruct) { // Update the orientation and gravity tasks @@ -340,11 +424,11 @@ bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map>& wrenchMap, const double linkHeight) +bool HumanIK::updateFloorContactTasks(const std::unordered_map>& wrenchMap, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU, const double linkHeight) { for (const auto& [node, data] : wrenchMap) { - if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), linkHeight)) + if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), I_position, I_R_IMU, I_linearVelocity, I_omega_IMU, linkHeight)) { BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTasks] Error in updating " "the floor contact task of node {}", @@ -357,6 +441,10 @@ bool HumanIK::updateFloorContactTasks(const std::unordered_map nodeStruct) bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct, std::string refFrame) { + // Set the log prefix + constexpr auto logPrefix = "[HumanIK::calibrateAllWithWorld]"; + // reset the robot state Eigen::VectorXd jointVelocities; jointVelocities.resize(this->getDoFsNumber()); @@ -451,7 +546,14 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_OrientationTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_OrientationTasks[node].frameName).getRotation()); m_OrientationTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_OrientationTasks[node].calibrationMatrix = secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + if (m_useMeasuredBasePose) + { + m_OrientationTasks[node].calibrationMatrix = toManifRot(m_initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + } else + { + m_OrientationTasks[node].calibrationMatrix = secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + BiomechanicalAnalysis::log()->info("{} Assuming initial base pose is identity for OrientationTask.", logPrefix); + } } else { // compute the rotation matrix from the IMU to the link frame as: @@ -459,7 +561,14 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_GravityTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_GravityTasks[node].frameName).getRotation()); m_GravityTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_GravityTasks[node].calibrationMatrix = secondaryCalib * m_GravityTasks[node].calibrationMatrix; + if (m_useMeasuredBasePose) + { + m_GravityTasks[node].calibrationMatrix = toManifRot(m_initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_GravityTasks[node].calibrationMatrix; + } else + { + m_GravityTasks[node].calibrationMatrix = secondaryCalib * m_GravityTasks[node].calibrationMatrix; + BiomechanicalAnalysis::log()->info("{} Assuming initial base pose is identity for GravityTask.", logPrefix); + } } } // set the flag to true to reset the integration @@ -473,6 +582,9 @@ bool HumanIK::advance() // Initialize ok flag to true bool ok{true}; + // Log prefix for error messages + constexpr auto logPrefix = "[HumanIK::advance]"; + // Advance the QP solver ok = ok && m_qpIK.advance(); // Check if the output of the QP solver is valid @@ -503,10 +615,13 @@ bool HumanIK::advance() if (m_tPose) { - Eigen::Matrix4d basePose; // Pose of the base + if (!m_useMeasuredBasePose) + { + m_basePose.setIdentity(); + BiomechanicalAnalysis::log()->info("{} Resetting base pose to identity.", logPrefix); + } Eigen::VectorXd initialJointPositions; // Initial positions of the joints - basePose.setIdentity(); // Set the base pose to the identity matrix - m_system.dynamics->setState({basePose.topRightCorner<3, 1>(), toManifRot(basePose.topLeftCorner<3, 3>()), m_calibrationJointPositions}); + m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3, 3>()), m_calibrationJointPositions}); m_tPose = false; } @@ -587,6 +702,120 @@ bool HumanIK::getBaseAngularVelocity(Eigen::Ref baseAngularVelo return true; } +bool HumanIK::initializePoseTask(const std::string& taskName, + const std::shared_ptr taskHandler) +{ + // Log prefix for error messages + constexpr auto logPrefix = "[HumanIK::initializePoseTask]"; + + // Flag to indicate successful initialization + bool ok{true}; + + // Retrieve frame name parameter from config file, using the task handler + std::string frameName; + if (!taskHandler->getParameter("frame_name", frameName)) + { + BiomechanicalAnalysis::log()->error("{} Parameter frame_name of the {} task is missing", logPrefix, taskName); + return false; + } + + // Retrieve priority parameter from config file, using the task handler + int priority; + if (!taskHandler->getParameter("priority", priority)) + { + BiomechanicalAnalysis::log()->warn("{} Parameter priority of the {} task is missing, setting to 1 (soft priority)", logPrefix, taskName); + priority = 1; + } + + // Retrieve the mask parameter from config file, using the task handler + std::vector mask; + size_t numTrue; + if (!taskHandler->getParameter("mask", mask)) + { + numTrue = 6; + } + else + { + numTrue = std::count(mask.begin(), mask.end(), true) + 3; + } + + // Retrieve weight parameter from config file, using the task handler + std::vector weight; + if ((!taskHandler->getParameter("weight", weight)) && (priority != 0)) + { + BiomechanicalAnalysis::log()->error("{} Parameter weight of the {} task is missing", logPrefix, taskName); + return false; + + if (weight.size() != numTrue) + { + BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " + "{}, it should be {}", + logPrefix, + taskName, + weight.size(), + numTrue); + return false; + } + + } + + m_PoseTasks[frameName].weight.resize(weight.size()); + m_PoseTasks[frameName].weight = Eigen::Map(weight.data(), weight.size()); + + // Create an SE3Task object for the pose task + m_PoseTasks[frameName].task = std::make_shared(); + + //***************************************************************************************************** + // Retrieve Transformation matrix IMU-to-link from configuration file exampleIK.ini + //***************************************************************************************************** + + std::vector transformation_matrix; + if (taskHandler->getParameter("transformation_matrix", transformation_matrix)) + { + // Convert transformation matrix to ManifRot and assign it to IMU_R_link + Eigen::Matrix4d transformation_matrix_eigen = Eigen::Map>(transformation_matrix.data()); + m_PoseTasks[frameName].IMU_H_link_init = manif::SE3d(transformation_matrix_eigen.block<3, 1>(0, 3), + BipedalLocomotion::Conversions::toManifRot(transformation_matrix_eigen.block<3, 3>(0, 0))); + } else + { + // If transformation_matrix parameter is missing, set IMU_H_link to identity + std::string frame_name; + taskHandler->getParameter("frame_name", frame_name); + BiomechanicalAnalysis::log()->warn("{} Parameter transformation_matrix of the {} task is " + "missing, setting the transformation " + "matrix from the IMU to the frame {} to identity", + logPrefix, + taskName, + frame_name); + m_PoseTasks[frameName].IMU_H_link_init.setIdentity(); + } + m_PoseTasks[frameName].IMU_H_link = m_PoseTasks[frameName].IMU_H_link_init; + //***************************************************************************************************** + + // Initialize the SE3Task object + ok = ok && m_PoseTasks[frameName].task->setKinDyn(m_kinDyn); + ok = ok && m_PoseTasks[frameName].task->initialize(taskHandler); + + // Add the pose task to the QP solver + if (priority == 0) + { + ok = ok && m_qpIK.addTask(m_PoseTasks[frameName].task, taskName, priority); + } + else + { + ok = ok && m_qpIK.addTask(m_PoseTasks[frameName].task, taskName, priority, m_PoseTasks[frameName].weight); + } + + // Check if initialization was successful + if (!ok) + { + BiomechanicalAnalysis::log()->error("{} Error in the initialization of the {} task", logPrefix, taskName); + return false; + } + + return ok; +} + bool HumanIK::initializeOrientationTask(const std::string& taskName, const std::shared_ptr taskHandler) { @@ -805,24 +1034,73 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName, return false; } - // Retrieve weight parameter from the task handler + // Set node number and task name for the FloorContactTask + m_FloorContactTasks[nodeNumber].nodeNumber = nodeNumber; + m_FloorContactTasks[nodeNumber].taskName = taskName; + + // Retrieve the mask parameter from config file, using the task handler + std::vector mask; + size_t numTrue; + if (!taskHandler->getParameter("mask", mask)) + { + numTrue = 6; + } + else + { + numTrue = std::count(mask.begin(), mask.end(), true) + 3; + } + + // Retrieve weight parameter from config file, using the task handler std::vector weight; if (!taskHandler->getParameter("weight", weight)) { BiomechanicalAnalysis::log()->error("{} Parameter weight of the {} task is missing", logPrefix, taskName); return false; + + if (weight.size() != numTrue) + { + BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " + "{}, it should be {}", + logPrefix, + taskName, + weight.size(), + numTrue); + return false; + } + } - // Check that the weight is a 3D vector - if (weight.size() != 3) + m_FloorContactTasks[nodeNumber].weight.resize(weight.size()); + m_FloorContactTasks[nodeNumber].weight = Eigen::Map(weight.data(), weight.size()); + + // Create an SE3Task object for the pose task + m_FloorContactTasks[nodeNumber].task = std::make_shared(); + + //***************************************************************************************************** + // Retrieve Transformation matrix IMU-to-link from configuration file exampleIK.ini + //***************************************************************************************************** + + std::vector transformation_matrix; + if (taskHandler->getParameter("transformation_matrix", transformation_matrix)) { - BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " - "{}, it should be 3", - logPrefix, - taskName, - weight.size()); - return false; + // Convert transformation matrix to ManifRot and assign it to IMU_R_link + Eigen::Matrix4d transformation_matrix_eigen = Eigen::Map>(transformation_matrix.data()); + m_FloorContactTasks[nodeNumber].IMU_H_link_init = manif::SE3d(transformation_matrix_eigen.block<3, 1>(0, 3), + BipedalLocomotion::Conversions::toManifRot(transformation_matrix_eigen.block<3, 3>(0, 0))); + } else + { + // If transformation_matrix parameter is missing, set IMU_H_link to identity + std::string frame_name; + taskHandler->getParameter("frame_name", frame_name); + BiomechanicalAnalysis::log()->warn("{} Parameter transformation_matrix of the {} task is " + "missing, setting the transformation " + "matrix from the IMU to the frame {} to identity", + logPrefix, + taskName, + frame_name); + m_FloorContactTasks[nodeNumber].IMU_H_link_init.setIdentity(); } + m_FloorContactTasks[nodeNumber].IMU_H_link = m_FloorContactTasks[nodeNumber].IMU_H_link_init; // Retrieve vertical force threshold parameter from the task handler and assign it to the // corresponding FloorContactTask @@ -835,17 +1113,7 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName, return false; } - // Map weight vector to Eigen::Vector3d and assign it to the corresponding FloorContactTask - m_FloorContactTasks[nodeNumber].weight = Eigen::Map(weight.data()); - - // Set node number and task name for the FloorContactTask - m_FloorContactTasks[nodeNumber].nodeNumber = nodeNumber; - m_FloorContactTasks[nodeNumber].taskName = taskName; - - // Create an R3Task object for the floor contact task - m_FloorContactTasks[nodeNumber].task = std::make_shared(); - - // Initialize the R3Task object + // Initialize the SE3Task object ok = ok && m_FloorContactTasks[nodeNumber].task->setKinDyn(m_kinDyn); ok = ok && m_FloorContactTasks[nodeNumber].task->initialize(taskHandler); @@ -853,6 +1121,12 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName, ok = ok && m_qpIK.addTask(m_FloorContactTasks[nodeNumber].task, taskName, 1, m_FloorContactTasks[nodeNumber].weight); // Check if initialization was successful + if (!ok) + { + BiomechanicalAnalysis::log()->error("{} Error in the initialization of the {} task", logPrefix, taskName); + return false; + } + return ok; } diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp index 0fc96d5..c8dd424 100644 --- a/src/IK/tests/HumanIKTest.cpp +++ b/src/IK/tests/HumanIKTest.cpp @@ -32,6 +32,8 @@ TEST_CASE("InverseKinematics test") Eigen::VectorXd qInitial(kinDyn->getNrOfDegreesOfFreedom()); BiomechanicalAnalysis::IK::HumanIK ik; + Eigen::Vector3d I_position = Eigen::Vector3d::Random(); + Eigen::Vector3d I_linearVelocity = Eigen::Vector3d::Random(); manif::SO3d I_R_IMU; manif::SO3Tangentd I_omega_IMU; I_R_IMU.setRandom(); @@ -47,13 +49,21 @@ TEST_CASE("InverseKinematics test") mapNodeData[7].I_R_IMU = I_R_IMU; mapNodeData[7].I_omega_IMU = I_omega_IMU; + std::unordered_map poseNodeData; + poseNodeData["link0"].I_R_IMU = I_R_IMU; + poseNodeData["link0"].I_omega_IMU = I_omega_IMU; + poseNodeData["link0"].I_position = I_position; + poseNodeData["link0"].I_linearVelocity = I_linearVelocity; + qInitial.setConstant(0.0); REQUIRE(ik.initialize(paramHandler, kinDyn)); REQUIRE(ik.setDt(0.1)); - REQUIRE(ik.updateOrientationTask(3, I_R_IMU, I_omega_IMU)); + REQUIRE(ik.updatePoseTask("link0", I_position, I_R_IMU, I_linearVelocity, I_omega_IMU)); + REQUIRE(ik.updateOrientationTask(4, I_R_IMU, I_omega_IMU)); REQUIRE(ik.updateFloorContactTask(10, 11.0)); REQUIRE(ik.updateGravityTask(10, I_R_IMU)); + REQUIRE(ik.updatePoseTasks(poseNodeData)); REQUIRE(ik.updateOrientationAndGravityTasks(mapNodeData)); REQUIRE(ik.updateJointConstraintsTask()); REQUIRE(ik.updateJointRegularizationTask()); diff --git a/src/IK/tests/configTestIK.toml b/src/IK/tests/configTestIK.toml index 9281540..4d468e4 100644 --- a/src/IK/tests/configTestIK.toml +++ b/src/IK/tests/configTestIK.toml @@ -1,21 +1,24 @@ tasks = [ - "PELVIS_TASK", "T8_TASK", "RIGHT_UPPER_ARM_TASK", "RIGHT_FORE_ARM_TASK", - "LEFT_UPPER_ARM_TASK", "LEFT_FORE_ARM_TASK", "RIGHT_UPPER_LEG_TASK", - "RIGHT_LOWER_LEG_TASK", "LEFT_UPPER_LEG_TASK", "LEFT_LOWER_LEG_TASK", + "PELVIS_TASK", "T8_TASK", "RIGHT_UPPER_ARM_TASK", "RIGHT_FORE_ARM_TASK", + "LEFT_UPPER_ARM_TASK", "LEFT_FORE_ARM_TASK", "RIGHT_UPPER_LEG_TASK", + "RIGHT_LOWER_LEG_TASK", "LEFT_UPPER_LEG_TASK", "LEFT_LOWER_LEG_TASK", "GRAVITY_TASK_1", "FLOOR_CONTACT_TASK_1", "JOINT_LIMITS_TASK", "JOINT_REG_TASK", "JOINT_VEL_LIMITS_TASK" ] +use_measured_base_pose = true + [IK] robot_velocity_variable_name = "robot_velocity" verbosity = false [PELVIS_TASK] -type = "SO3Task" +type = "SE3Task" robot_velocity_variable_name = "robot_velocity" frame_name = "link0" +kp_linear = 1.0 kp_angular = 1.0 node_number = 3 -weight = [1.0, 1.0, 1.0] +weight = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] [T8_TASK] type = "SO3Task" @@ -102,8 +105,9 @@ type = "FloorContactTask" robot_velocity_variable_name = "robot_velocity" frame_name = "link10" kp_linear = 1.0 +kp_angular = 1.0 node_number = 10 -weight = [10.0, 10.0, 10.0] +weight = [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] vertical_force_threshold = 60.0 [JOINT_LIMITS_TASK] diff --git a/src/examples/IK/exampleIK.ini b/src/examples/IK/exampleIK.ini index 0ddf363..226028f 100644 --- a/src/examples/IK/exampleIK.ini +++ b/src/examples/IK/exampleIK.ini @@ -7,6 +7,8 @@ tasks ("PELVIS_TASK", "T8_TASK", "RIGHT_UPPER_ARM_TASK "RIGHT_UPPER_LEG_TASK", "RIGHT_LOWER_LEG_TASK", "LEFT_UPPER_LEG_TASK", "LEFT_LOWER_LEG_TASK", "RIGHT_TOE_TASK", "LEFT_TOE_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "JOINT_LIMITS_TASK", "JOINT_REG_TASK") +use_measured_base_pose = true + [IK] robot_velocity_variable_name "robot_velocity" verbosity false