yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Clarify movement code

Browse Source
This commit is contained in:
Bastian Hofmann
2023-01-16 12:41:34 +00:00
parent f9f1766004
commit 3199b400d1
4 changed files with 16 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/ign_actor_plugin/src/ActorSystem.cpp
View File

@@ -17,7 +17,6 @@
#include <ignition/gazebo/EntityComponentManager.hh>
#include <sdf/Actor.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Mesh.hh>
@@ -180,7 +179,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto actorPose = trajectoryPoseComp->Data();
auto targetPose = movementTarget.target_position;
auto targetDirection = actorPose.Pos() - math::Vector3d(targetPose[0],targetPose[1],targetPose[2]);
auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos();
if (targetDirection.Length()<0.05){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
@@ -200,15 +199,9 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
}
auto targetYaw = atan2(targetDirection.Y(),targetDirection.X());
auto currentYaw = actorPose.Rot().Yaw();
auto currentYaw = fmod(actorPose.Rot().Yaw(),M_PI);
auto angularDirection = fmod((targetYaw - currentYaw), 2*M_PI);
if (angularDirection>M_PI){
angularDirection-=M_PI;
}
if (angularDirection<=-M_PI){
angularDirection+=M_PI;
}
auto angularDirection = fmod((targetYaw - currentYaw), M_PI+0.001); // additional 0.001 rad to prevent instant flip through 180 rotation
auto turnSpeed = 1.0;
if (angularDirection < 0){
@@ -225,25 +218,10 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break;
}
double distance;
if (oldTimeSeconds<=newTimeSeconds){
auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
distance = oldPos - newPos;
}else{
auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
// \/
auto endPos = currentRootNodeAnimation->KeyFrame(currentRootNodeAnimation->FrameCount()-1).second.Pose().X();
// Big Distance
auto startPos = currentRootNodeAnimation->KeyFrame(0).second.Pose().X();
// \/
auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
distance = (oldPos - endPos) + (startPos - newPos);
igndbg << (oldPos - endPos) << "|" << (startPos - newPos) << std::endl;
}
double distance = (deltaTimeSeconds/duration)*movementTarget.animation_distance;
actorPose.Pos() += targetDirection.Normalize() * distance;
actorPose.Rot().Euler(0,0,targetYaw);
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
*animationTimeComponent = components::AnimationTime(newTime);