From 1491ca586b909d721f645163446b98a0b643810d Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Wed, 28 Mar 2018 16:39:22 +0200 Subject: [PATCH 1/9] added some tutorials --- index.rst | 132 +++++++++++++++++++ tutos/Cmakelist.rst | 29 +++++ tutos/Energie_constraint.rst | 135 ++++++++++++++++++++ tutos/TowardsX_ray.rst | 53 ++++++++ tutos/Trajectory_generator_using_TRAXXS.rst | 70 ++++++++++ tutos/change_robot's_configuration.rst | 69 ++++++++++ tutos/gravity_regularization_task.rst | 82 ++++++++++++ 7 files changed, 570 insertions(+) create mode 100644 index.rst create mode 100644 tutos/Cmakelist.rst create mode 100644 tutos/Energie_constraint.rst create mode 100644 tutos/TowardsX_ray.rst create mode 100644 tutos/Trajectory_generator_using_TRAXXS.rst create mode 100644 tutos/change_robot's_configuration.rst create mode 100644 tutos/gravity_regularization_task.rst diff --git a/index.rst b/index.rst new file mode 100644 index 00000000..439b72d9 --- /dev/null +++ b/index.rst @@ -0,0 +1,132 @@ +.. image:: _static/orca-l.png + :width: 40px + :align: left + +ORCA Control +============ + +ORCA is a c++ whole-body reactive controller meant to compute the desired actuation torque +of a robot given some tasks to perform and some constraints. + +The problem is written as a **quadratic problem** : + +.. math:: + + \min_{x} \frac{1}{2}x^tHx + x^tg + + \text{subject to} + + lb \leq x \leq ub + + lb_A \leq Ax \leq ub_A + +* ``x`` the optimisation vector +* ``H`` the hessian matrix (:math:`size(x) \times size(x)`) +* ``g`` the gradient vector (:math:`size(x) \times 1`) +* ``A`` the constraint matrix (:math:`size(x) \times size(x)`) +* ``lb`` and ``ub`` the lower and upper bounds of ``x`` (:math:`size(x) \times 1`) +* ``lbA`` and ``ubA`` the lower and upper bounds of ``A`` (:math:`size(x) \times 1`) + +Tasks are written as **weighted euclidian distance function** : + +.. math:: + + w_{task} \lVert \mathbf{E}x + \mathbf{f} \rVert_{W_{norm}}^2 + +* ``x`` the optimisation vector, or **part** of the optimisation vector +* ``E`` the linear matrix of the affine function (:math:`size(x) \times size(x)`) +* ``f`` the origin vector (:math:`size(x) \times 1`) +* ``w task`` the weight of the tasks in the overall quadratic cost (scalar :math:`[0:1]`) +* ``W norm`` the weight of the euclidean norm (:math:`size(x) \times size(x)`) + +Given n_t tasks, the **overall cost function** is such that: + +.. math:: + + \frac{1}{2}x^tHx + x^tg = \frac{1}{2} \sum_{i=1}^{n_t} w_{task,i} \lVert \mathbf{E}_ix + \mathbf{f}_i \rVert_{W_{norm,i}}^2 + +Constraints are written as **double bounded linear function** : + +.. math:: + + lb_C \leq Cx \leq ub_C + +* ``C`` the constraint matrix (:math:`size(x) \times size(x)`) +* ``lbC`` and ``ubC`` the lower and upper bounds of ``A`` (:math:`size(x) \times 1`) + + +.. .. math:: + +.. \underset{n\times 1}{\mathrm{Y}} = \underset{n\times p}{X} \times +.. \underset{p\times 1}{\theta} + \underset{n\times 1}{\varepsilon} + +.. .. code-block:: c++ +.. +.. auto robot = std::make_shared(); +.. if(!robot->loadModelFromFile(urdf_url)) +.. { +.. return -1; +.. } +.. robot->setBaseFrame("link_0"); +.. robot->setGravity(Eigen::Vector3d(0,0,-9.81)); + +The remainder of the documentation describes "classical" tasks and cosntraints which +one may want to define + +.. toctree:: + :name: install + :caption: Installation and Configuration + :glob: + :maxdepth: 2 + + install/* + +.. toctree:: + :name: optim + :caption: Optim + :glob: + :maxdepth: 2 + + optim/* + +.. toctree:: + :name: tasks + :caption: Tasks + :glob: + :maxdepth: 2 + + tasks/* + +.. toctree:: + :name: constraints + :caption: Constraints + :glob: + :maxdepth: 2 + + cstrs/* + +.. toctree:: + :name: tools + :caption: Tools + :glob: + :maxdepth: 2 + + tools/* + +.. toctree:: + :name: tutos + :caption: Tutorials + :glob: + :maxdepth: 2 + + tutos/* + +.. image:: _static/isir.png + :width: 100px + :align: left + +.. image:: _static/cnrs.png + :width: 100px + +.. image:: _static/upmc.png + :width: 250px diff --git a/tutos/Cmakelist.rst b/tutos/Cmakelist.rst new file mode 100644 index 00000000..c2f05975 --- /dev/null +++ b/tutos/Cmakelist.rst @@ -0,0 +1,29 @@ +CmakeList for using Orca and Traxxs +-------------------------------------------------- + +.. code-block:: bash + + cmake_minimum_required(VERSION 3.1.0) + project("your_project") + + ########### orocos ############ + find_package(orocos_kdl REQUIRED) + ########### orca ############# + find_package(orca REQUIRED) + find_package(gazebo REQUIRED) + find_package(traxxs REQUIRED) + link_directories(${GAZEBO_LIBRARY_DIRS}) + + + include_directories( + ${orocos_kdl_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ) + + + # Warning pour gazebo 8 + list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS} -fext-numeric-literals") + + add_executable(demo0 demo0.cpp) + target_link_libraries(demo0 ${orocos_kdl_LIBRARIES} orca::orca ${GAZEBO_LIBRARIES} pthread tinyxml traxxs::traxxs traxxs::traxxs_softmotion) + install(TARGETS demo0 DESTINATION devel/lib) \ No newline at end of file diff --git a/tutos/Energie_constraint.rst b/tutos/Energie_constraint.rst new file mode 100644 index 00000000..42ebdab8 --- /dev/null +++ b/tutos/Energie_constraint.rst @@ -0,0 +1,135 @@ +Energy constraint +----------------- + + +This tutorial give an example to add a new constraint. We will add a constraint to robot's energy (for some security criterion). + +This class inherit GenericConstraint, and the **ControleVariable** associate is **JointSpaceTorque**. Remember the problem is written as a **quadratic problem**: + +.. math:: + + lb_C \leq Cx \leq ub_C + +So, we have to set the bounds and the constraint matrix, and those bounds and constraint are expressed explicitly by JointTorque. For this problem, we will create a new class, who is called **EnergyConstraint**. + + +.. code-block:: cpp + + class EnergyConstraint : public GenericConstraint + { + void updateConstraintFunction() + { + ... + + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; + + } + void resize() + { + ... + + constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf + constraintFunction().constraintMatrix()= C; + } + }; + +Now, let's see an complete example : + +.. code-block:: cpp + + class EnergyConstraint : public GenericConstraint + { + public: + EnergyConstraint() : GenericConstraint( ControlVariable::JointSpaceTorque ) + { + } + + void setAccelerationDes(Eigen::VectorXd Acc) + { + AccelerationDes_ = Acc ; + } + + void updateConstraintFunction() + { + MutexLock lock(mutex); + auto world = gazebo::physics::get_world(); + double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6; + const double n_horizon_steps(1); + double horizon_temps = n_horizon_steps * sim_step_dt ; + + ... + // Compute the current energy of end-effector + double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr; + double ec_next(0.); + ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0]; + ... + Eigen::VectorXd ec_max(1),ec_min(1); + ec_min << -100 - ec_next ; + + ec_max << ec_lim - ec_next ; + + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; + + } + + void resize() + { + MutexLock lock(mutex); + const int dim = OptimisationVector().getSize(getControlVariable()); + + ... + ... + C = delta_X.transpose() * Lambda_ * J * Masse.inverse(); + + constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf + constraintFunction().constraintMatrix()= C; + } + + + protected: + + Eigen::Matrix AccelerationDes_; + }; + +The function ``resize ()`` set the new dimension of constraint matrix and bounds with respect to **JointTorque**'s dimension, using ``dim = OptimisationVector().getSize(getControlVariable());``, also, it set the value of constraint matrix. +The function ``setAccelerationDes(Eigen::VectorXd Acc)`` return acceleration desired who is used in the function ``updateConstraintFunction()`` for compute the bounds lb_C et ub_C , +In the main() code: + +.. code-block:: cpp + + int main(int argc, char** argv) + { + ... + + + EnergyConstraint energycstr; + energycstr.setRobotModel(robot); + Eigen::VectorXd Accelerationdes; + Accelerationdes.resize(6); + Accelerationdes = cart_acc_pid.getCommand(); + energycstr.setAccelerationDes(Accelerationdes); + energycstr.resize(); + energycstr.updateConstraintFunction(); + + ... + energycstr.insertInProblem(); + energycstr.activate(); + + ... + + while(i++ <= 3000000) + { + ... + energycstr.insertInProblem(); + energycstr.update(); + + ...} + + ... + + return 0 + } + +You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints. See the complete code with "orca-pu-gazebo.cc" in examples. diff --git a/tutos/TowardsX_ray.rst b/tutos/TowardsX_ray.rst new file mode 100644 index 00000000..e0dd9ebe --- /dev/null +++ b/tutos/TowardsX_ray.rst @@ -0,0 +1,53 @@ +End-effector pointing +---------------------------- + +This tutorials show you how to define the rigth rotation matrix to pointing an target. This section referenced the work of Lucas Joseph, "**Towards X-ray medical imaging with robots in the open: +safety without compromising performances**" + +We know that the vector bewteen end-effector and the target can be computed by : + +.. math:: + + z_o^{des} = \frac{X_T - Xp}{||X_T - X_p||}. + +* ``X_T`` the position of end-effector expressed in base frame. +* ``X_p`` the position of target expressed in base frame. + +.. code-block:: cpp + + void trajectoire(Eigen::Affine3d &cart_pos_ref,int choix, Eigen::Matrix4d T, Eigen::Vector3d Pos_des, Eigen::Matrix3d Rot_des) + { + + Eigen::Matrix3d rot, rot_base; + rot_base = Eigen::AngleAxisd(3.14, Eigen::Vector3d::UnitX()); + + rot =rot_base* Rot_des; + cart_pos_ref.translation() = Pos_des; + cart_pos_ref.linear() = rot; + + } +Initially, the frame of end-effector is turned by 180° respect to axis x. So, we have to premultiplying the desired rotation by the base rotation. This function ``trajectoire(...)`` compute the desired orientation and position for cartesien acceleration task. + +.. code-block:: cpp + + int main() + { + + ... + Pose_laser << 1.,0,0; + Pose_ee = T.block(0,3,3,1); + Z_des = (-Pose_ee + Pose_laser) ; + Z_des = Z_des.normalized(); + Z_des_P = (T.block(0,0,3,3).inverse())*Z_des ; + Z_des_P=Z_des_P.normalized(); + axe_rot = Z_des_P.cross(Z_P); + axe_rot = axe_rot.normalized(); + angle_rot = acos(Z_des_P.dot(Z_P)); + Rot_des =Eigen::AngleAxisd(-angle_rot, axe_rot); + ... + return 0 + } + +A rotation can just define by an angle and an axis of rotation. The difference bewteen the position vector of end-effector and the position of target gives us the vector Z desired, and we obtain Z_des expressed in end-effector frame by multiplying this vector by the matrix de transformation bewteen the base frame and end-effector frame. The axis of rotation is obtained by the cross product bewteen Z_des_P and Z of end-effector frame. +The angle of rotation is given by the dot product. +Finally, the function ``Eigen::AngleAxisd(-angle_rot, axe_rot); `` compute the desired rotation. \ No newline at end of file diff --git a/tutos/Trajectory_generator_using_TRAXXS.rst b/tutos/Trajectory_generator_using_TRAXXS.rst new file mode 100644 index 00000000..9e42a5df --- /dev/null +++ b/tutos/Trajectory_generator_using_TRAXXS.rst @@ -0,0 +1,70 @@ +Trajectory generator: Traxxs +-------------------------------------- + + +TRAXXS is a framework to create and manage trajectories. See the complete information in "https://github.com/akeolab/traxxs/tree/4a2baff9064df5b3b7f288a44c2189c909ac1fed". + + +This tutorials not show you how to use TRAXXS for generate a trajectory, because there is many example in the web of TRAXXS. Let's see how to use TRAXXS in ORCA. + + +.. code-block:: cpp + + template< class T > + using sptr = std::shared_ptr; + using namespace traxxs; + + int main(int argc, char** argv) + { + ... + + // Velocity, acceleration, and jeck bounds + path::PathBounds4d path_bounds; + path_bounds.dx << 0.4, 0.4, 0.4, 1.0; + path_bounds.ddx = 100.0 * path_bounds.dx; + path_bounds.j = 200.0 * path_bounds.ddx; + + std::vector< std::shared_ptr < path::PathSegment > > segments; // Declare the complete segment + segments = createMonSegments(path_bounds); // Create the segment via "createMonSegments" function and apply the bounds + + //Transform the path to a trajectory + auto trajectory = std::make_shared< trajectory::Trajectory >(); + if ( !trajectory->set< ArcTrajGenSoftMotion >( segments ) ) + return 1; + + trajectory::TrajectoryState state, state_new; + + // the status of the tracker, the tracker is used to send the next position + tracker::TrackerStatus status; + auto validator = std::make_shared< tracker::TrackerSpaceValidatorPose4d >( 1e-2, 10 ); + tracker::TrackerSpacePursuit tracker( validator ); + //tracker::TrackerTimePursuit tracker; + tracker.reset( trajectory ); + if ( !trajectory->getState( 0.0, state) ) { + cerr << "Could not get state !\n"; + return 1; + } + + for ( unsigned int i = 0; i < 20000; ++i ) + { + + ... + status = tracker.next( dt, state, state_new ); + // if the tracker fails + if ( status == tracker::TrackerStatus::Error ) { + std::cerr << "Tracker error.\n"; + break; + } + // The next position is used in cartesian acceleration task. + ... + Pos_des=state_new.x.segment(0,3); + trajectoire(cart_pos_ref, 0,T,bottom,Pos_des); + ... + // if the trajectory is finished + if ( status == tracker::TrackerStatus::Finished ) + break; + } + ... + return 0 + } + \ No newline at end of file diff --git a/tutos/change_robot's_configuration.rst b/tutos/change_robot's_configuration.rst new file mode 100644 index 00000000..044a9a8e --- /dev/null +++ b/tutos/change_robot's_configuration.rst @@ -0,0 +1,69 @@ +Reconfigure your robot's initial state +-------------------------------------- + + + +.. code-block:: cpp + + + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + ... + + bool setModelConfiguration(gazebo::physics::ModelPtr gazebo_model , std::vector joint_names,std::vector joint_positions) + { + if (!gazebo_model) + { + std::cerr << "Model is not loaded" << std::endl; + return false; + } + + if (joint_names.size() != joint_positions.size()) + { + std::cerr << "joint_names lenght should be the same as joint_positions : " << joint_names.size() << " vs " << joint_positions.size() << std::endl; + return false; + } + + auto world = gazebo::physics::get_world(); + // make the service call to pause gazebo + bool is_paused = world->IsPaused(); + if (!is_paused) world->SetPaused(true); + + std::map joint_position_map; + for (unsigned int i = 0; i < joint_names.size(); i++) + { + joint_position_map[joint_names[i]] = joint_positions[i]; + } + + gazebo_model->SetJointPositions(joint_position_map); + + // resume paused state before this call + world->SetPaused(is_paused); + + return true; + } + + int main(int argc, char** argv) + { + ... + setModelConfiguration( gz_model , { "joint_0","joint_1" ,"joint_2", "joint_3","joint_4","joint_5","joint_6"} , {1.0,0.,0.0,-1.57,0.0,1.57,0.}); + ... + return 0 + + } + +The function ``SetModelConfiguration`` change robot's initial state. In main program, you specify the joint's name and the value of joint's initial position. +Here is a example who use Kuka lwr4+ with 7 joints. + +.. code-block :: cpp + + setModelConfiguration( gz_model , { "joint_0","joint_1" ,"joint_2", "joint_3","joint_4","joint_5","joint_6"} , {1.0,0.,0.0,-1.57,0.0,1.57,0.}); diff --git a/tutos/gravity_regularization_task.rst b/tutos/gravity_regularization_task.rst new file mode 100644 index 00000000..67a3c997 --- /dev/null +++ b/tutos/gravity_regularization_task.rst @@ -0,0 +1,82 @@ +Gravity regularization task +--------------------------- + +This section give an example to let u know how create a new task. the example here is gravity regularization task, this class is define before main program. + +.. code-block:: cpp + + class GravityrRegularisationTask : public RegularisationTask + { + + void updateAffineFunction() + { + + + Eigen::MatrixXd Massetot ; // Masstot is including the floating base + Eigen::VectorXd vel; // Gravity_torque_tot is including the floating base + + vel.setZero(6 + this->robot().getNrOfDegreesOfFreedom()); // initialize a vector of velocity include the floating base. + vel.tail(this->robot().getNrOfDegreesOfFreedom()) = this->robot().getJointVel(); + + Massetot = this->robot().getFreeFloatingMassMatrix(); + // set A and b + EuclidianNorm().b() =-2* Massetot.inverse() * (this->robot(). generalizedBiasForces() - vel); + EuclidianNorm().A() = 2*Massetot.inverse(); + } + + }; + + +In orca control section, we said that the tasks are written as **weighted euclidian distance function**: + +.. math:: + + w_{task} \lVert \mathbf{E}x + \mathbf{f} \rVert_{W_{norm}}^2 + + +For this class, we not use all optimization vector, but only variables corresponds to Floating base joint torque(6*1) and joint space joint torque(ndof*1). +using ``this->robot().`` allow you to get all robot's parameters, for example, ``this->robot().generalizedBiasForces()`` return force include gravity torque and Coriolis term +see more detail in ``RobotDynTree.cc``. + +As the task is expressed by a linear form, we have to specify the term **E** and **f** associate. They are called **A** and **b** here and redefine by: + +.. code-block:: cpp + + EuclidianNorm().b() = ... + EuclidianNorm().A() = ... + +Finally, in main() code, we have to declare this class, update it, insert it in the problem and active it, the function ``setWeight()`` represente how much this task is important respect to other task. : + +.. code-block:: cpp + + int main(int argc, char** argv) + { + ... + + + GravityrRegularisationTask trq_reg_task; + trq_reg_task.setRobotModel(robot); + trq_reg_task.setName("gravity_reg_task"); + trq_reg_task.setWeight(1E-4); + trq_reg_task.update(); + + ... + trq_reg_task.insertInProblem(); + trq_reg_task.activate(); + + ... + + while(i++ <= 3000000) + { + ... + + trq_reg_task.update(); + + ...} + + ... + + return 0 + } + +See the complete code with "orca-pu-gazebo.cc" in examples. \ No newline at end of file From 62513d074485747687b68f251c20ba274872d8a5 Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Wed, 28 Mar 2018 16:40:38 +0200 Subject: [PATCH 2/9] Delete index.rst --- index.rst | 132 ------------------------------------------------------ 1 file changed, 132 deletions(-) delete mode 100644 index.rst diff --git a/index.rst b/index.rst deleted file mode 100644 index 439b72d9..00000000 --- a/index.rst +++ /dev/null @@ -1,132 +0,0 @@ -.. image:: _static/orca-l.png - :width: 40px - :align: left - -ORCA Control -============ - -ORCA is a c++ whole-body reactive controller meant to compute the desired actuation torque -of a robot given some tasks to perform and some constraints. - -The problem is written as a **quadratic problem** : - -.. math:: - - \min_{x} \frac{1}{2}x^tHx + x^tg - - \text{subject to} - - lb \leq x \leq ub - - lb_A \leq Ax \leq ub_A - -* ``x`` the optimisation vector -* ``H`` the hessian matrix (:math:`size(x) \times size(x)`) -* ``g`` the gradient vector (:math:`size(x) \times 1`) -* ``A`` the constraint matrix (:math:`size(x) \times size(x)`) -* ``lb`` and ``ub`` the lower and upper bounds of ``x`` (:math:`size(x) \times 1`) -* ``lbA`` and ``ubA`` the lower and upper bounds of ``A`` (:math:`size(x) \times 1`) - -Tasks are written as **weighted euclidian distance function** : - -.. math:: - - w_{task} \lVert \mathbf{E}x + \mathbf{f} \rVert_{W_{norm}}^2 - -* ``x`` the optimisation vector, or **part** of the optimisation vector -* ``E`` the linear matrix of the affine function (:math:`size(x) \times size(x)`) -* ``f`` the origin vector (:math:`size(x) \times 1`) -* ``w task`` the weight of the tasks in the overall quadratic cost (scalar :math:`[0:1]`) -* ``W norm`` the weight of the euclidean norm (:math:`size(x) \times size(x)`) - -Given n_t tasks, the **overall cost function** is such that: - -.. math:: - - \frac{1}{2}x^tHx + x^tg = \frac{1}{2} \sum_{i=1}^{n_t} w_{task,i} \lVert \mathbf{E}_ix + \mathbf{f}_i \rVert_{W_{norm,i}}^2 - -Constraints are written as **double bounded linear function** : - -.. math:: - - lb_C \leq Cx \leq ub_C - -* ``C`` the constraint matrix (:math:`size(x) \times size(x)`) -* ``lbC`` and ``ubC`` the lower and upper bounds of ``A`` (:math:`size(x) \times 1`) - - -.. .. math:: - -.. \underset{n\times 1}{\mathrm{Y}} = \underset{n\times p}{X} \times -.. \underset{p\times 1}{\theta} + \underset{n\times 1}{\varepsilon} - -.. .. code-block:: c++ -.. -.. auto robot = std::make_shared(); -.. if(!robot->loadModelFromFile(urdf_url)) -.. { -.. return -1; -.. } -.. robot->setBaseFrame("link_0"); -.. robot->setGravity(Eigen::Vector3d(0,0,-9.81)); - -The remainder of the documentation describes "classical" tasks and cosntraints which -one may want to define - -.. toctree:: - :name: install - :caption: Installation and Configuration - :glob: - :maxdepth: 2 - - install/* - -.. toctree:: - :name: optim - :caption: Optim - :glob: - :maxdepth: 2 - - optim/* - -.. toctree:: - :name: tasks - :caption: Tasks - :glob: - :maxdepth: 2 - - tasks/* - -.. toctree:: - :name: constraints - :caption: Constraints - :glob: - :maxdepth: 2 - - cstrs/* - -.. toctree:: - :name: tools - :caption: Tools - :glob: - :maxdepth: 2 - - tools/* - -.. toctree:: - :name: tutos - :caption: Tutorials - :glob: - :maxdepth: 2 - - tutos/* - -.. image:: _static/isir.png - :width: 100px - :align: left - -.. image:: _static/cnrs.png - :width: 100px - -.. image:: _static/upmc.png - :width: 250px From 60ca4963f93bf7ec734cb956a1cfe7646bfad814 Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Wed, 28 Mar 2018 16:42:51 +0200 Subject: [PATCH 3/9] Added some tutorials --- docs/source/index.rst | 10 +- docs/source/tutos/Cmakelist.rst | 29 ++++ docs/source/tutos/Energie_constraint.rst | 135 ++++++++++++++++++ docs/source/tutos/TowardsX_ray.rst | 53 +++++++ .../Trajectory_generator_using_TRAXXS.rst | 70 +++++++++ .../tutos/change_robot's_configuration.rst | 69 +++++++++ .../tutos/gravity_regularization_task.rst | 82 +++++++++++ 7 files changed, 447 insertions(+), 1 deletion(-) create mode 100644 docs/source/tutos/Cmakelist.rst create mode 100644 docs/source/tutos/Energie_constraint.rst create mode 100644 docs/source/tutos/TowardsX_ray.rst create mode 100644 docs/source/tutos/Trajectory_generator_using_TRAXXS.rst create mode 100644 docs/source/tutos/change_robot's_configuration.rst create mode 100644 docs/source/tutos/gravity_regularization_task.rst diff --git a/docs/source/index.rst b/docs/source/index.rst index e3ccf042..439b72d9 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -112,7 +112,15 @@ one may want to define :maxdepth: 2 tools/* - + +.. toctree:: + :name: tutos + :caption: Tutorials + :glob: + :maxdepth: 2 + + tutos/* + .. image:: _static/isir.png :width: 100px :align: left diff --git a/docs/source/tutos/Cmakelist.rst b/docs/source/tutos/Cmakelist.rst new file mode 100644 index 00000000..c2f05975 --- /dev/null +++ b/docs/source/tutos/Cmakelist.rst @@ -0,0 +1,29 @@ +CmakeList for using Orca and Traxxs +-------------------------------------------------- + +.. code-block:: bash + + cmake_minimum_required(VERSION 3.1.0) + project("your_project") + + ########### orocos ############ + find_package(orocos_kdl REQUIRED) + ########### orca ############# + find_package(orca REQUIRED) + find_package(gazebo REQUIRED) + find_package(traxxs REQUIRED) + link_directories(${GAZEBO_LIBRARY_DIRS}) + + + include_directories( + ${orocos_kdl_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ) + + + # Warning pour gazebo 8 + list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS} -fext-numeric-literals") + + add_executable(demo0 demo0.cpp) + target_link_libraries(demo0 ${orocos_kdl_LIBRARIES} orca::orca ${GAZEBO_LIBRARIES} pthread tinyxml traxxs::traxxs traxxs::traxxs_softmotion) + install(TARGETS demo0 DESTINATION devel/lib) \ No newline at end of file diff --git a/docs/source/tutos/Energie_constraint.rst b/docs/source/tutos/Energie_constraint.rst new file mode 100644 index 00000000..42ebdab8 --- /dev/null +++ b/docs/source/tutos/Energie_constraint.rst @@ -0,0 +1,135 @@ +Energy constraint +----------------- + + +This tutorial give an example to add a new constraint. We will add a constraint to robot's energy (for some security criterion). + +This class inherit GenericConstraint, and the **ControleVariable** associate is **JointSpaceTorque**. Remember the problem is written as a **quadratic problem**: + +.. math:: + + lb_C \leq Cx \leq ub_C + +So, we have to set the bounds and the constraint matrix, and those bounds and constraint are expressed explicitly by JointTorque. For this problem, we will create a new class, who is called **EnergyConstraint**. + + +.. code-block:: cpp + + class EnergyConstraint : public GenericConstraint + { + void updateConstraintFunction() + { + ... + + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; + + } + void resize() + { + ... + + constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf + constraintFunction().constraintMatrix()= C; + } + }; + +Now, let's see an complete example : + +.. code-block:: cpp + + class EnergyConstraint : public GenericConstraint + { + public: + EnergyConstraint() : GenericConstraint( ControlVariable::JointSpaceTorque ) + { + } + + void setAccelerationDes(Eigen::VectorXd Acc) + { + AccelerationDes_ = Acc ; + } + + void updateConstraintFunction() + { + MutexLock lock(mutex); + auto world = gazebo::physics::get_world(); + double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6; + const double n_horizon_steps(1); + double horizon_temps = n_horizon_steps * sim_step_dt ; + + ... + // Compute the current energy of end-effector + double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr; + double ec_next(0.); + ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0]; + ... + Eigen::VectorXd ec_max(1),ec_min(1); + ec_min << -100 - ec_next ; + + ec_max << ec_lim - ec_next ; + + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; + + } + + void resize() + { + MutexLock lock(mutex); + const int dim = OptimisationVector().getSize(getControlVariable()); + + ... + ... + C = delta_X.transpose() * Lambda_ * J * Masse.inverse(); + + constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf + constraintFunction().constraintMatrix()= C; + } + + + protected: + + Eigen::Matrix AccelerationDes_; + }; + +The function ``resize ()`` set the new dimension of constraint matrix and bounds with respect to **JointTorque**'s dimension, using ``dim = OptimisationVector().getSize(getControlVariable());``, also, it set the value of constraint matrix. +The function ``setAccelerationDes(Eigen::VectorXd Acc)`` return acceleration desired who is used in the function ``updateConstraintFunction()`` for compute the bounds lb_C et ub_C , +In the main() code: + +.. code-block:: cpp + + int main(int argc, char** argv) + { + ... + + + EnergyConstraint energycstr; + energycstr.setRobotModel(robot); + Eigen::VectorXd Accelerationdes; + Accelerationdes.resize(6); + Accelerationdes = cart_acc_pid.getCommand(); + energycstr.setAccelerationDes(Accelerationdes); + energycstr.resize(); + energycstr.updateConstraintFunction(); + + ... + energycstr.insertInProblem(); + energycstr.activate(); + + ... + + while(i++ <= 3000000) + { + ... + energycstr.insertInProblem(); + energycstr.update(); + + ...} + + ... + + return 0 + } + +You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints. See the complete code with "orca-pu-gazebo.cc" in examples. diff --git a/docs/source/tutos/TowardsX_ray.rst b/docs/source/tutos/TowardsX_ray.rst new file mode 100644 index 00000000..e0dd9ebe --- /dev/null +++ b/docs/source/tutos/TowardsX_ray.rst @@ -0,0 +1,53 @@ +End-effector pointing +---------------------------- + +This tutorials show you how to define the rigth rotation matrix to pointing an target. This section referenced the work of Lucas Joseph, "**Towards X-ray medical imaging with robots in the open: +safety without compromising performances**" + +We know that the vector bewteen end-effector and the target can be computed by : + +.. math:: + + z_o^{des} = \frac{X_T - Xp}{||X_T - X_p||}. + +* ``X_T`` the position of end-effector expressed in base frame. +* ``X_p`` the position of target expressed in base frame. + +.. code-block:: cpp + + void trajectoire(Eigen::Affine3d &cart_pos_ref,int choix, Eigen::Matrix4d T, Eigen::Vector3d Pos_des, Eigen::Matrix3d Rot_des) + { + + Eigen::Matrix3d rot, rot_base; + rot_base = Eigen::AngleAxisd(3.14, Eigen::Vector3d::UnitX()); + + rot =rot_base* Rot_des; + cart_pos_ref.translation() = Pos_des; + cart_pos_ref.linear() = rot; + + } +Initially, the frame of end-effector is turned by 180° respect to axis x. So, we have to premultiplying the desired rotation by the base rotation. This function ``trajectoire(...)`` compute the desired orientation and position for cartesien acceleration task. + +.. code-block:: cpp + + int main() + { + + ... + Pose_laser << 1.,0,0; + Pose_ee = T.block(0,3,3,1); + Z_des = (-Pose_ee + Pose_laser) ; + Z_des = Z_des.normalized(); + Z_des_P = (T.block(0,0,3,3).inverse())*Z_des ; + Z_des_P=Z_des_P.normalized(); + axe_rot = Z_des_P.cross(Z_P); + axe_rot = axe_rot.normalized(); + angle_rot = acos(Z_des_P.dot(Z_P)); + Rot_des =Eigen::AngleAxisd(-angle_rot, axe_rot); + ... + return 0 + } + +A rotation can just define by an angle and an axis of rotation. The difference bewteen the position vector of end-effector and the position of target gives us the vector Z desired, and we obtain Z_des expressed in end-effector frame by multiplying this vector by the matrix de transformation bewteen the base frame and end-effector frame. The axis of rotation is obtained by the cross product bewteen Z_des_P and Z of end-effector frame. +The angle of rotation is given by the dot product. +Finally, the function ``Eigen::AngleAxisd(-angle_rot, axe_rot); `` compute the desired rotation. \ No newline at end of file diff --git a/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst b/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst new file mode 100644 index 00000000..9e42a5df --- /dev/null +++ b/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst @@ -0,0 +1,70 @@ +Trajectory generator: Traxxs +-------------------------------------- + + +TRAXXS is a framework to create and manage trajectories. See the complete information in "https://github.com/akeolab/traxxs/tree/4a2baff9064df5b3b7f288a44c2189c909ac1fed". + + +This tutorials not show you how to use TRAXXS for generate a trajectory, because there is many example in the web of TRAXXS. Let's see how to use TRAXXS in ORCA. + + +.. code-block:: cpp + + template< class T > + using sptr = std::shared_ptr; + using namespace traxxs; + + int main(int argc, char** argv) + { + ... + + // Velocity, acceleration, and jeck bounds + path::PathBounds4d path_bounds; + path_bounds.dx << 0.4, 0.4, 0.4, 1.0; + path_bounds.ddx = 100.0 * path_bounds.dx; + path_bounds.j = 200.0 * path_bounds.ddx; + + std::vector< std::shared_ptr < path::PathSegment > > segments; // Declare the complete segment + segments = createMonSegments(path_bounds); // Create the segment via "createMonSegments" function and apply the bounds + + //Transform the path to a trajectory + auto trajectory = std::make_shared< trajectory::Trajectory >(); + if ( !trajectory->set< ArcTrajGenSoftMotion >( segments ) ) + return 1; + + trajectory::TrajectoryState state, state_new; + + // the status of the tracker, the tracker is used to send the next position + tracker::TrackerStatus status; + auto validator = std::make_shared< tracker::TrackerSpaceValidatorPose4d >( 1e-2, 10 ); + tracker::TrackerSpacePursuit tracker( validator ); + //tracker::TrackerTimePursuit tracker; + tracker.reset( trajectory ); + if ( !trajectory->getState( 0.0, state) ) { + cerr << "Could not get state !\n"; + return 1; + } + + for ( unsigned int i = 0; i < 20000; ++i ) + { + + ... + status = tracker.next( dt, state, state_new ); + // if the tracker fails + if ( status == tracker::TrackerStatus::Error ) { + std::cerr << "Tracker error.\n"; + break; + } + // The next position is used in cartesian acceleration task. + ... + Pos_des=state_new.x.segment(0,3); + trajectoire(cart_pos_ref, 0,T,bottom,Pos_des); + ... + // if the trajectory is finished + if ( status == tracker::TrackerStatus::Finished ) + break; + } + ... + return 0 + } + \ No newline at end of file diff --git a/docs/source/tutos/change_robot's_configuration.rst b/docs/source/tutos/change_robot's_configuration.rst new file mode 100644 index 00000000..044a9a8e --- /dev/null +++ b/docs/source/tutos/change_robot's_configuration.rst @@ -0,0 +1,69 @@ +Reconfigure your robot's initial state +-------------------------------------- + + + +.. code-block:: cpp + + + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + ... + + bool setModelConfiguration(gazebo::physics::ModelPtr gazebo_model , std::vector joint_names,std::vector joint_positions) + { + if (!gazebo_model) + { + std::cerr << "Model is not loaded" << std::endl; + return false; + } + + if (joint_names.size() != joint_positions.size()) + { + std::cerr << "joint_names lenght should be the same as joint_positions : " << joint_names.size() << " vs " << joint_positions.size() << std::endl; + return false; + } + + auto world = gazebo::physics::get_world(); + // make the service call to pause gazebo + bool is_paused = world->IsPaused(); + if (!is_paused) world->SetPaused(true); + + std::map joint_position_map; + for (unsigned int i = 0; i < joint_names.size(); i++) + { + joint_position_map[joint_names[i]] = joint_positions[i]; + } + + gazebo_model->SetJointPositions(joint_position_map); + + // resume paused state before this call + world->SetPaused(is_paused); + + return true; + } + + int main(int argc, char** argv) + { + ... + setModelConfiguration( gz_model , { "joint_0","joint_1" ,"joint_2", "joint_3","joint_4","joint_5","joint_6"} , {1.0,0.,0.0,-1.57,0.0,1.57,0.}); + ... + return 0 + + } + +The function ``SetModelConfiguration`` change robot's initial state. In main program, you specify the joint's name and the value of joint's initial position. +Here is a example who use Kuka lwr4+ with 7 joints. + +.. code-block :: cpp + + setModelConfiguration( gz_model , { "joint_0","joint_1" ,"joint_2", "joint_3","joint_4","joint_5","joint_6"} , {1.0,0.,0.0,-1.57,0.0,1.57,0.}); diff --git a/docs/source/tutos/gravity_regularization_task.rst b/docs/source/tutos/gravity_regularization_task.rst new file mode 100644 index 00000000..67a3c997 --- /dev/null +++ b/docs/source/tutos/gravity_regularization_task.rst @@ -0,0 +1,82 @@ +Gravity regularization task +--------------------------- + +This section give an example to let u know how create a new task. the example here is gravity regularization task, this class is define before main program. + +.. code-block:: cpp + + class GravityrRegularisationTask : public RegularisationTask + { + + void updateAffineFunction() + { + + + Eigen::MatrixXd Massetot ; // Masstot is including the floating base + Eigen::VectorXd vel; // Gravity_torque_tot is including the floating base + + vel.setZero(6 + this->robot().getNrOfDegreesOfFreedom()); // initialize a vector of velocity include the floating base. + vel.tail(this->robot().getNrOfDegreesOfFreedom()) = this->robot().getJointVel(); + + Massetot = this->robot().getFreeFloatingMassMatrix(); + // set A and b + EuclidianNorm().b() =-2* Massetot.inverse() * (this->robot(). generalizedBiasForces() - vel); + EuclidianNorm().A() = 2*Massetot.inverse(); + } + + }; + + +In orca control section, we said that the tasks are written as **weighted euclidian distance function**: + +.. math:: + + w_{task} \lVert \mathbf{E}x + \mathbf{f} \rVert_{W_{norm}}^2 + + +For this class, we not use all optimization vector, but only variables corresponds to Floating base joint torque(6*1) and joint space joint torque(ndof*1). +using ``this->robot().`` allow you to get all robot's parameters, for example, ``this->robot().generalizedBiasForces()`` return force include gravity torque and Coriolis term +see more detail in ``RobotDynTree.cc``. + +As the task is expressed by a linear form, we have to specify the term **E** and **f** associate. They are called **A** and **b** here and redefine by: + +.. code-block:: cpp + + EuclidianNorm().b() = ... + EuclidianNorm().A() = ... + +Finally, in main() code, we have to declare this class, update it, insert it in the problem and active it, the function ``setWeight()`` represente how much this task is important respect to other task. : + +.. code-block:: cpp + + int main(int argc, char** argv) + { + ... + + + GravityrRegularisationTask trq_reg_task; + trq_reg_task.setRobotModel(robot); + trq_reg_task.setName("gravity_reg_task"); + trq_reg_task.setWeight(1E-4); + trq_reg_task.update(); + + ... + trq_reg_task.insertInProblem(); + trq_reg_task.activate(); + + ... + + while(i++ <= 3000000) + { + ... + + trq_reg_task.update(); + + ...} + + ... + + return 0 + } + +See the complete code with "orca-pu-gazebo.cc" in examples. \ No newline at end of file From 3d2e37d879cbf12204f7454b5a7eca067004b0cb Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Fri, 30 Mar 2018 08:50:10 +0200 Subject: [PATCH 4/9] Delete TowardsX_ray.rst --- docs/source/tutos/TowardsX_ray.rst | 53 ------------------------------ 1 file changed, 53 deletions(-) delete mode 100644 docs/source/tutos/TowardsX_ray.rst diff --git a/docs/source/tutos/TowardsX_ray.rst b/docs/source/tutos/TowardsX_ray.rst deleted file mode 100644 index e0dd9ebe..00000000 --- a/docs/source/tutos/TowardsX_ray.rst +++ /dev/null @@ -1,53 +0,0 @@ -End-effector pointing ----------------------------- - -This tutorials show you how to define the rigth rotation matrix to pointing an target. This section referenced the work of Lucas Joseph, "**Towards X-ray medical imaging with robots in the open: -safety without compromising performances**" - -We know that the vector bewteen end-effector and the target can be computed by : - -.. math:: - - z_o^{des} = \frac{X_T - Xp}{||X_T - X_p||}. - -* ``X_T`` the position of end-effector expressed in base frame. -* ``X_p`` the position of target expressed in base frame. - -.. code-block:: cpp - - void trajectoire(Eigen::Affine3d &cart_pos_ref,int choix, Eigen::Matrix4d T, Eigen::Vector3d Pos_des, Eigen::Matrix3d Rot_des) - { - - Eigen::Matrix3d rot, rot_base; - rot_base = Eigen::AngleAxisd(3.14, Eigen::Vector3d::UnitX()); - - rot =rot_base* Rot_des; - cart_pos_ref.translation() = Pos_des; - cart_pos_ref.linear() = rot; - - } -Initially, the frame of end-effector is turned by 180° respect to axis x. So, we have to premultiplying the desired rotation by the base rotation. This function ``trajectoire(...)`` compute the desired orientation and position for cartesien acceleration task. - -.. code-block:: cpp - - int main() - { - - ... - Pose_laser << 1.,0,0; - Pose_ee = T.block(0,3,3,1); - Z_des = (-Pose_ee + Pose_laser) ; - Z_des = Z_des.normalized(); - Z_des_P = (T.block(0,0,3,3).inverse())*Z_des ; - Z_des_P=Z_des_P.normalized(); - axe_rot = Z_des_P.cross(Z_P); - axe_rot = axe_rot.normalized(); - angle_rot = acos(Z_des_P.dot(Z_P)); - Rot_des =Eigen::AngleAxisd(-angle_rot, axe_rot); - ... - return 0 - } - -A rotation can just define by an angle and an axis of rotation. The difference bewteen the position vector of end-effector and the position of target gives us the vector Z desired, and we obtain Z_des expressed in end-effector frame by multiplying this vector by the matrix de transformation bewteen the base frame and end-effector frame. The axis of rotation is obtained by the cross product bewteen Z_des_P and Z of end-effector frame. -The angle of rotation is given by the dot product. -Finally, the function ``Eigen::AngleAxisd(-angle_rot, axe_rot); `` compute the desired rotation. \ No newline at end of file From d9f19a22ce4feb1505475bad09ce2743342cbe97 Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Fri, 20 Apr 2018 13:50:49 +0200 Subject: [PATCH 5/9] Delete Energie_constraint.rst --- docs/source/tutos/Energie_constraint.rst | 135 ----------------------- 1 file changed, 135 deletions(-) delete mode 100644 docs/source/tutos/Energie_constraint.rst diff --git a/docs/source/tutos/Energie_constraint.rst b/docs/source/tutos/Energie_constraint.rst deleted file mode 100644 index 42ebdab8..00000000 --- a/docs/source/tutos/Energie_constraint.rst +++ /dev/null @@ -1,135 +0,0 @@ -Energy constraint ------------------ - - -This tutorial give an example to add a new constraint. We will add a constraint to robot's energy (for some security criterion). - -This class inherit GenericConstraint, and the **ControleVariable** associate is **JointSpaceTorque**. Remember the problem is written as a **quadratic problem**: - -.. math:: - - lb_C \leq Cx \leq ub_C - -So, we have to set the bounds and the constraint matrix, and those bounds and constraint are expressed explicitly by JointTorque. For this problem, we will create a new class, who is called **EnergyConstraint**. - - -.. code-block:: cpp - - class EnergyConstraint : public GenericConstraint - { - void updateConstraintFunction() - { - ... - - constraintFunction().lowerBound() = ec_min; - constraintFunction().upperBound() = ec_max; - - } - void resize() - { - ... - - constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf - constraintFunction().constraintMatrix()= C; - } - }; - -Now, let's see an complete example : - -.. code-block:: cpp - - class EnergyConstraint : public GenericConstraint - { - public: - EnergyConstraint() : GenericConstraint( ControlVariable::JointSpaceTorque ) - { - } - - void setAccelerationDes(Eigen::VectorXd Acc) - { - AccelerationDes_ = Acc ; - } - - void updateConstraintFunction() - { - MutexLock lock(mutex); - auto world = gazebo::physics::get_world(); - double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6; - const double n_horizon_steps(1); - double horizon_temps = n_horizon_steps * sim_step_dt ; - - ... - // Compute the current energy of end-effector - double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr; - double ec_next(0.); - ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0]; - ... - Eigen::VectorXd ec_max(1),ec_min(1); - ec_min << -100 - ec_next ; - - ec_max << ec_lim - ec_next ; - - constraintFunction().lowerBound() = ec_min; - constraintFunction().upperBound() = ec_max; - - } - - void resize() - { - MutexLock lock(mutex); - const int dim = OptimisationVector().getSize(getControlVariable()); - - ... - ... - C = delta_X.transpose() * Lambda_ * J * Masse.inverse(); - - constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf - constraintFunction().constraintMatrix()= C; - } - - - protected: - - Eigen::Matrix AccelerationDes_; - }; - -The function ``resize ()`` set the new dimension of constraint matrix and bounds with respect to **JointTorque**'s dimension, using ``dim = OptimisationVector().getSize(getControlVariable());``, also, it set the value of constraint matrix. -The function ``setAccelerationDes(Eigen::VectorXd Acc)`` return acceleration desired who is used in the function ``updateConstraintFunction()`` for compute the bounds lb_C et ub_C , -In the main() code: - -.. code-block:: cpp - - int main(int argc, char** argv) - { - ... - - - EnergyConstraint energycstr; - energycstr.setRobotModel(robot); - Eigen::VectorXd Accelerationdes; - Accelerationdes.resize(6); - Accelerationdes = cart_acc_pid.getCommand(); - energycstr.setAccelerationDes(Accelerationdes); - energycstr.resize(); - energycstr.updateConstraintFunction(); - - ... - energycstr.insertInProblem(); - energycstr.activate(); - - ... - - while(i++ <= 3000000) - { - ... - energycstr.insertInProblem(); - energycstr.update(); - - ...} - - ... - - return 0 - } - -You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints. See the complete code with "orca-pu-gazebo.cc" in examples. From 236118c2c77ba0f610f279637b32af6560b40c9f Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Fri, 20 Apr 2018 13:51:26 +0200 Subject: [PATCH 6/9] Add files via upload --- docs/source/tutos/Energie_constraint.rst | 134 +++++++++++++++++++++++ 1 file changed, 134 insertions(+) create mode 100644 docs/source/tutos/Energie_constraint.rst diff --git a/docs/source/tutos/Energie_constraint.rst b/docs/source/tutos/Energie_constraint.rst new file mode 100644 index 00000000..4ca6d040 --- /dev/null +++ b/docs/source/tutos/Energie_constraint.rst @@ -0,0 +1,134 @@ +Energy constraint +----------------- + + +This tutorial provides an example of addition of a new constraint. The added constraint is related to the robot provisional kinetic energy (which has been recently introduced in https://hal.archives-ouvertes.fr/hal-01614508/en). + +This class inherits from GenericConstraint, and the associated **ControleVariable** is **JointSpaceTorque**. Remember the problem is written as a **quadratic problem**: + +.. math:: + + lb_C \leq Cx \leq ub_C + +for which we have to set the bounds and the constraint matrix. Those bounds and constraint are expressed explicitly as a function of joint torque. For this problem, we will create a new class, called **EnergyConstraint**. + +.. code-block:: cpp + + class EnergyConstraint : public GenericConstraint + { + void updateConstraintFunction() + { + ... + + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; + + } + void resize() + { + ... + + constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf + constraintFunction().constraintMatrix()= C; + } + }; + +Now, let's see an complete example : + +.. code-block:: cpp + + class EnergyConstraint : public GenericConstraint + { + public: + EnergyConstraint() : GenericConstraint( ControlVariable::JointSpaceTorque ) + { + } + + void setAccelerationDes(Eigen::VectorXd Acc) + { + AccelerationDes_ = Acc ; + } + + void updateConstraintFunction() + { + MutexLock lock(mutex); + auto world = gazebo::physics::get_world(); + double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6; + const double n_horizon_steps(1); + double horizon_temps = n_horizon_steps * sim_step_dt ; + + ... + // Compute the current energy of end-effector + double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr; + double ec_next(0.); + ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0]; + ... + Eigen::VectorXd ec_max(1),ec_min(1); + ec_min << -100 - ec_next ; + + ec_max << ec_lim - ec_next ; + + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; + + } + + void resize() + { + MutexLock lock(mutex); + const int dim = OptimisationVector().getSize(getControlVariable()); + + ... + ... + C = delta_X.transpose() * Lambda_ * J * Masse.inverse(); + + constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf + constraintFunction().constraintMatrix()= C; + } + + + protected: + + Eigen::Matrix AccelerationDes_; + }; + +The function ``resize ()`` set the new dimension of constraint matrix and bounds with respect to **JointTorque**'s dimension, using ``dim = OptimisationVector().getSize(getControlVariable());``, also, it set the value of constraint matrix. +The function ``setAccelerationDes(Eigen::VectorXd Acc)`` return acceleration desired who is used in the function ``updateConstraintFunction()`` for compute the bounds lb_C et ub_C , +In the main() code: + +.. code-block:: cpp + + int main(int argc, char** argv) + { + ... + + + EnergyConstraint energycstr; + energycstr.setRobotModel(robot); + Eigen::VectorXd Accelerationdes; + Accelerationdes.resize(6); + Accelerationdes = cart_acc_pid.getCommand(); + energycstr.setAccelerationDes(Accelerationdes); + energycstr.resize(); + energycstr.updateConstraintFunction(); + + ... + energycstr.insertInProblem(); + energycstr.activate(); + + ... + + while(i++ <= 3000000) + { + ... + energycstr.insertInProblem(); + energycstr.update(); + + ...} + + ... + + return 0 + } + +You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints. See the complete code with "orca-pu-gazebo.cc" in examples. From 9026b517ecf5656c344efe69c2267fea24aa2f02 Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Fri, 20 Apr 2018 13:54:55 +0200 Subject: [PATCH 7/9] Update Trajectory_generator_using_TRAXXS.rst --- .../Trajectory_generator_using_TRAXXS.rst | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst b/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst index 9e42a5df..660585c9 100644 --- a/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst +++ b/docs/source/tutos/Trajectory_generator_using_TRAXXS.rst @@ -10,6 +10,25 @@ This tutorials not show you how to use TRAXXS for generate a trajectory, because .. code-block:: cpp + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + #include + #include + #include "samples_helpers.hpp" + #include "samples_helpers_cart.hpp" + + + template< class T > using sptr = std::shared_ptr; using namespace traxxs; @@ -25,7 +44,7 @@ This tutorials not show you how to use TRAXXS for generate a trajectory, because path_bounds.j = 200.0 * path_bounds.ddx; std::vector< std::shared_ptr < path::PathSegment > > segments; // Declare the complete segment - segments = createMonSegments(path_bounds); // Create the segment via "createMonSegments" function and apply the bounds + segments = createMonSegments(path_bounds); // Create the segment via "createMonSegments" function and apply the bounds //Transform the path to a trajectory auto trajectory = std::make_shared< trajectory::Trajectory >(); @@ -67,4 +86,4 @@ This tutorials not show you how to use TRAXXS for generate a trajectory, because ... return 0 } - \ No newline at end of file + From f6d0d33a427ad950a3f17cb3bd490db486f1f860 Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Thu, 26 Apr 2018 13:54:29 +0200 Subject: [PATCH 8/9] Add files via upload --- docs/source/tutos/Energie_constraint.rst | 90 +++++++++++++++++++----- 1 file changed, 72 insertions(+), 18 deletions(-) diff --git a/docs/source/tutos/Energie_constraint.rst b/docs/source/tutos/Energie_constraint.rst index 4ca6d040..e8a16703 100644 --- a/docs/source/tutos/Energie_constraint.rst +++ b/docs/source/tutos/Energie_constraint.rst @@ -16,6 +16,21 @@ for which we have to set the bounds and the constraint matrix. Those bounds and class EnergyConstraint : public GenericConstraint { + public: + + + void stateInitialize() + { + // This function initialize and resize the attributes + ... + } + + void updateState() + { + // This function update all the attributes + ... + } + void updateConstraintFunction() { ... @@ -23,7 +38,7 @@ for which we have to set the bounds and the constraint matrix. Those bounds and constraintFunction().lowerBound() = ec_min; constraintFunction().upperBound() = ec_max; - } + } void resize() { ... @@ -31,6 +46,15 @@ for which we have to set the bounds and the constraint matrix. Those bounds and constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf constraintFunction().constraintMatrix()= C; } + + protected: + // Declare the attributes of this class + Eigen::VectorXd AccelerationDes_; + bool indice; + string name_of_cstr ; + + Eigen::MatrixXd Masse_, J_, J_transp_, Lambda_ ; + Eigen::VectorXd Xd_curr_, gravity_Torque_, jdotqdot_; }; Now, let's see an complete example : @@ -44,32 +68,58 @@ Now, let's see an complete example : { } + + void setAccelerationDes(Eigen::VectorXd Acc) { AccelerationDes_ = Acc ; } + void stateInitialize() + { + const int ndof_ = robot().getNrOfDegreesOfFreedom(); + Masse_.resize(ndof_,ndof_); + J_.resize(6,ndof_); + jdotqdot_.resize(6); + Lambda_.resize(6,6); + gravity_Torque_.resize(ndof_); + Xd_curr_.resize(6); + AccelerationDes_.resize(6); + } + + void updateState() + { + const int ndof_ = robot().getNrOfDegreesOfFreedom(); + + Masse_ = (robot().getFreeFloatingMassMatrix()).block(6,6,ndof_,ndof_); + J_ = (robot().getFrameFreeFloatingJacobian("link_7")).block(0,6,6,ndof_); + Lambda_ = (J_*(Masse_.inverse())*J_.transpose()).inverse(); + Xd_curr_ = robot().getFrameVel("link_7"); + gravity_Torque_ = (robot().generalizedBiasForces()).segment(6,ndof_); + jdotqdot_ = robot().getFrameBiasAcc("link_7"); + + } void updateConstraintFunction() { - MutexLock lock(mutex); - auto world = gazebo::physics::get_world(); - double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6; - const double n_horizon_steps(1); - double horizon_temps = n_horizon_steps * sim_step_dt ; + MutexLock lock(mutex); + auto world = gazebo::physics::get_world(); + double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6; + const double n_horizon_steps(1); + double horizon_temps = n_horizon_steps * sim_step_dt ; - ... - // Compute the current energy of end-effector - double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr; - double ec_next(0.); - ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0]; - ... - Eigen::VectorXd ec_max(1),ec_min(1); - ec_min << -100 - ec_next ; + ... + // Compute the current energy of end-effector + double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr; + double ec_next(0.); + ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0]; + ... + Eigen::VectorXd ec_max(1),ec_min(1); + ec_min << -100 - ec_next ; - ec_max << ec_lim - ec_next ; + ec_max << ec_lim - ec_next ; - constraintFunction().lowerBound() = ec_min; - constraintFunction().upperBound() = ec_max; + constraintFunction().lowerBound() = ec_min; + constraintFunction().upperBound() = ec_max; } @@ -108,11 +158,13 @@ In the main() code: Eigen::VectorXd Accelerationdes; Accelerationdes.resize(6); Accelerationdes = cart_acc_pid.getCommand(); + energycstr.stateInitialize(); + energycstr.updateState(); energycstr.setAccelerationDes(Accelerationdes); energycstr.resize(); energycstr.updateConstraintFunction(); - ... + energycstr.insertInProblem(); energycstr.activate(); @@ -122,6 +174,8 @@ In the main() code: { ... energycstr.insertInProblem(); + energycstr.updateState(); + energycstr.update(); ...} From 70ab084387cb21a8a18b2d8fb5ec85db6c02f56b Mon Sep 17 00:00:00 2001 From: ZzhengP <36111459+ZzhengP@users.noreply.github.com> Date: Mon, 14 May 2018 11:49:55 +0200 Subject: [PATCH 9/9] Add files via upload --- docs/source/tutos/Energie_constraint.rst | 2 +- .../tutos/gravity_regularization_task.rst | 21 +++++++++---------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/docs/source/tutos/Energie_constraint.rst b/docs/source/tutos/Energie_constraint.rst index e8a16703..00812a34 100644 --- a/docs/source/tutos/Energie_constraint.rst +++ b/docs/source/tutos/Energie_constraint.rst @@ -185,4 +185,4 @@ In the main() code: return 0 } -You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints. See the complete code with "orca-pu-gazebo.cc" in examples. +You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints. See the complete code in "orca-pu-constraint.hpp". (https://github.com/ZzhengP/Stage) diff --git a/docs/source/tutos/gravity_regularization_task.rst b/docs/source/tutos/gravity_regularization_task.rst index 67a3c997..ede0c93c 100644 --- a/docs/source/tutos/gravity_regularization_task.rst +++ b/docs/source/tutos/gravity_regularization_task.rst @@ -11,17 +11,16 @@ This section give an example to let u know how create a new task. the example he void updateAffineFunction() { - - Eigen::MatrixXd Massetot ; // Masstot is including the floating base - Eigen::VectorXd vel; // Gravity_torque_tot is including the floating base - - vel.setZero(6 + this->robot().getNrOfDegreesOfFreedom()); // initialize a vector of velocity include the floating base. - vel.tail(this->robot().getNrOfDegreesOfFreedom()) = this->robot().getJointVel(); - - Massetot = this->robot().getFreeFloatingMassMatrix(); + const unsigned int ndof = robot().getNrOfDegreesOfFreedom(); + + Eigen::VectorXd Gravity_torque; + Gravity_torque = this->robot(). generalizedBiasForces(); // set A and b - EuclidianNorm().b() =-2* Massetot.inverse() * (this->robot(). generalizedBiasForces() - vel); - EuclidianNorm().A() = 2*Massetot.inverse(); + Eigen::MatrixXd MatrixId; + MatrixId.resize(6+ndof,6+ndof); + MatrixId.setIdentity(); // Delaration of Matrix E + EuclidianNorm().b() = -0.5*Gravity_torque; + EuclidianNorm().A() = MatrixId; } }; @@ -79,4 +78,4 @@ Finally, in main() code, we have to declare this class, update it, insert it in return 0 } -See the complete code with "orca-pu-gazebo.cc" in examples. \ No newline at end of file +See the complete code in "orca-pu-regularisation.hpp". (https://github.com/ZzhengP/Stage) \ No newline at end of file