Streaming odometry info, CoM and footsteps #145

SimoneMic · 2023-04-12T15:00:56Z

On this PR a parallel thread periodically check the status of the feet and computes the virtual unicycle, i.e. it is streaming odometry information of the robot.
In the method updateModule is also streamed the planned CoM and the relative planned footsteps (only once per double support).

This is part of the changes required for the integration with the navigation stack #142 and the PR #143
Splitting into multiple PRs to improve readability.

…nning. Streaming also planned footsteps and CoM

S-Dafarra

Also in this case, I would ask to move as much logic as possible from Module.cpp to dedicated classes

S-Dafarra · 2023-04-12T15:07:54Z

src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h

+         * Get the footprints planned by the unicycle generator
+         * @param leftFootprints vector containing the desired poses of the left foot on the xy plane
+         * @param rightFootprints vector containing the desired poses of the right foot on the xy plane
+         * @return true/false in case of success/failure.
+         */
+        bool getFootprints(std::vector<iDynTree::Vector3>& leftFootprints, std::vector<iDynTree::Vector3>& rightFootprints);


From here it is not clear that the first two elements are the position and the third element is the angle. I would suggest using a dedicated struct

S-Dafarra · 2023-04-12T15:10:42Z

src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h

+        /**
+         * Get the unicycle state from the feet of the robot in the odom frame (aka world frame)
+         * @param virtualUnicyclePose pose of the virtual unicycle in the odom frame
+         * @param referenceUnicyclePose pose of the reference virtual unicycle in the odom frame
+         * @param stanceFoot returns a string with the stance foot used for the computation
+         * @return true/false in case of success/failure.
+         */
+        bool getUnicycleState(iDynTree::Vector3& virtualUnicyclePose, iDynTree::Vector3& referenceUnicyclePose, const std::string& stanceFoot);


Is stanceFoot an input or an output of the method? If it is an input, could you move it as a first input?

S-Dafarra · 2023-04-12T15:12:59Z

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

+        pose(0) = leftDequeue.at(i).position(0);
+        pose(1) = leftDequeue.at(i).position(1);
+        pose(2) = leftDequeue.at(i).angle;
+        leftFootprints.push_back(pose);


I would expect this method to fill the input vectors rather than appending new elements. I would suggest to resize them to the correct dimension at the beginning, and then edit the elements by index. In this way, you can avoid to allocate new memory if the number of footprints remains constant over time.

S-Dafarra · 2023-04-12T15:18:13Z

src/TrajectoryPlanner/src/TrajectoryGenerator.cpp

+    //Odom (expressed in odom frame)
+    double s_theta = std::sin(unicycleAngleOdom);
+    double c_theta = std::cos(unicycleAngleOdom);
+    unicycleRotation(0,0) = c_theta;
+    unicycleRotation(0,1) = -s_theta;
+    unicycleRotation(1,0) = s_theta;
+    unicycleRotation(1,1) = c_theta;
+    unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot;   // position in respect to the support foot
+    unicycleOdom = unicyclePosition + footPosition;     //postion in odom frame
+    virtualUnicyclePose(0) = unicycleOdom(0);
+    virtualUnicyclePose(1) = unicycleOdom(1);
+    virtualUnicyclePose(2) = unicycleAngleOdom;
+
+    iDynTree::Vector2 referencePointInAbsoluteFrame;
+    iDynTree::toEigen(referencePointInAbsoluteFrame) = unicycleRotation * (iDynTree::toEigen(m_referencePointDistance)) + unicycleOdom;
+    referenceUnicyclePose(0) = referencePointInAbsoluteFrame(0);
+    referenceUnicyclePose(1) = referencePointInAbsoluteFrame(1);
+    referenceUnicyclePose(2) = unicycleAngleOdom;
+


Maybe this can be handy: https://github.com/robotology/unicycle-footstep-planner/blob/f9db393b73ef92b2bf93ceac91da7a1db607f707/include/UnicycleFoot.h#L60

S-Dafarra · 2023-04-12T15:19:19Z

src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini

+# Loop rate for the thread computing the navigation trigger
+navigationTriggerLoopRate   100
+


As mentioned in #144, this would fit well in a separate file named navigation

S-Dafarra · 2023-04-12T15:29:24Z

src/WalkingModule/src/Module.cpp

+// This thread publishes the internal informations of the virtual unicycle extracted from the feet and the CoM speed
+// It's the internal odometry
+void WalkingModule::computeVirtualUnicycleThread()
+{
+    yInfo() << "[WalkingModule::computeVirtualUnicycleThread] Starting Thread";
+    bool inContact = false;
+    while (m_runThreads)
+    {
+        if (m_robotState != WalkingFSM::Walking)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate));
+            continue;
+        }
+
+        iDynTree::Vector3 virtualUnicyclePose, virtualUnicycleReference;
+        std::string stanceFoot;
+        iDynTree::Transform footTransformToWorld, root_linkTransform;
+        //Stance foot check
+        if (m_leftInContact.size() > 0)
+        {
+            if (m_leftInContact.at(0))
+            {
+                stanceFoot = "left";
+                footTransformToWorld = m_FKSolver->getLeftFootToWorldTransform();
+            }
+            else
+            {
+                stanceFoot = "right";
+                footTransformToWorld = m_FKSolver->getRightFootToWorldTransform();
+            }
+            inContact = true;
+        }
+        else if (m_rightInContact.size() > 0)
+        {
+            if (m_rightInContact.at(0))
+            {
+                stanceFoot = "right";
+                footTransformToWorld = m_FKSolver->getRightFootToWorldTransform();
+            }
+            else
+            {
+                stanceFoot = "left";
+                footTransformToWorld = m_FKSolver->getLeftFootToWorldTransform();
+            }
+            inContact = true;
+        }
+        else
+        {
+            inContact = false;
+        }
+        root_linkTransform = m_FKSolver->getRootLinkToWorldTransform();   //world -> rootLink transform
+        //Data conversion and port writing
+        if (inContact)
+        {
+            if (m_trajectoryGenerator->getUnicycleState(virtualUnicyclePose, virtualUnicycleReference, stanceFoot))
+            {
+                //send data
+                yarp::os::Stamp stamp (0, yarp::os::Time::now());   //move to private member of the class
+                m_unicyclePort.setEnvelope(stamp);
+                auto& data = m_unicyclePort.prepare();
+                data.clear();
+                auto& unicyclePose = data.addList();
+                unicyclePose.addFloat64(virtualUnicyclePose(0));
+                unicyclePose.addFloat64(virtualUnicyclePose(1));
+                unicyclePose.addFloat64(virtualUnicyclePose(2));
+                auto& referencePose = data.addList();
+                referencePose.addFloat64(virtualUnicycleReference(0));
+                referencePose.addFloat64(virtualUnicycleReference(1));
+                referencePose.addFloat64(virtualUnicycleReference(2));
+                data.addString(stanceFoot);
+
+                //add root link stransform: X, Y, Z, r, p, yaw,
+                auto& transform = data.addList();
+                transform.addFloat64(root_linkTransform.getPosition()(0));
+                transform.addFloat64(root_linkTransform.getPosition()(1));
+                transform.addFloat64(root_linkTransform.getPosition()(2));
+                transform.addFloat64(root_linkTransform.getRotation().asRPY()(0));
+                transform.addFloat64(root_linkTransform.getRotation().asRPY()(1));
+                transform.addFloat64(root_linkTransform.getRotation().asRPY()(2));
+
+                //planned velocity of the CoM
+                auto comVel = m_stableDCMModel->getCoMVelocity();
+                auto& velData = data.addList();
+                velData.addFloat64(comVel(0));
+                velData.addFloat64(comVel(1));
+                m_unicyclePort.write();
+            }
+            else
+            {
+                yError() << "[WalkingModule::computeVirtualUnicycleThread] Could not getUnicycleState from m_trajectoryGenerator (no data sent).";
+            }
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate));
+    }
+    yInfo() << "[WalkingModule::computeVirtualUnicycleThread] Terminated thread";
+}


As mentioned in #144 it would be best if the modifications to Module.cpp were minimal. This file is meant to connect all the different bits and pieces. It is more of a coordinator, so it should have limited logic.

SimoneMic · 2023-04-27T14:29:10Z

I have updated the code to satisfy the requests.
Now there's a separate file NavigationHelper that holds all the code needed by the Nav2 stack.

S-Dafarra · 2023-04-27T15:16:33Z

I will wait for #144 to be merged first as they share a lot of code

…ntrollers into odom_stream

SimoneMic added 2 commits April 12, 2023 14:45

Streaming odometry info of the virtual unicycle used for footstep pla…

fa8eed8

…nning. Streaming also planned footsteps and CoM

removed m_replanningTriggerPort. Not concerned by these changes

2acaf82

SimoneMic mentioned this pull request Apr 12, 2023

Walking-controller expanded with navigation features #142

Open

S-Dafarra requested changes Apr 12, 2023

View reviewed changes

SimoneMic added 2 commits April 18, 2023 17:10

main update NavigationHelper

4742c72

added config files

ece926f

Merge branch 'robotology:master' into odom_stream

34c958b

SimoneMic requested a review from GiulioRomualdi as a code owner June 23, 2023 09:26

SimoneMic added 2 commits June 23, 2023 09:33

config ergoCubSN000 update

01ba7bc

Merge branch 'odom_stream' of https://github.com/SimoneMic/walking-co…

d611ea0

…ntrollers into odom_stream

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Streaming odometry info, CoM and footsteps #145

Streaming odometry info, CoM and footsteps #145

SimoneMic commented Apr 12, 2023

S-Dafarra left a comment

S-Dafarra Apr 12, 2023

S-Dafarra Apr 12, 2023

S-Dafarra Apr 12, 2023

S-Dafarra Apr 12, 2023

S-Dafarra Apr 12, 2023

S-Dafarra Apr 12, 2023

SimoneMic commented Apr 27, 2023

S-Dafarra commented Apr 27, 2023

		# Loop rate for the thread computing the navigation trigger
		navigationTriggerLoopRate 100

Streaming odometry info, CoM and footsteps #145

Are you sure you want to change the base?

Streaming odometry info, CoM and footsteps #145

Conversation

SimoneMic commented Apr 12, 2023

S-Dafarra left a comment

Choose a reason for hiding this comment

S-Dafarra Apr 12, 2023

Choose a reason for hiding this comment

S-Dafarra Apr 12, 2023

Choose a reason for hiding this comment

S-Dafarra Apr 12, 2023

Choose a reason for hiding this comment

S-Dafarra Apr 12, 2023

Choose a reason for hiding this comment

S-Dafarra Apr 12, 2023

Choose a reason for hiding this comment

S-Dafarra Apr 12, 2023

Choose a reason for hiding this comment

SimoneMic commented Apr 27, 2023

S-Dafarra commented Apr 27, 2023