From 1eaa7efe547f407dff68855c5c93c1604a94cd75 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Thu, 15 Jul 2021 14:01:36 +0200
Subject: [PATCH] (FR, RK) Fix orientation PID controller (resetting I-value).
Fix visualization
---
.../ObstacleAwarePlatformUnit.cpp | 12 +++++++++---
1 file changed, 9 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index b15b4c7bf..fc0274a7e 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -171,6 +171,8 @@ armarx::ObstacleAwarePlatformUnit::moveTo(
invalidate(m_control_data.target_vel);
invalidate(m_control_data.target_rot_vel);
+ m_rot_pid_controller.reset();
+
schedule_high_level_control_loop(control_mode::position);
}
@@ -460,8 +462,10 @@ const
ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
- return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
- uncapped_target_rot_vel);
+ return std::clamp(uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_rot_vel);
+
+ // return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ // uncapped_target_rot_vel);
}
@@ -607,7 +611,7 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
l_prog.add(Cylinder{"boundingCylinder"}
.position(agent_pos)
.color(Color::cyan(255, 64))
- .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, 0, 1000)).matrix())
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
.radius(m_viz.boundingCircle.radius)
.height(2000));
}
@@ -687,6 +691,8 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
.color(Color::cyan(255, 128))
.width(25));
}
+
+ // TODO rotation arrow
}
// Agent.
--
GitLab