diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..43719cd --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +*.pt +build +devel +logs +.catkin_tools diff --git a/src/unitree_rl/CMakeLists.txt b/src/unitree_rl/CMakeLists.txt new file mode 100644 index 0000000..17955fb --- /dev/null +++ b/src/unitree_rl/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.0.2) +project(unitree_rl) + +set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}") + +find_package(Torch REQUIRED) +find_package(unitree_legged_sdk REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + controller_manager + genmsg + joint_state_controller + robot_state_publisher + roscpp + gazebo_ros + std_msgs + tf + geometry_msgs + unitree_legged_msgs +) + +find_package(gazebo REQUIRED) + +catkin_package( + CATKIN_DEPENDS + unitree_legged_msgs +) + +message("-- CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}") +if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64.*") + set(ARCH amd64) +else() + set(ARCH arm64) +endif() + +set(EXTRA_LIBS -pthread ${unitree_legged_sdk_LIBRARIES}) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${unitree_legged_sdk_INCLUDE_DIRS} + ../unitree_controller/include + +) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + +add_library(model lib/model.cpp lib/model.hpp) +target_link_libraries(model "${TORCH_LIBRARIES}") +set_property(TARGET model PROPERTY CXX_STANDARD 14) + +add_library(observation_buffer lib/observation_buffer.cpp lib/observation_buffer.hpp) +target_link_libraries(observation_buffer "${TORCH_LIBRARIES}") +set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14) + +add_executable(unitree_rl src/unitree_rl.cpp ) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}" + model observation_buffer +) +# add_dependencies(${PROJECT_NAME} unitree_legged_msgs_gencpp) \ No newline at end of file diff --git a/src/unitree_rl/include/unitree_rl.hpp b/src/unitree_rl/include/unitree_rl.hpp new file mode 100644 index 0000000..60be355 --- /dev/null +++ b/src/unitree_rl/include/unitree_rl.hpp @@ -0,0 +1,56 @@ +#ifndef UNITREE_RL +#define UNITREE_RL + +#include +#include +#include +#include +#include "../lib/model.cpp" +#include "../lib/observation_buffer.hpp" +#include "unitree_legged_msgs/MotorCmd.h" + +class Unitree_RL : public Model +{ +public: + + Unitree_RL(); + void modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr& msg); + void jointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg); + void cmdvelCallback(const geometry_msgs::Twist::ConstPtr& msg); + void runModel(const ros::TimerEvent& event); + torch::Tensor forward() override; + torch::Tensor compute_observation() override; + + ObservationBuffer history_obs_buf; + torch::Tensor history_obs; + +private: + + std::string ros_namespace; + + std::vector torque_command_topics; + + ros::Subscriber model_state_subscriber_; + ros::Subscriber joint_state_subscriber_; + ros::Subscriber cmd_vel_subscriber_; + + std::map torque_publishers; + std::vector torque_commands; + + geometry_msgs::Twist vel; + geometry_msgs::Pose pose; + geometry_msgs::Twist cmd_vel; + + std::vector joint_names; + std::vector joint_positions; + std::vector joint_velocities; + + torch::Tensor torques; + + ros::Timer timer; + + std::chrono::high_resolution_clock::time_point start_time; + +}; + +#endif \ No newline at end of file diff --git a/src/unitree_rl/launch/start_env.launch b/src/unitree_rl/launch/start_env.launch new file mode 100644 index 0000000..7405294 --- /dev/null +++ b/src/unitree_rl/launch/start_env.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 5000 + + + + + + + + + + + + + + + + + diff --git a/src/unitree_rl/lib/model.cpp b/src/unitree_rl/lib/model.cpp new file mode 100644 index 0000000..b476ba7 --- /dev/null +++ b/src/unitree_rl/lib/model.cpp @@ -0,0 +1,76 @@ +#include "model.hpp" + +void Model::init_models(std::string actor_path, std::string encoder_path, std::string vq_path) +{ + this->actor = torch::jit::load(actor_path); + this->encoder = torch::jit::load(encoder_path); + this->vq = torch::jit::load(vq_path); + this->init_observations(); +} + +torch::Tensor Model::quat_rotate_inverse(torch::Tensor q, torch::Tensor v) +{ + c10::IntArrayRef shape = q.sizes(); + torch::Tensor q_w = q.index({torch::indexing::Slice(), -1}); + torch::Tensor q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)}); + torch::Tensor a = v * (2.0 * torch::pow(q_w, 2) - 1.0).unsqueeze(-1); + torch::Tensor b = torch::cross(q_vec, v, /*dim=*/-1) * q_w.unsqueeze(-1) * 2.0; + torch::Tensor c = q_vec * torch::bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0; + return a - b + c; +} + +torch::Tensor Model::compute_torques(torch::Tensor actions) +{ + actions *= this->params.action_scale; + torch::Tensor torques = this->params.p_gains * (actions + this->params.default_dof_pos - this->obs.dof_pos) - this->params.d_gains * this->obs.dof_vel; + torch::Tensor clamped = torch::clamp(torques, -(this->params.torque_limits), this->params.torque_limits); + return clamped; +} + +torch::Tensor Model::compute_observation() +{ + std::cout << "You may need to override this compute_observation() function" << std::endl; + + torch::Tensor obs = torch::cat({(this->quat_rotate_inverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale, + (this->quat_rotate_inverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale, + this->quat_rotate_inverse(this->obs.base_quat, this->obs.gravity_vec), + this->obs.commands * this->params.commands_scale, + (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale, + this->obs.dof_vel * this->params.dof_vel_scale, + this->obs.actions}, + 1); + + obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); + + // printf("observation size: %d, %d\n", obs.sizes()[0], obs.sizes()[1]); + + return obs; +} + +void Model::init_observations() +{ + this->obs.lin_vel = torch::tensor({{0.0, 0.0, 0.0}}); + this->obs.ang_vel = torch::tensor({{0.0, 0.0, 0.0}}); + this->obs.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}}); + this->obs.commands = torch::tensor({{0.0, 0.0, 0.0}}); + this->obs.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}}); + this->obs.dof_pos = this->params.default_dof_pos; + this->obs.dof_vel = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); + this->obs.actions = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); +} + +torch::Tensor Model::forward() +{ + std::cout << "You may need to override this forward() function" << std::endl; + + torch::Tensor obs = this->compute_observation(); + + torch::Tensor actor_input = torch::cat({obs}, 1); + + torch::Tensor action = this->actor.forward({actor_input}).toTensor(); + + this->obs.actions = action; + torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions); + + return clamped; +} diff --git a/src/unitree_rl/lib/model.hpp b/src/unitree_rl/lib/model.hpp new file mode 100644 index 0000000..1c20cb0 --- /dev/null +++ b/src/unitree_rl/lib/model.hpp @@ -0,0 +1,69 @@ +#ifndef MODEL_HPP +#define MODEL_HPP + +#include +#include +#include + +struct ModelParams { + int num_observations; + float damping; + float stiffness; + float action_scale; + float num_of_dofs; + float lin_vel_scale; + float ang_vel_scale; + float dof_pos_scale; + float dof_vel_scale; + float clip_obs; + float clip_actions; + torch::Tensor torque_limits; + torch::Tensor d_gains; + torch::Tensor p_gains; + torch::Tensor commands_scale; + torch::Tensor default_dof_pos; +}; + +struct Observations { + torch::Tensor lin_vel; + torch::Tensor ang_vel; + torch::Tensor gravity_vec; + torch::Tensor commands; + torch::Tensor base_quat; + torch::Tensor dof_pos; + torch::Tensor dof_vel; + torch::Tensor actions; +}; + +class Model { +public: + Model(){}; + ModelParams params; + Observations obs; + + virtual torch::Tensor forward(); + virtual torch::Tensor compute_observation(); + + torch::Tensor compute_torques(torch::Tensor actions); + torch::Tensor quat_rotate_inverse(torch::Tensor q, torch::Tensor v); + void init_observations(); + void init_models(std::string actor_path, std::string encoder_path, std::string vq_path); + +protected: + // rl module + torch::jit::script::Module actor; + torch::jit::script::Module encoder; + torch::jit::script::Module vq; + // observation buffer + torch::Tensor lin_vel; + torch::Tensor ang_vel; + torch::Tensor gravity_vec; + torch::Tensor commands; + torch::Tensor base_quat; + torch::Tensor dof_pos; + torch::Tensor dof_vel; + torch::Tensor actions; + +}; + +#endif // MODEL_HPP \ No newline at end of file diff --git a/src/unitree_rl/lib/observation_buffer.cpp b/src/unitree_rl/lib/observation_buffer.cpp new file mode 100644 index 0000000..4f0adea --- /dev/null +++ b/src/unitree_rl/lib/observation_buffer.cpp @@ -0,0 +1,45 @@ +#include "observation_buffer.hpp" + +ObservationBuffer::ObservationBuffer() {} + +ObservationBuffer::ObservationBuffer(int num_envs, + int num_obs, + int include_history_steps) + : num_envs(num_envs), + num_obs(num_obs), + include_history_steps(include_history_steps) +{ + num_obs_total = num_obs * include_history_steps; + obs_buf = torch::zeros({num_envs, num_obs_total}, torch::dtype(torch::kFloat32)); +} + +void ObservationBuffer::reset(std::vector reset_idxs, torch::Tensor new_obs) +{ + std::vector indices; + for (int idx : reset_idxs) { + indices.push_back(torch::indexing::Slice(idx)); + } + obs_buf.index_put_(indices, new_obs.repeat({1, include_history_steps})); +} + +void ObservationBuffer::insert(torch::Tensor new_obs) +{ + // Shift observations back. + torch::Tensor shifted_obs = obs_buf.index({torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(num_obs, num_obs * include_history_steps)}).clone(); + obs_buf.index({torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(0, num_obs * (include_history_steps - 1))}) = shifted_obs; + + // Add new observation. + obs_buf.index({torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(-num_obs, torch::indexing::None)}) = new_obs; +} + +torch::Tensor ObservationBuffer::get_obs_vec(std::vector obs_ids) +{ + std::vector obs; + for (int i = obs_ids.size() - 1; i >= 0; --i) + { + int obs_id = obs_ids[i]; + int slice_idx = include_history_steps - obs_id - 1; + obs.push_back(obs_buf.index({torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(slice_idx * num_obs, (slice_idx + 1) * num_obs)})); + } + return torch::cat(obs, -1); +} diff --git a/src/unitree_rl/lib/observation_buffer.hpp b/src/unitree_rl/lib/observation_buffer.hpp new file mode 100644 index 0000000..72be75c --- /dev/null +++ b/src/unitree_rl/lib/observation_buffer.hpp @@ -0,0 +1,24 @@ +#ifndef OBSERVATION_BUFFER_HPP +#define OBSERVATION_BUFFER_HPP + +#include +#include + +class ObservationBuffer { +public: + ObservationBuffer(int num_envs, int num_obs, int include_history_steps); + ObservationBuffer(); + + void reset(std::vector reset_idxs, torch::Tensor new_obs); + void insert(torch::Tensor new_obs); + torch::Tensor get_obs_vec(std::vector obs_ids); + +private: + int num_envs; + int num_obs; + int include_history_steps; + int num_obs_total; + torch::Tensor obs_buf; +}; + +#endif // OBSERVATION_BUFFER_HPP \ No newline at end of file diff --git a/src/unitree_rl/package.xml b/src/unitree_rl/package.xml new file mode 100644 index 0000000..d5cbbad --- /dev/null +++ b/src/unitree_rl/package.xml @@ -0,0 +1,33 @@ + + + unitree_rl + 0.0.0 + The unitree_rl package + + Ziqi Fan + + TODO + + catkin + genmsg + controller_manager + joint_state_controller + robot_state_publisher + roscpp + std_msgs + controller_manager + joint_state_controller + robot_state_publisher + roscpp + std_msgs + controller_manager + joint_state_controller + robot_state_publisher + roscpp + std_msgs + unitree_legged_msgs + + + + + diff --git a/src/unitree_rl/src/unitree_rl.cpp b/src/unitree_rl/src/unitree_rl.cpp new file mode 100644 index 0000000..38f2652 --- /dev/null +++ b/src/unitree_rl/src/unitree_rl.cpp @@ -0,0 +1,164 @@ +#include "../include/unitree_rl.hpp" +#include + +Unitree_RL::Unitree_RL() +{ + ros::NodeHandle nh; + start_time = std::chrono::high_resolution_clock::now(); + + cmd_vel = geometry_msgs::Twist(); + + torque_commands.resize(12); + + ros_namespace = "/a1_gazebo/"; + + joint_names = { + "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", + }; + + for (int i = 0; i < 12; ++i) + { + torque_publishers[joint_names[i]] = nh.advertise( + ros_namespace + joint_names[i].substr(0, joint_names[i].size() - 6) + "_controller/command", 10); + } + + std::string package_name = "unitree_rl"; + std::string actor_path = ros::package::getPath(package_name) + "/models/actor.pt"; + std::string encoder_path = ros::package::getPath(package_name) + "/models/encoder.pt"; + std::string vq_path = ros::package::getPath(package_name) + "/models/vq_layer.pt"; + this->init_models(actor_path, encoder_path, vq_path); + + this->params.num_observations = 45; + this->params.clip_obs = 100.0; + this->params.clip_actions = 100.0; + this->params.damping = 0.5; + this->params.stiffness = 20; + this->params.d_gains = torch::ones(12) * this->params.damping; + this->params.p_gains = torch::ones(12) * this->params.stiffness; + this->params.action_scale = 0.25; + this->params.num_of_dofs = 12; + this->params.lin_vel_scale = 2.0; + this->params.ang_vel_scale = 0.25; + this->params.dof_pos_scale = 1.0; + this->params.dof_vel_scale = 0.05; + this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale}); + + // hip, thigh, calf + this->params.torque_limits = torch::tensor({{20.0, 55.0, 55.0, // front left + 20.0, 55.0, 55.0, // front right + 20.0, 55.0, 55.0, // rear left + 20.0, 55.0, 55.0}}); // rear right + + this->params.default_dof_pos = torch::tensor({{0.1000, 0.8000, -1.5000, + -0.1000, 0.8000, -1.5000, + 0.1000, 1.0000, -1.5000, + -0.1000, 1.0000, -1.5000}}); + + this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); + + // Create a subscriber object + model_state_subscriber_ = nh.subscribe( + "/gazebo/model_states", 10, &Unitree_RL::modelStatesCallback, this); + + joint_state_subscriber_ = nh.subscribe( + "/a1_gazebo/joint_states", 10, &Unitree_RL::jointStatesCallback, this); + + cmd_vel_subscriber_ = nh.subscribe( + "/cmd_vel", 10, &Unitree_RL::cmdvelCallback, this); + + timer = nh.createTimer(ros::Duration(0.005), &Unitree_RL::runModel, this); +} + +void Unitree_RL::modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg) +{ + + vel = msg->twist[2]; + pose = msg->pose[2]; +} + +void Unitree_RL::cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg) +{ + cmd_vel = *msg; +} + +void Unitree_RL::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +{ + joint_positions = msg->position; + joint_velocities = msg->velocity; +} + +void Unitree_RL::runModel(const ros::TimerEvent &event) +{ + auto duration = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count(); + start_time = std::chrono::high_resolution_clock::now(); + + this->obs.lin_vel = torch::tensor({{vel.linear.x, vel.linear.y, vel.linear.z}}); + this->obs.ang_vel = torch::tensor({{vel.angular.x, vel.angular.y, vel.angular.z}}); + this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z}}); + this->obs.base_quat = torch::tensor({{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}}); + this->obs.dof_pos = torch::tensor({{joint_positions[1], joint_positions[2], joint_positions[0], + joint_positions[4], joint_positions[5], joint_positions[3], + joint_positions[7], joint_positions[8], joint_positions[6], + joint_positions[10], joint_positions[11], joint_positions[9]}}); + this->obs.dof_vel = torch::tensor({{joint_velocities[1], joint_velocities[2], joint_velocities[0], + joint_velocities[4], joint_velocities[5], joint_velocities[3], + joint_velocities[7], joint_velocities[8], joint_velocities[6], + joint_velocities[10], joint_velocities[11], joint_velocities[9]}}); + + torques = this->compute_torques(this->forward()); + + + for (int i = 0; i < 12; ++i) + { + torque_commands[i].tau = torques[0][i].item(); + torque_commands[i].mode = 0x0A; + + torque_publishers[joint_names[i]].publish(torque_commands[i]); + } +} + +torch::Tensor Unitree_RL::compute_observation() +{ + torch::Tensor obs = torch::cat({// (this->quat_rotate_inverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale, + (this->quat_rotate_inverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale, + this->quat_rotate_inverse(this->obs.base_quat, this->obs.gravity_vec), + this->obs.commands * this->params.commands_scale, + (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale, + this->obs.dof_vel * this->params.dof_vel_scale, + this->obs.actions}, + 1); + obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); + return obs; +} + +torch::Tensor Unitree_RL::forward() +{ + torch::Tensor obs = this->compute_observation(); + + history_obs_buf.insert(obs); + history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5}); + + torch::Tensor encoding = this->encoder.forward({history_obs}).toTensor(); + + torch::Tensor z = this->vq.forward({encoding}).toTensor(); + + torch::Tensor actor_input = torch::cat({obs, z}, 1); + + torch::Tensor action = this->actor.forward({actor_input}).toTensor(); + + this->obs.actions = action; + torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions); + + return clamped; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "unitree_rl"); + Unitree_RL unitree_rl; + ros::spin(); + return 0; +} diff --git a/src/unitree_rl/worlds/stairs.world b/src/unitree_rl/worlds/stairs.world new file mode 100644 index 0000000..5963133 --- /dev/null +++ b/src/unitree_rl/worlds/stairs.world @@ -0,0 +1,332 @@ + + + + + + 0.0002 + 1 + 5000 + 0 0 -9.81 + + + quick + 50 + 1.3 + + + 0.0 + 0.2 + 10.0 + 0.001 + + + + + + + + 12 + + + + + + model://sun + + + + model://ground_plane + + + + true + + -2 2 0.5 0 0 0 + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.2 0.2 0.2 1.0 + .421 0.225 0.0 1.0 + + + + + + 3 0 0 0 0 0 + 1 1 1 + + -1.26 -0 0.075 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + -1.26 -0 0.075 0 -0 1.5708 + + + -0.98 -0 0.225 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + -0.98 -0 0.225 0 -0 1.5708 + + + -0.7 -0 0.375 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + -0.7 -0 0.375 0 -0 1.5708 + + + -0.42 -0 0.525 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + -0.42 -0 0.525 0 -0 1.5708 + + + -0.14 -0 0.675 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + -0.14 -0 0.675 0 -0 1.5708 + + + 0.14 0 0.825 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + 0.14 0 0.825 0 -0 1.5708 + + + 0.42 0 0.975 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + 0.42 0 0.975 0 -0 1.5708 + + + 0.7 0 1.125 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + 0.7 0 1.125 0 -0 1.5708 + + + 0.98 0 1.275 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + 0.98 0 1.275 0 -0 1.5708 + + + 1.26 0 1.425 0 -0 1.5708 + + + 10 0.28 0.15 + + + + + 1 1 1 1 + + + 0 + + + + + + 10 0.28 0.15 + + + 1.26 0 1.425 0 -0 1.5708 + + + + + +