diff --git a/src/gaz_simulation/world/gaz_new_test.sdf b/src/gaz_simulation/world/gaz_new_test.sdf
index 22fe398..c565fd7 100644
--- a/src/gaz_simulation/world/gaz_new_test.sdf
+++ b/src/gaz_simulation/world/gaz_new_test.sdf
@@ -37,7 +37,7 @@
     
     <actor name="actor_walking">
       <plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so">
-        
+
       </plugin>
       <skin>
         <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp
index 27935d1..cd3cbc7 100644
--- a/src/ign_actor_plugin/src/ActorSystem.cpp
+++ b/src/ign_actor_plugin/src/ActorSystem.cpp
@@ -48,29 +48,50 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani
     *animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero());
     _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
 
-    int foundIndex = -1;
+    int foundAnimationIndex = -1;
     for (int animationIndex = 0  ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) {
         auto animation = actorComp->Data().AnimationByIndex(animationIndex);
         if (animation->Name() == animationName){
-            foundIndex = animationIndex;
+            foundAnimationIndex = animationIndex;
             break;
         }
     }
 
-    if(foundIndex!=-1){
+    if(foundAnimationIndex != -1){
         igndbg << "Found animation to play" << std::endl;
-        auto fileName = actorComp->Data().AnimationByIndex(foundIndex)->Filename();
-        igndbg << "Path: " << fileName << std::endl;
-        igndbg << "Name: " << common::MeshManager::Instance()->Load(fileName)->Name() << std::endl;
+        auto fileName = actorComp->Data().AnimationByIndex(foundAnimationIndex)->Filename();
 
-        auto skeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();
+        currentSkeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();
 
-        if (skeleton != nullptr){
-            duration = skeleton->Animation(0)->Length();
+        igndbg << "FileName: " << fileName << std::endl;
+        igndbg << "AnimCnt: " << currentSkeleton->AnimationCount() << std::endl;
+
+        auto rootNode = currentSkeleton->RootNode()->Name();
+
+        currentRootNodeAnimation = currentSkeleton->Animation(0)->NodeAnimationByName(rootNode);
+        currentTranslationAlign = currentSkeleton->AlignTranslation(0, rootNode);
+        currentRotationAlign = currentSkeleton->AlignRotation(0, rootNode);
+
+        igndbg << "Translation: " << currentTranslationAlign << std::endl;
+        igndbg << "Rotation: " << currentRotationAlign << std::endl;
+
+        if (currentSkeleton != nullptr){
+            duration = currentSkeleton->Animation(0)->Length();
             igndbg << "length: " << duration << " seconds." << std::endl;
         } else {
             ignerr << "No skeleton found!" << std::endl;
         }
+
+
+
+        auto initialPose = currentRootNodeAnimation -> FrameAt(0).Pose();
+        auto poseComponent = _ecm.Component<components::Pose>(entity);
+
+        auto newPose = initialPose;
+        newPose.Pos().X(0);
+        newPose.Pos().Y(0);
+        *poseComponent = components::Pose(newPose);
+
     } else {
          ignerr << "No animation found!" << std::endl;
     }
@@ -82,40 +103,26 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         case SETUP: {
             igndbg << "Setting up actor plugin" << std::endl;
 
-            auto actorComp = _ecm.Component<components::Actor>(entity);
+            auto actorComponent = _ecm.Component<components::Actor>(entity);
 
-            auto animationNameComp = _ecm.Component<components::AnimationName>(entity);
-            if (nullptr == animationNameComp) {
-                _ecm.CreateComponent(entity, components::AnimationName(actorComp->Data().AnimationByIndex(0)->Name()));
+            auto animationNameComponent = _ecm.Component<components::AnimationName>(entity);
+            if (nullptr == animationNameComponent) {
+                _ecm.CreateComponent(entity, components::AnimationName(actorComponent->Data().AnimationByIndex(0)->Name()));
             }
 
-            auto animTimeComp = _ecm.Component<components::AnimationTime>(entity);
-            if (nullptr == animTimeComp) {
+            auto animTimeComponent = _ecm.Component<components::AnimationTime>(entity);
+            if (nullptr == animTimeComponent) {
                 _ecm.CreateComponent(entity, components::AnimationTime());
             }
 
-            ignition::math::Pose3d initialPose;
-            auto poseComp = _ecm.Component<components::Pose>(entity);
-            if (nullptr == poseComp) {
+            auto poseComponent = _ecm.Component<components::Pose>(entity);
+            if (nullptr == poseComponent) {
                 _ecm.CreateComponent(entity, components::Pose(ignition::math::Pose3d::Zero));
-            } else {
-                initialPose = poseComp->Data();
-
-                // We'll be setting the actor's X/Y pose with respect to the world. So we
-                // zero the current values.
-                auto newPose = initialPose;
-                newPose.Pos().X(0);
-                newPose.Pos().Y(0);
-                *poseComp = components::Pose(newPose);
             }
 
-
-
-            auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(entity);
-            if (nullptr == trajectoryPoseComp) {
-                // Leave Z to the pose component, control only 2D with Trajectory
-                //initialPose.Pos().Z(0);
-                _ecm.CreateComponent(entity, components::TrajectoryPose(initialPose));
+            auto trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
+            if (nullptr == trajectoryPoseComponent) {
+                _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
             }
 
             currentState = ActorSystemState::IDLE;
@@ -142,35 +149,114 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
             *animationTimeComponent = components::AnimationTime(newTime);
             _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
-            
+
+
+            /*
+            if(currentRootNodeAnimation == nullptr){
+                break;
+            }
+
+            auto rootTransformation = currentRootNodeAnimation->FrameAt(newTimeSeconds,false);
+            math::Matrix4d totalTf = currentTranslationAlign * rootTransformation * currentRotationAlign;
+
+            auto translation = totalTf.Translation();
+            translation[0] = 0;
+            totalTf.SetTranslation(translation);
+
+            auto poseComp = _ecm.Component<components::Pose>(entity);
+            *poseComp = components::Pose (totalTf.Pose());
+            _ecm.SetChanged(entity, components::Pose::typeId, ComponentState::OneTimeChange);
+            */
+
             break;
         }
         case MOVEMENT:{
             if (lastState == ActorSystemState::IDLE){
+                igndbg << "Starting Movement..." << std::endl;
                 switchAnimation(_ecm, movementTarget.animation_name);
             }
 
             auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
             auto actorPose = trajectoryPoseComp->Data();
-            auto initialPose = actorPose;
             auto targetPose = movementTarget.target_position;
-    
-            double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length();
-    
-            auto animTimeComp = _ecm.Component<components::AnimationTime>(
-                this->entity);
 
-            auto animTime = animTimeComp->Data() +
-                    std::chrono::duration_cast<std::chrono::steady_clock::duration>(
-                        std::chrono::duration<double>(
-                            distanceTraveled *
-                            1
-                        )
-                    );
-            *animTimeComp = components::AnimationTime(animTime);
+            auto targetDirection = actorPose.Pos() - math::Vector3d(targetPose[0],targetPose[1],targetPose[2]);
+
+            if (targetDirection.Length()<0.05){
+                currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
+                currentState = ActorSystemState::IDLE;
+                break;
+            }
+
+            auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
+            auto oldTimeSeconds = std::chrono::duration<double>(animationTimeComponent->Data()).count();
+            auto deltaTimeSeconds = std::chrono::duration<double>(_info.dt).count();
+            auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
+            auto newTime = animationTimeComponent->Data() + _info.dt;
+
+            if(newTimeSeconds >= duration){
+                newTimeSeconds -= duration;
+                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds));
+            }
+
+            auto targetYaw = atan2(targetDirection.Y(),targetDirection.X());
+            auto currentYaw = actorPose.Rot().Yaw();
+
+            auto angularDirection = fmod((targetYaw - currentYaw), 2*M_PI);
+            if (angularDirection>M_PI){
+                angularDirection-=M_PI;
+            }
+            if (angularDirection<=-M_PI){
+                angularDirection+=M_PI;
+            }
+            auto turnSpeed = 1.0;
+
+            if (angularDirection < 0){
+                turnSpeed = -turnSpeed;
+            }
+
+            if (abs(angularDirection) > 0.01){
+                actorPose.Rot().Euler(0,0,actorPose.Rot().Yaw()+(turnSpeed*deltaTimeSeconds));
+                *trajectoryPoseComp = components::TrajectoryPose(actorPose);
+                _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
+            }else{
+                if (nullptr == currentRootNodeAnimation){
+                    ignerr << "Current animation doesn't move root node, this is unsupported for movement animations." << std::endl;
+                    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;
+                }
+
+                actorPose.Pos() += targetDirection.Normalize() * distance;
+                actorPose.Rot().Euler(0,0,targetYaw);
+                *trajectoryPoseComp = components::TrajectoryPose(actorPose);
+                *animationTimeComponent = components::AnimationTime(newTime);
+
+                _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
+                _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
+            }
             break;
         }
         case IDLE:
+            if (lastState != currentState){
+                igndbg << "Now idling..." << std::endl;
+            }
             break;
     }
     lastState = currentState;
@@ -192,7 +278,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
     this->entity = _entity;
     this->currentState = ActorSystemState::SETUP;
     this->lastState = ActorSystemState::IDLE;
-    
+
     rclcpp::init(0, {});
 
     std::string topic = "ActorPlugin";
@@ -202,6 +288,8 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
     
     node = rclcpp::Node::make_shared("moveService", topic);
     
+    #pragma clang diagnostic push
+    #pragma ide diagnostic ignored "UnusedValue"
     animationServer = rclcpp_action::create_server<action::Animation>(
             node,
             "animation",
@@ -216,7 +304,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
                 }
 
             },
-            [this](const AnimationServerGoalPtr PH1) {
+            [this](const AnimationServerGoalPtr& animationGoalPtr) {
                 if (this->currentState == ActorSystemState::IDLE) {
                     return rclcpp_action::CancelResponse::REJECT;
                 }else{
@@ -227,14 +315,15 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
                 }
 
             },
-            [this](const AnimationServerGoalPtr PH1) {
+            [this](const AnimationServerGoalPtr& animationGoalPtr) {
                 threadMutex.lock();
-                currentAnimationGoalPtr = PH1;
+                currentAnimationGoalPtr = animationGoalPtr;
                 currentState = ActorSystemState::ANIMATION;
                 threadMutex.unlock();
             }
     );
 
+
     movementServer = rclcpp_action::create_server<action::Movement>(
             node,
             "movement",
@@ -249,7 +338,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
                     return rclcpp_action::GoalResponse::REJECT;
                 }
             },
-            [this](const MovementServerGoalPtr goalUuid ){
+            [this](const MovementServerGoalPtr& movementGoalPtr ){
                 if (this->currentState == ActorSystemState::IDLE) {
                     return rclcpp_action::CancelResponse::REJECT;
                 }else{
@@ -259,13 +348,14 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
                     return rclcpp_action::CancelResponse::ACCEPT;
                 }
             },
-            [this](const MovementServerGoalPtr goalUuid ){
+            [this](const MovementServerGoalPtr& movementGoalPtr ){
                 threadMutex.lock();
-                currentMovementGoalPtr = goalUuid;
+                currentMovementGoalPtr = movementGoalPtr;
                 currentState = ActorSystemState::MOVEMENT;
                 threadMutex.unlock();
             }
     );
+    #pragma clang diagnostic pop
 
     igndbg << "Spinning node..." << std::endl;
     std::thread spinThread([this]() {
diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp
index 137cc93..dbbaeb4 100644
--- a/src/ign_actor_plugin/src/ActorSystem.hpp
+++ b/src/ign_actor_plugin/src/ActorSystem.hpp
@@ -18,6 +18,8 @@
 #include <ign_actor_plugin_msgs/action/movement.hpp>
 #include <rclcpp_action/server_goal_handle.hpp>
 #include <string>
+#include <ignition/common/Skeleton.hh>
+#include <ignition/common/NodeAnimation.hh>
 
 using namespace ign_actor_plugin_msgs;
 using rclcpp_action::ServerGoalHandle;
@@ -33,47 +35,50 @@ using rclcpp_action::ServerGoalHandle;
 
 namespace ignition {
     namespace gazebo {
-        enum ActorSystemState{
-            SETUP,IDLE,MOVEMENT,ANIMATION
+        enum ActorSystemState {
+            SETUP, IDLE, MOVEMENT, ANIMATION
         };
-        
-        class ActorSystem:
+
+        class ActorSystem :
                 public System,
                 public ISystemPreUpdate,
                 public ISystemConfigure {
-            
-            private:
-                std::shared_ptr<rclcpp::Node> node;
-                std::shared_ptr<AnimationActionServer> animationServer;
-                std::shared_ptr<MovementActionServer> movementServer;
-                ActorSystemState currentState,lastState;
-                action::Animation::Goal animationTarget;
-                action::Movement::Goal movementTarget;
-                AnimationServerGoalPtr currentAnimationGoalPtr;
-                MovementServerGoalPtr currentMovementGoalPtr;
-                double duration{};
-                Entity entity{kNullEntity};
-                std::mutex  threadMutex;
-            
-            public:
-                ActorSystem();
-            
-            public:
-                ~ActorSystem() override;
-            
-            public:
-                void PreUpdate(const UpdateInfo &_info,
-                               EntityComponentManager &_ecm) override;
-            
-            public:
-                void Configure(const ignition::gazebo::Entity &_entity,
-                               const std::shared_ptr<const sdf::Element> &_sdf,
-                               EntityComponentManager &_ecm,
-                               EventManager &/*_eventMgr*/) override;
-            
-            private:
-                void switchAnimation(EntityComponentManager &_ecm, std::string& animationName);
-            
+
+        private:
+            std::shared_ptr<rclcpp::Node> node;
+            std::shared_ptr<AnimationActionServer > animationServer;
+            std::shared_ptr<MovementActionServer > movementServer;
+            ActorSystemState currentState, lastState;
+            action::Animation::Goal animationTarget;
+            action::Movement::Goal movementTarget;
+            AnimationServerGoalPtr currentAnimationGoalPtr;
+            MovementServerGoalPtr currentMovementGoalPtr;
+            double duration{};
+            Entity entity{kNullEntity};
+            std::mutex threadMutex;
+            std::shared_ptr<common::Skeleton> currentSkeleton;
+            common::NodeAnimation* currentRootNodeAnimation;
+            math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
+
+        public:
+            ActorSystem();
+
+        public:
+            ~ActorSystem() override;
+
+        public:
+            void PreUpdate(const UpdateInfo &_info,
+                           EntityComponentManager &_ecm) override;
+
+        public:
+            void Configure(const ignition::gazebo::Entity &animationGoalPtr,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           EventManager &/*_eventMgr*/) override;
+
+        private:
+            void switchAnimation(EntityComponentManager &_ecm, std::string &animationName);
+
 
         };
     }