advrhumanoids / cartesio_planning Goto Github PK

At the moment "ci/contact_name" is hardcoded in visualisation. Eventually we should use a "tf_prefix".

We should provide a way to distribute internally the MA57 linear solver which is used by default in the optimal contorl problem in cartesio_planning. WHat do you think is the best way @alaurenzi ?

When initializing the validity checker functions, there are two equal functions that checks the collisions/self-collisions. One is in validity_checker_context.cpp (make_collision_checker()) and one is in validity_checker_factory.cpp. The first one is used. Is there a reason why two functions were written? @alaurenzi @EnricoMingo

Every time a feasible joint state is found, it should be sent (once) to a topic, so that it can be connected to the planning_server

Hi all,

In validity_checker_factory.cpp, specifically in the lambda function of MakeCentroidalStaticsChecker(), cs_ros->publish() function make the planner dies after few call of the aferomentioned validity check. This function is used to update and publish the arrays in rviz that represent the contact forces.

Let's keep this conversation open until the bug is not found.

This to avoid that the goal_sampler can not be stopped. Started implementation in fruscio_demo branch.

The API should be similar to the robot state publisher of cartesian interface:

RobotMarkerPublisher(ModelInterface::ConstPtr model);

void publishMarkers(ros::Time time, std::vector<std::string> red_links);

or similar.

Let's keep the code documented (higher priority to header files, but implementations should contain short notes as well

• robot_viz.h header
• robot_viz.cpp implementation
• state_wrapper.h header
• state_wrapper.cpp implementation
• validity_predicate_aggregate.h header
• validity_predicate_aggregate.cpp implementation
• stability_detection.h header
• stability_detection.cpp implementation
• cartesio_ompl_planner.h header
• cartesio_ompl_planner.cpp implementation
• trajectory_interpolation.cpp header
• trajectory_interpolation.cpp implementation
• trajectory_interpolation.cpp header
• trajectory_interpolation.cpp implementation
• cartesian_constraint.h header
• cartesian_constraint.cpp implementation
• goal_sampler.h header
• goal_sampler.cpp implementation
• planner_executor.cpp implementation

So far, we moved the init() function from setContactLinks() and setContactRotationMatrix functions to a public class member function in order to minimize its number of calls and OpenSoT problem generation as well.

At the moment, the procedure should be:

• setContactLinks(),
• setOptimizeTorque(),
• setFrictionCoefficient(),
• init(),
• setContactRotationMatrices(),

If the init() is called after setting the rotation matrices, they will be re-initialized as identity matrices leading to an error. In my opinion this procedure is not intuitive and the init() function should be put again inside the setContactLinks(), setOptimizeTorque(), setFrictionCoefficien() functions.

For our first implementation, the service should just contain the name of the ros parameter containing the new manifold.

The one of CartesIO-control is overkill and contains several useless features. It should provide the following features:

• publish tf (optionally)
• pose reference topic
• pose reference services (new feature)

Traj viewer should expose an action or service to send the computed trajectory to the robot (using RobotInterface)

As title

Let's start with a service to modify the list of polygon points.

Hi everyone,
Me and @FrancescoRuscelli were using centroidal_statics stability check (branch devel of this repo) to verify some poses coming from CentroidalPlanner repo. A lot of them were rejected and going deeper into the analysis we found out, together with @alaurenzi and @EnricoMingo that there is an issue in OpenSoT when solving the dynamic_feasibility problem with friction cones (see g_1 vector from the logger).
We wait for your feedback.

The presence of an origin's offset between the world and the base_ling frames in the URDF creates inconsistencies between start/goal robots, planner TF and planning scene robot.

Hi there,
Just encountered a problem: when a plan is requested a second/third... time, it may happen that the trajectory (_planner->getSolutionPath()) remains the same, as the previous planned one. For example:

void OmplPlanner::setStartAndGoalStates(const Eigen::VectorXd& start,
                                        const Eigen::VectorXd& goal,
                                        const double threshold)

...
    _pdef->setStartAndGoalStates(ompl_start, ompl_goal, threshold);

    Eigen::VectorXd int_start, int_goal;
    _sw->getState(ompl_start.get(), int_start);
    _sw->getState(ompl_goal.get(), int_goal);

    std::cout << "start: " << int_start.transpose() << std::endl;
    std::cout << "goal: " << int_goal.transpose() << std::endl;

produces, as expected:

start:     1.26619    0.509053    -1.89093 3.08153e-06     0.74163     1.26619
goal:     0.895672       0.6943     -1.54941 -6.60662e-06     0.897883     0.895682

but, after solving:

_planner->setStartAndGoalStates(starting_q, planning_qgoal, _goal_thrs);
_planner->solve(_planning_time, _planner_type)

std::cout << "trajectory: " << std::endl;
for (const auto & it : _planner->getSolutionPath()) {
    std::cout << it.transpose() << std::endl;
}
std::cout << std::endl;

this prints:

trajectory: 
    0.871956     0.715716     -1.50945 -5.14212e-06      0.91714     0.871965
    1.26619    0.509052    -1.89093 3.08155e-06    0.741629     1.26619

So the last point is equal the given start, which is the (correct) trajectory but of the previous planner request.
This does not happen always, but often.

I found out that this seems to be solved easily explicity calling the clear methods in the OmplPlanner::setStartAndGoalStates method before the _pdef->setStartAndGoalStates():

// set start and goal
_pdef->clearStartStates();
_pdef->clearGoal();
_pdef->clearSolutionPaths();

(not sure if all of three are necessary, but _pdef->clearSolutionPaths(); for sure to solve my problem)

@lucarossini-iit

When solving with a PositionCartesianSolver, setting a task to the Com, lots of error like the one written in the title are shown (notice the lower c of 'com' in the error, while I set a "Com" task). Although these errors, the task is accomplished correctly.

The planner samples continuous-type joints in an infinite interval leading to difficulties of finding a solution in a finite-time.

@lucarossini-iit, digging out I found out that the problem is about how the error is computed:

bool PositionCartesianSolver::solve()
{
    // allocate variables
    Eigen::VectorXd q, qcurr, dq, error;

    // initial q
    _model->getJointPosition(qcurr);

    // initial cost
    getError(error);
    ...

Indeed, the q of the PositionCartesianSolver::_model is correctly in the final position (as shown by the yellow nspg marker).
But, getError looks among the error of the cartesian tasks inside the _task_map:

void PositionCartesianSolver::getError(Eigen::VectorXd& error) const
{
    error.setZero(_n_task);

    int error_idx = 0;
    for(auto pair : _task_map)
    {
        auto t = pair.second;
        t->update(*_model); // tbd: optimize

        error.segment(error_idx, t->size) = t->error;
        error_idx += t->size;
    }
}

This _task_map is the problem: in the t->update(*_model); // tbd: optimize:

void PositionCartesianSolver::CartesianTaskData::update(XBot::ModelInterface& model)
{
    J.setZero(size, model.getJointNum());
    error.setZero(size);

    /* If task was disabled, error and jacobian are zero */
    auto active = task->getActivationState();

    if(active == ActivationState::Disabled)
    {
        return;
    }


    /* Error computation */
    Eigen::Affine3d T, Tdes;
    ctask->getCurrentPose(T);
    ctask->getPoseReference(Tdes);

    std::cout << "CartesianTaskData current pose " << T.translation().transpose() << std::endl;
    std::cout << "CartesianTaskData desired pose " << Tdes.translation().transpose() << std::endl;
    ...

there is this ctask->getCurrentPose(T) that takes the pose of the real robot (which still needs to move), instead of the pose of the nspg-yellow robot, e.g.:

CartesianTaskData current pose  0.571033 0.0688122  0.118524
CartesianTaskData desired pose 0.379009 0.455405 0.132772

Indeed the error XXXXX of the issue title is exactly the error between the tcp of the real robot and the given goal.

So, it seems that the CartesianTaskData inside the _task_map are not using the correct model of the ci, but instead the real robot.

What do you think about creating the PlanningSceneWrapper object as a private member of planner_executor.cpp (as you did with the NodeHandle or ModelInterfacePtr) and pass it to the functions that require it? @alaurenzi @EnricoMingo