The ct_ros from ethz-adrl

CollisionCostTermCGG.hpp for online trajectory generation

Hi, now I am trying to use the code in /ct_ros_nodes/examples/HyA/HyANLOC_ObastacleAcc.cpp for online trajectory generation. So I need to change the target position for end effector as well as obstacle position online! Could you tell me how can I make it?
Now, I just tried:

       std::vector<NLOptConSolver::OptConProblem_t::CostFunctionPtr_t> costs = nloc.getCostFunctionInstances();
        for (int i = 0; i < nloc.getCostFunctionInstances().size(); i++)
        {
            costs[i]->getIntermediateTermById(intTermID)->updateReferenceState(x_d);
            costs[i]->getFinalTermById(term_quad_final_id)->updateReferenceState(x_d);
        }

but, then the nloc.solve() did not get the desired trajectory!

migration from setInputBoxConstraints and setStateBoxConstraints to setBoxConstraints

Hi,
Some executable example don't build with control-toolbox v3.0.2(HyANLOC_ObstacleAcc.cpp, HyANLOC_TaskSpaceVel.cpp, HyANLOC_TaskSpace.cpp, HyADMS.cpp and HyANLOC_TaskSpaceAcc.cpp),
They use setInputBoxConstraints() and setStateBoxConstraints() to set 'ContinuousOptConProblem' which do not exist. I used the following solution to make it build, is it correct ?

//HyQSimulator.cpp line 138
    std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> boxConstraints(
        new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());

    // input constraint bounds
    ct::core::ControlVector<control_dim> u_lb = -100 * ct::core::ControlVector<control_dim>::Ones();
    ct::core::ControlVector<control_dim> u_ub = 100 * ct::core::ControlVector<control_dim>::Ones();
    // input constraint terms
    std::shared_ptr<ct::optcon::ControlInputConstraint<state_dim, control_dim>> controlConstraint(
        new ct::optcon::ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub));
    controlConstraint->setName("ControlInputConstraint");
    
    // state constraint bounds
    ct::core::StateVector<state_dim> x_lb, x_ub;
    x_lb.head<njoints>() = -3.14 * Eigen::Matrix<double, njoints, 1>::Ones(); // lower bound on position
    x_ub.head<njoints>() =  3.14 * Eigen::Matrix<double, njoints, 1>::Ones(); // upper bound on position
    x_lb.tail<njoints>() = -100 * Eigen::Matrix<double, njoints, 1>::Ones(); // lower bound on velocity
    x_ub.tail<njoints>() =  100 * Eigen::Matrix<double, njoints, 1>::Ones(); // upper bound on velocity
    // state constraint terms
    std::shared_ptr<ct::optcon::StateConstraint<state_dim, control_dim>> stateConstraint(
        new ct::optcon::StateConstraint<state_dim, control_dim>(x_lb, x_ub));
    stateConstraint->setName("StateConstraint");
    // add and initialize constraint terms    
    boxConstraints->addIntermediateConstraint(controlConstraint, true);
    boxConstraints->addIntermediateConstraint(stateConstraint, true);
    boxConstraints->addTerminalConstraint(stateConstraint,true);
    boxConstraints->initialize();

ethz-adrl / ct_ros Goto Github PK

ct_ros's People

Contributors

Stargazers

Watchers

Forkers

ct_ros's Issues

CollisionCostTermCGG.hpp for online trajectory generation

migration from setInputBoxConstraints and setStateBoxConstraints to setBoxConstraints

Recommend Projects

React

Vue.js

Typescript

TensorFlow

Django

Laravel

D3

Recommend Topics

javascript

web

server

Machine learning

Visualization

Game

Recommend Org

Facebook

Microsoft

Google

Alibaba

D3

Tencent