Skip to content

Commit

Permalink
xx commit
Browse files Browse the repository at this point in the history
  • Loading branch information
gr-arm-7xx2 committed Jul 5, 2024
1 parent 38123f1 commit 498845d
Show file tree
Hide file tree
Showing 5 changed files with 54 additions and 27 deletions.
5 changes: 4 additions & 1 deletion control_algorithms/admittance/include/admittance/Admittance.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,10 @@ class Admittance
Vector6d arm_twist_;
Vector6d wrench_external_;
Vector6d arm_desired_twist_adm_;
Vector6d arm_desired_accelaration;
Vector6d arm_desired_acceleration;
Vector6d arm_desired_twist;
Vector6d arm_desired_acceleration_adm_;


Vector7d desired_pose_;
Vector3d desired_pose_position_;
Expand Down
2 changes: 1 addition & 1 deletion control_algorithms/admittance/include/hybridadmittance/HybridAdmittance.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ class Admittance
Vector6d arm_twist_;
Vector6d wrench_external_;
Vector6d arm_desired_twist_adm_;
Vector6d arm_desired_accelaration;
Vector6d arm_desired_acceleration;
Vector6d car_desired_twist_adm_;
Vector6d car_desired_accelaration;

Expand Down
59 changes: 41 additions & 18 deletions control_algorithms/admittance/src/Admittance/Admittance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@ Admittance::Admittance(ros::NodeHandle &n,

// Init integrator
arm_desired_twist_adm_.setZero();
arm_desired_twist.setZero();
arm_desired_acceleration.setZero();


ft_arm_ready_ = false;
Expand Down Expand Up @@ -115,19 +117,40 @@ void Admittance::compute_admittance() {
// Translation error w.r.t. desired equilibrium
Vector6d coupling_wrench_arm;

coupling_wrench_arm= D_ * (arm_desired_twist_adm_) + K_*error;
arm_desired_accelaration = M_.inverse() * ( - coupling_wrench_arm + wrench_external_);
coupling_wrench_arm= 9 * D_ * (arm_twist_ - arm_desired_twist) + 10 * K_*error;
// arm_desired_acceleration = M_.inverse() * (- coupling_wrench_arm + wrench_external_);
arm_desired_acceleration_adm_ = M_.inverse() * (- coupling_wrench_arm + wrench_external_) + arm_desired_acceleration;

double a_acc_norm = (arm_desired_accelaration.segment(0, 3)).norm();

// double a_acc_norm = (arm_desired_acceleration.segment(0, 3)).norm();

// if (a_acc_norm > arm_max_acc_) {
// ROS_WARN_STREAM_THROTTLE(1, "Admittance generates high arm accelaration!"
// << " norm: " << a_acc_norm);
// arm_desired_acceleration.segment(0, 3) *= (arm_max_acc_ / a_acc_norm);
// }
// else {
// ROS_WARN_STREAM_THROTTLE(1, "Admittance generates [normal] arm accelaration!"
// << " norm: " << a_acc_norm);
// }
double a_acc_norm = (arm_desired_acceleration_adm_.segment(0, 3)).norm();
if (a_acc_norm > arm_max_acc_) {
ROS_WARN_STREAM_THROTTLE(1, "Admittance generates high arm accelaration!"
<< " norm: " << a_acc_norm);
arm_desired_accelaration.segment(0, 3) *= (arm_max_acc_ / a_acc_norm);
arm_desired_acceleration_adm_.segment(0, 3) *= (arm_max_acc_ / a_acc_norm);
}
else {
ROS_WARN_STREAM_THROTTLE(1, "Admittance generates [normal] arm accelaration!"
<< " norm: " << a_acc_norm);
}

// Integrate for velocity based interface
ros::Duration duration = loop_rate_.expectedCycleTime();
arm_desired_twist_adm_ += arm_desired_accelaration * duration.toSec();
// arm_desired_twist_adm_ += arm_desired_acceleration * duration.toSec();
// arm_desired_twist_adm_ = arm_desired_acceleration * duration.toSec() + arm_twist_;
arm_desired_twist_adm_ = arm_desired_acceleration_adm_ * duration.toSec() + arm_twist_;


}

//!- CALLBACKS -!//
Expand Down Expand Up @@ -200,19 +223,19 @@ void Admittance::send_commands_to_robot() {

// }
geometry_msgs::Twist arm_twist_cmd;
arm_twist_cmd.linear.x = arm_desired_twist_adm_(0)*0.3;
arm_twist_cmd.linear.y = arm_desired_twist_adm_(1)*0.3;
arm_twist_cmd.linear.z = arm_desired_twist_adm_(2)*0.3;
arm_twist_cmd.angular.x = arm_desired_twist_adm_(3)*0.3;
arm_twist_cmd.angular.y = arm_desired_twist_adm_(4)*0.3;
arm_twist_cmd.angular.z = arm_desired_twist_adm_(5)*0.3;

// arm_twist_cmd.linear.x = arm_desired_twist_adm_(0);
// arm_twist_cmd.linear.y = arm_desired_twist_adm_(1);
// arm_twist_cmd.linear.z = arm_desired_twist_adm_(2);
// arm_twist_cmd.angular.x = arm_desired_twist_adm_(3);
// arm_twist_cmd.angular.y = arm_desired_twist_adm_(4);
// arm_twist_cmd.angular.z = arm_desired_twist_adm_(5);
// arm_twist_cmd.linear.x = arm_desired_twist_adm_(0)*0.3;
// arm_twist_cmd.linear.y = arm_desired_twist_adm_(1)*0.3;
// arm_twist_cmd.linear.z = arm_desired_twist_adm_(2)*0.3;
// arm_twist_cmd.angular.x = arm_desired_twist_adm_(3)*0.3;
// arm_twist_cmd.angular.y = arm_desired_twist_adm_(4)*0.3;
// arm_twist_cmd.angular.z = arm_desired_twist_adm_(5)*0.3;

arm_twist_cmd.linear.x = arm_desired_twist_adm_(0);
arm_twist_cmd.linear.y = arm_desired_twist_adm_(1);
arm_twist_cmd.linear.z = arm_desired_twist_adm_(2);
arm_twist_cmd.angular.x = arm_desired_twist_adm_(3);
arm_twist_cmd.angular.y = arm_desired_twist_adm_(4);
arm_twist_cmd.angular.z = arm_desired_twist_adm_(5);
pub_arm_cmd_.publish(arm_twist_cmd);
}

Expand Down
5 changes: 3 additions & 2 deletions control_algorithms/admittance/src/Admittance/WrenchSignalGenerate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,12 +29,13 @@ int main(int argc, char ** argv)
// wrench_msg.wrench.force.z = 5*sin(t);
if(static_cast<int>(t)%10 < 5)
{
wrench_msg.wrench.force.z = 1;
wrench_msg.wrench.force.z = 15;
}
else
{
wrench_msg.wrench.force.z = -1;
wrench_msg.wrench.force.z = -15;
}
wrench_msg.header.stamp = ros::Time::now();
t += 1/TOPIC_HZ;
wrench_pub.publish(wrench_msg);
loop_rate.sleep();
Expand Down
10 changes: 5 additions & 5 deletions control_algorithms/admittance/src/HybridAdmittance/HybridAdmittance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,27 +114,27 @@ void Admittance::compute_admittance() {
Vector6d coupling_wrench_car;

coupling_wrench_arm = D_ * (arm_desired_twist_adm_) + K_*error;
arm_desired_accelaration = M_.inverse() * ( - coupling_wrench_arm + wrench_external_);
arm_desired_acceleration = M_.inverse() * (- coupling_wrench_arm + wrench_external_);

coupling_wrench_car = Dcar_ * (car_desired_twist_adm_);
car_desired_accelaration = M_.inverse() * ( - coupling_wrench_car + wrench_external_);

double a_acc_norm = (arm_desired_accelaration.segment(0, 3)).norm();
double a_acc_norm = (arm_desired_acceleration.segment(0, 3)).norm();

if (a_acc_norm > arm_max_acc_) {
ROS_WARN_STREAM_THROTTLE(1, "Admittance generates high arm accelaration!"
<< " norm: " << a_acc_norm);
arm_desired_accelaration.segment(0, 3) *= (arm_max_acc_ / a_acc_norm);
arm_desired_acceleration.segment(0, 3) *= (arm_max_acc_ / a_acc_norm);
}

double b_acc_norm = (arm_desired_accelaration.segment(0, 3)).norm();
double b_acc_norm = (arm_desired_acceleration.segment(0, 3)).norm();

if (b_acc_norm > 10) {
car_desired_accelaration.segment(0, 3) *= (1.5 / b_acc_norm);
}
// Integrate for velocity based interface
ros::Duration duration = loop_rate_.expectedCycleTime();
arm_desired_twist_adm_ += arm_desired_accelaration * duration.toSec();
arm_desired_twist_adm_ += arm_desired_acceleration * duration.toSec();
car_desired_twist_adm_ += car_desired_accelaration * duration.toSec();
}

Expand Down

0 comments on commit 498845d

Please sign in to comment.