Skip to content

Commit

Permalink
[Ninja] workaround to calculate rotational erro of joint
Browse files Browse the repository at this point in the history
  • Loading branch information
@sugihara-16
sugihara-16 committed Sep 15, 2024
1 parent 09fa1e6 commit 385fa65
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 3 deletions.
7 changes: 7 additions & 0 deletions robots/ninja/include/ninja/ninja_navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include <ninja/model/ninja_robot_model.h>
#include <geometry_msgs/Pose.h>
#include <regex>
#include <aerial_robot_control/control/utils/pid.h>

namespace aerial_robot_navigation
{
Expand Down Expand Up @@ -36,6 +37,7 @@ namespace aerial_robot_navigation
KDL::JntArray goal_joint_pos_;
KDL::JntArray start_joint_pos_;


};
class NinjaNavigator : public BeetleNavigator
{
Expand All @@ -53,6 +55,8 @@ namespace aerial_robot_navigation
void setCoM2Base(const KDL::Frame com2base){com2base_ = com2base;}
void morphingProcess();

bool getFreeJointFlag(){return free_joint_flag_;}
std::vector<double> getJointPosErr(){return joint_pos_errs_;}
template<class T> T getCom2Base();
protected:
std::mutex mutex_com2base_;
Expand All @@ -74,6 +78,7 @@ namespace aerial_robot_navigation
ros::Publisher joint_control_pub_;

std::map<int, ModuleData> assembled_modules_data_;
std::vector<double> joint_pos_errs_;

KDL::Rotation target_com_rot_;
KDL::Frame com2base_;
Expand All @@ -83,6 +88,8 @@ namespace aerial_robot_navigation
double morphing_vel_;
double joint_pos_chnage_thresh_;
int joint_process_func_;

bool free_joint_flag_;

};
template<> inline KDL::Frame NinjaNavigator::getCom2Base()
Expand Down
54 changes: 51 additions & 3 deletions robots/ninja/src/ninja_navigation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ void NinjaNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
joint_control_pub_ = nh_.advertise<sensor_msgs::JointState>("joints_ctrl", 1);
target_com_rot_sub_ = nh_.subscribe("/target_com_rot", 1, &NinjaNavigator::setTargetCoMRotCallback, this);
target_joints_pos_sub_ = nh_.subscribe("/assembly/target_joint_pos", 1, &NinjaNavigator::assemblyJointPosCallback, this);
joint_pos_errs_.resize(module_joint_num_);
}

void NinjaNavigator::update()
Expand Down Expand Up @@ -106,6 +107,11 @@ void NinjaNavigator::updateEntSysState()
}
setModuleNum(assembled_modules_ids_.size());
std::sort(assembled_modules_ids_.begin(), assembled_modules_ids_.end());

auto it = std::find(assembled_modules_ids_.begin(), assembled_modules_ids_.end(), my_id_);
if (it != assembled_modules_ids_.end()) {
my_index_ = std::distance(assembled_modules_ids_.begin(), it);
}
if(control_flag_){
reconfig_flag_ = (pre_assembled_modules_ != module_num_) ? true : false;
pre_assembled_modules_ = module_num_;
Expand Down Expand Up @@ -527,7 +533,7 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst
switch(joint_process_func_)
{
case CONSTANT:
if(id == my_id_)
if(id == my_id_ && !free_joint_flag_)
{
joint_send_flag = true;
joints_ctrl_msg.name.push_back(target_joint_name);
Expand All @@ -554,6 +560,7 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst

void NinjaNavigator::morphingProcess()
{
if(!getCurrentAssembled()) return;
sensor_msgs::JointState joints_ctrl_msg;
bool joint_send_flag = false;
std::map<int, std::string> joint_map;
Expand All @@ -571,7 +578,7 @@ void NinjaNavigator::morphingProcess()
{
data.first_joint_processed_time_[i] = -1.0;
data.joint_pos_(i) = data.goal_joint_pos_(i);
if(id == my_id_)
if(id == my_id_ && !free_joint_flag_)
{
joint_send_flag = true;
joints_ctrl_msg.name.push_back(joint_map[i]);
Expand All @@ -591,7 +598,7 @@ void NinjaNavigator::morphingProcess()
default:
break;
}
if(id == my_id_)
if(id == my_id_ && !free_joint_flag_)
{
joint_send_flag = true;
joints_ctrl_msg.name.push_back(joint_map[i]);
Expand All @@ -600,6 +607,46 @@ void NinjaNavigator::morphingProcess()
}
}
if(joint_send_flag) joint_control_pub_.publish(joints_ctrl_msg);

/* calculate joint pos err */
int neighbor_id_;
if(my_id_ < leader_id_)
neighbor_id_ = assembled_modules_ids_[my_index_+1];
else if(my_id_ > leader_id_)
neighbor_id_ = assembled_modules_ids_[my_index_-1];
else
return;
try
{
KDL::Frame myCog2NeighborCog;
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer_.lookupTransform(my_name_ + std::to_string(my_id_) + std::string("/fc") ,
my_name_ + std::to_string(neighbor_id_) + std::string("/fc"),
ros::Time(0));
tf::transformMsgToKDL(transformStamped.transform, myCog2NeighborCog);
double joint_pos_roll, joint_pos_pitch, joint_pos_yaw;
// myCog2NeighborCog.M.GetRPY(joint_pos_roll, joint_pos_pitch, joint_pos_yaw);
if(my_id_ < leader_id_)
{
joint_pos_yaw = std::atan2(-myCog2NeighborCog.M(0,1),myCog2NeighborCog.M(0,0));
joint_pos_pitch = std::asin(myCog2NeighborCog.M(0,2));
joint_pos_errs_[0] = assembled_modules_data_[neighbor_id_].goal_joint_pos_(0) - joint_pos_pitch;
joint_pos_errs_[1] = assembled_modules_data_[my_id_].goal_joint_pos_(1) - joint_pos_yaw;
}
else if(my_id_ > leader_id_)
{
joint_pos_yaw = std::atan2(-myCog2NeighborCog.M(0,1),myCog2NeighborCog.M(0,0));
joint_pos_pitch = std::asin(myCog2NeighborCog.M(0,2));
joint_pos_errs_[0] = assembled_modules_data_[my_id_].goal_joint_pos_(0) - (-joint_pos_pitch);
joint_pos_errs_[1] = assembled_modules_data_[neighbor_id_].goal_joint_pos_(1) - (-joint_pos_yaw);
}
}
catch (tf2::TransformException& ex)
{
ROS_ERROR_STREAM("not exist neighbor mentioned. ID is "<<neighbor_id_ );
return;
}

}


Expand All @@ -609,6 +656,7 @@ void NinjaNavigator::rosParamInit()
getParam<double>(nh, "morphing_vel", morphing_vel_, M_PI/4.0);
getParam<double>(nh, "joint_pos_change_thresh", joint_pos_chnage_thresh_, 0.01);
getParam<int>(nh, "joint_process_func", joint_process_func_, CONSTANT);
getParam<bool>(nh, "free_joint_flag", free_joint_flag_, false);
BeetleNavigator::rosParamInit();
}

Expand Down

0 comments on commit 385fa65

Please sign in to comment.