pr2_arm_ik.cpp

Go to the documentation of this file.

#include <reactive_grasping_pr2/pr2_arm_ik.h>

#include <cmath>
#include <algorithm>

namespace pr2_arm_ik {
        
        static VectorT goal;
        static Obstacles obstacles;

        // constants for weighting the different costs
        static float h = 0.5f;
        static float c_b   = 0.0007f;
        static float c_p   = 0.0003f;
        static float c_u   = 0.1f;
        static float sigma = 0.01f;
        static float sigma_p = 0.005f;
        static float c_xT = 0.04f;

        void setGoal(const VectorT &g) {
                goal = g;
        }

        void setObstacles(const Obstacles &obs) {
                obstacles = obs;
        }

        Obstacles &getObstacles() {
                return obstacles;
        }

        void dynamics(const VectorT &x, const VectorT &u, VectorT &xNext) {
                //int n = x.size();
                int m = u.size();
                xNext.setZero(x.size());
                for (int i = 0; i+m <= x.size(); i+=m) {
                        for (int j = 0; j < m; ++j)
                                xNext(i+j) = x(i+j) + u(j);
                }
        }

        void dynamicsD(const VectorT &x, const VectorT &u, Deriv &d) {
                int n = x.size();
                int m = u.size();
        
                d.fx.resize(n, n);
                d.fx.setIdentity();
        
                d.fu.setZero(n, m);
                for(int i = 0; i < n; ++i) {
                        int pos = int(i % m);
                        d.fu(i, pos) = 1.;
                }
        
                d.fxx.resize(n);
                d.fxu.resize(n);
                d.fuu.resize(n);
                for (int i = 0; i < n; ++i) {
                        d.fxx[i].setZero(n,n);
                        d.fxu[i].setZero(n,m);
                        d.fuu[i].setZero(m,m);
                }
        }

        float cost(const VectorT &x, const VectorT &u) {
                int m = u.size();
                float goalCost = c_xT * (x.head(m) - goal).squaredNorm(); 
                float obstacleSum = 0.;
                float partSum = 0.;
                // calculate obstacle cost
                for (int i = 0; i < obstacles.size(); ++i) {
                        obstacleSum += std::exp((-1.f / sigma) * (x.head(m) - obstacles[i]).squaredNorm());
                }
                // also make sure none of the parts collide with the goal
                for (int i = 0; i+m <= x.size(); i+=m) {
                        partSum += std::exp((-1.f / sigma_p) * (x.block(i, 0, m , 1)  - goal).squaredNorm());
                }
                // if (obstacleSum != 0.)
                //         std::cout <<  "obstacleSum " << obstacleSum << std::endl;
                float totalCost = goalCost + std::pow(c_u, h) * u.squaredNorm() + c_b * obstacleSum + c_p * partSum;
                return totalCost;
        }

        void costD(const VectorT &x, const VectorT &u, Deriv &d) { 
                int n = x.size();
                int m = u.size();

                d.cu = 2.f * std::pow(c_u, h) * u;

                d.cuu.setIdentity(m, m);
                d.cuu *= 2.f * std::pow(c_u, h);

                d.cx.setZero(n);
                d.cx.head(m) = 2.f * c_xT * (x.head(m) - goal);
                d.cxx.setIdentity(n, n);
                d.cxx *= 2.f * c_xT;

                // calculate obstacle derivative
                VectorT cx_obs;
                MatrixT cxx_obs;
                cx_obs.setZero(m);
                cxx_obs.setZero(m, m);
                for (int i = 0; i < obstacles.size(); ++i) {
                        VectorT tmp_diff = (x.head(m) - obstacles[i]);
                        MatrixT tmp_diff_outer = tmp_diff * tmp_diff.transpose();
                        cx_obs += (-2.f/sigma) * tmp_diff * std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                        //cxx_obs.array() +=  ((4. * tmp_diff_outer.array() - 2. * sigma) / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                        cxx_obs.array() +=  (4. * tmp_diff_outer.array());
                        cxx_obs.diagonal().array() -=  2. * sigma;
                        cxx_obs.array() = (cxx_obs.array() / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                }
                
                // calculate derivative of additional robot parts 
                VectorT cx_parts;
                MatrixT cxx_parts;
                cx_parts.setZero(x.size());
                cxx_parts.setZero(n, n);
                for (int i = m; i+m <= n; i+=m) {
                        VectorT tmp_diff = (x.block(i, 0, m, 1) - goal);
                        MatrixT tmp_diff_outer = tmp_diff * tmp_diff.transpose();
                        cx_parts.block(i, 0, m, 1)  += (-2.f/sigma_p) * tmp_diff * std::exp((-1.f / sigma_p) * tmp_diff.squaredNorm());
                        //cxx_parts.array() +=  ((4. * tmp_diff_outer.array() - 2. * sigma) / (sigma * sigma)) *  std::exp((-1.f / sigma) * tmp_diff.squaredNorm());
                        cxx_parts.block(i, i, m, m).array() +=  (4. * tmp_diff_outer.array());
                        cxx_parts.block(i, i, m, m).diagonal().array() -=  2. * sigma_p;
                        cxx_parts.block(i, i, m, m).array() = (cxx_parts.block(i, i, m, m).array() / (sigma_p * sigma_p)) *  std::exp((-1.f / sigma_p) * tmp_diff.squaredNorm());
                }
                
                d.cx.head(m) += c_b * cx_obs;
                d.cx += c_p * cx_parts;

                d.cxx.block(0, 0, m, m) += c_b * cxx_obs.block(0, 0, m, m);
                d.cxx += c_p * cxx_parts;

                d.cxu.setZero(n, m);
        }
}