Component/aikido ros control

[Shushman]

An actionlib client which receives an aikido::trajectory and sends it to an actionlib server after appropriately converting it to a control_msgs::FollowJointTrajectory.action goal.

N.B - Currently builds but needs to be tested. The pull request was started to check if the logic is correct.

---

This change is

[coveralls]

Coverage decreased (-3.6%) to 84.821% when pulling 5600cda on component/aikido_ros_control into 4fa0a81 on master.

[mkoval]

Reviewed 8 of 8 files at r1.
Review status: all files reviewed at latest revision, 21 unresolved discussions.

---

aikido/include/aikido/util/KinBodyParser.h, line 2 [r1] (raw file):

#ifndef AIKIDO_UTIL_KINBODY_PARSER_H
#define AIKIDO_UTIL_KINBODY_PARSER_H

This file is really part of #102. We either need to merge that pull request first or strip this file from the pull request.

---

aikido/src/util/CMakeLists.txt, line 6 [r1] (raw file):

  VanDerCorput.cpp
  StepSequence.cpp
  KinBodyParser.cpp

This file is really part of #102. We either need to merge that pull request first or strip this change from this pull request.

---

aikido/src/util/KinBodyParser.cpp, line 1 [r1] (raw file):

#include <algorithm>

This file is really part of #102. We either need to merge that pull request first or strip this file from the pull request.

---

aikido_ros_control/notes.txt, line 9 [r1] (raw file):

4. Check that it is metaskeleton state space and dof names respect that ordering

5. SO2 is a single effective value

Please delete this file.

---

aikido_ros_control/include/aikido/ros_control/RosTrajectoryExecutor.hpp, line 18 [r1] (raw file):

namespace ros_control {

class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor

Missing docstring for this class.

---

aikido_ros_control/include/aikido/ros_control/RosTrajectoryExecutor.hpp, line 27 [r1] (raw file):

        std::string serverName,
        ros::NodeHandle node,
        double trajTimeStep);

Missing docstring for this constructor and (more important) its arguments.

---

aikido_ros_control/include/aikido/ros_control/RosTrajectoryExecutor.hpp, line 32 [r1] (raw file):

    std::future<void> execute(
        trajectory::TrajectoryPtr _traj) override;

Add // Documentation inherited. if you want to use the superclass' docstrings.

---

aikido_ros_control/include/aikido/ros_control/RosTrajectoryExecutor.hpp, line 36 [r1] (raw file):

private:

    typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajectoryClient;

Use using =aliases instead of typedefs.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 37 [r1] (raw file):

    if (mPeriod.count() <= 0)
        throw std::invalid_argument("Period must be positive.");

Missing error checking on trajTimeStep.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 71 [r1] (raw file):

    if(!space)
        throw std::invalid_argument("Trajectory does not operate in this Executor's"
            " MetaSkeletonStateSpace.");

Misleading error message because this only checks that space is a MetaSkeletonStateSpace, not this specific state space. I suggest changing this to "Trajectory is not defined in a MetaSkeletonStateSpace."

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 75 [r1] (raw file):

    auto metaSkeleton = space->getMetaSkeleton();

    control_msgs::FollowJointTrajectoryGoal traj_goal;

Add a TODO about optionally populating path_tolerance, goal_tolerance, and goal_time_tolerance.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 115 [r1] (raw file):

    auto subspace_rn = space->getSubspace<aikido::statespace::Rn>(0);
    auto subspace_so2 = space->getSubspace<aikido::statespace::SO2>(0);
    std::shared_ptr<aikido::statespace::StateSpace> subspace;

The MetaSkeletonStateSpace contains one subspace per Joint. This logic only checks the type of the first joint. You likely want to loop over getNumSubspaces() and repeat this for each subspace.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 129 [r1] (raw file):

        else
            auto subspace = subspace_so2;
    }

This else block is meaningless because the subspace variables immediately go out of scope.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 150 [r1] (raw file):

            auto state = subspace_so2->createState();
            mTraj->evaluate(time_val, state);
            double angle_value = subspace_so2->getAngle(state);

You have to be careful here to guarantee continuity. States in an SO2 state space are equivalent modulo 2 * M_PI. You need to pick the right multiple of getAngle() to avoid introducing a discontinuity in the trajectory. Note that this is more complicated than normalizing the angle to be within some range: producing a continuous trajectory may require an arbitrary amount of winding.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 161 [r1] (raw file):

            // Is this correct??????
            jtpoint.positions.assign(state_values, state_values+n_dims);

This should be okay.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 162 [r1] (raw file):

            // Is this correct??????
            jtpoint.positions.assign(state_values, state_values+n_dims);
        }

This also needs to loop over getNumSubspaces().

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 186 [r1] (raw file):

    }

    if(duration - time_steps*mTrajTimeStep > std::numeric_limits<double>::epsilon())

This contains a lot of duplicate code. I would prefer to decouple the logic in the following way:

1. generate a list of times at which to sample the trajectory, which always includes the end point
2. iterate over those times and evaluate the trajectory

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 237 [r1] (raw file):

    //Wait for Result - need a TIMEOUT parameter
    double timeout = 30.0;
    bool finished_before_timeout = mTrajClientPtr -> waitForResult(ros::Duration(timeout));

We shouldn't block on a result here. The execute function is intended to return immediately and return a std::future that allows the use to (optionally) wait for execution to finish. While making that change, I would like to migrate to the same spin() architecture as @gilwoolee did in #101. The key idea is to replace the internal thread with a public spin() method that the user is responsible for calling periodically.

There are two key advantages to this architecture: (1) it is possible to run with no extra threads by manually calling spin() in a loop that checks the status of the future and (2) it is possible to combine multiple spin() calls in one thread. This reduces lock contention, helps make execution more deterministic, and simplifies testing.

I believe this requires using ActionClient directly, instead of through the SimpleActionClient helper class. You will also want to construct the ActionClient with a custom CallbackQueue so we have low-level control over when it processes messages. Look at my implementation in muul to see how this machinery works.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 275 [r1] (raw file):

                startTime = system_clock::now();
                trajInExecution = true;
            }

I don't think this logic belongs in the condition variable's callback function. Can we move this to the function body?

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 293 [r1] (raw file):

        // CartesianProduct which inherits virtual StateSpace 
        auto space = std::dynamic_pointer_cast<
            MetaSkeletonStateSpace>(mTraj->getStateSpace());

Can we do this once, inside execute, and store the result of the cast in a member variable?

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 316 [r1] (raw file):

        } 

        std::this_thread::sleep_until(now + mPeriod);

Release lockSpin before sleeping here. Otherwise execute will deadlock when attempting to acquire the mutex.

Also, sleeping until now + mPeriod will consistently lag behind the desired update period. I suggest keeping track of the desired time of the next update cycle (e.g. next_update), sleep_until(next_time), and updating next_time += mPeriod. This alternate implementation will not drift over time.

---

Comments from Reviewable

[coveralls]

Changes Unknown when pulling b4723ab on component/aikido_ros_control into * on master*.

[Shushman]

Review status: all files reviewed at latest revision, 21 unresolved discussions, some commit checks failed.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 71 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Misleading error message because this only checks that space is a MetaSkeletonStateSpace, not this specific state space. I suggest changing this to "Trajectory is not defined in a MetaSkeletonStateSpace."

FYI the same message is in `aikido/control/KinematicSimulationTrajectoryExecutor.cpp`

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 115 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

The MetaSkeletonStateSpace contains one subspace per Joint. This logic only checks the type of the first joint. You likely want to loop over getNumSubspaces() and repeat this for each subspace.

But do I generate a separate trajectory for each subspace?

---

Comments from Reviewable

[mkoval]

Reviewed 2 of 2 files at r2.
Review status: all files reviewed at latest revision, 21 unresolved discussions, some commit checks failed.

---

aikido/src/util/CMakeLists.txt, line 6 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

This file is really part of #102. We either need to merge that pull request first or strip this change from this pull request.

You deleted these files below, so you should also remove them from this `CMakeLists.txt` file.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 71 [r1] (raw file):

Previously, Shushman (Shushman Choudhury) wrote…

FYI the same message is in aikido/control/KinematicSimulationTrajectoryExecutor.cpp

Do as I say, not as I do. 😆

We should make the same change in that code. You are welcome to do it now, or we can punt until a future pull request.

---

aikido_ros_control/src/RosTrajectoryExecutor.cpp, line 115 [r1] (raw file):

Previously, Shushman (Shushman Choudhury) wrote…

But do I generate a separate trajectory for each subspace?

The `Trajectory` is defined over the `MetaSkeletonStateSpace`, which means that `evaluate`ing it produces a state that contains values of _all_ joint in the `MetaSkeleton`. The only change that is necessary is to repeat this logic for reach sub-space and corresponding sub-state, instead of only the first one.

---

Comments from Reviewable

[mkoval]

Closing this in favor of #111.