<commit_before>//=======================================================================
// Copyright (c) 2014-2017 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#pragma once

#ifdef ETL_CUDNN_MODE

#include "etl/impl/cublas/cuda.hpp"
#include "etl/impl/cudnn/cudnn.hpp"

#endif

namespace etl {

namespace impl {

namespace cudnn {

#ifdef ETL_CUDNN_MODE

/*!
 * \brief Create a CUDNN pooling descriptor for the given input matrix
 * \return a cudnn_wrapper around a created CUDNN filter tensor
 */
inline cudnn_wrapper<cudnnPoolingDescriptor_t> create_pooling_descriptor(cudnnPoolingMode_t mode, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2){
    cudnnPoolingDescriptor_t pooling_desc;
    cudnn_check(cudnnCreatePoolingDescriptor(&pooling_desc));
    cudnn_check(cudnnSetPooling2dDescriptor(pooling_desc, mode, CUDNN_PROPAGATE_NAN, c1, c2, p1, p2, s1, s2));

    return cudnn_wrapper<cudnnPoolingDescriptor_t>{pooling_desc};
}

/*!
 * \brief Create a CUDNN pooling descriptor for the given input matrix
 * \return a cudnn_wrapper around a created CUDNN filter tensor
 */
inline cudnn_wrapper<cudnnPoolingDescriptor_t> create_pooling_descriptor(cudnnPoolingMode_t mode, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3){
    int c[] = {int(c1), int(c2), int(c3)};
    int s[] = {int(s1), int(s2), int(s3)};
    int p[] = {int(p1), int(p2), int(p3)};

    cudnnPoolingDescriptor_t pooling_desc;
    cudnn_check(cudnnCreatePoolingDescriptor(&pooling_desc));
    cudnn_check(cudnnSetPoolingNdDescriptor(pooling_desc, mode, CUDNN_PROPAGATE_NAN, 3, c, p, s));

    return cudnn_wrapper<cudnnPoolingDescriptor_t>{pooling_desc};
}

/*!
 * \brief Apply the functor on sub and store the result in m
 * \param sub The sub expression
 * \param m The storage matrix
 * \param c1 The first dimension pooling ratio
 * \param c2 The second dimension pooling ratio
 */
template <typename X, typename Y>
void pool_2d(cudnnPoolingMode_t mode, const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
    using type = std::remove_const_t<value_t<X>>;

    decltype(auto) handle = start_cudnn();

    auto pooling_desc = create_pooling_descriptor(mode, c1, c2, s1, s2, p1, p2);

    auto x_tensor = create_tensor(x);
    auto y_tensor = create_tensor(y);

    type alpha[] = {1.0f};
    type beta[] = {0.0f};

    // Allocate GPU memory, if necessary

    x.ensure_gpu_up_to_date();
    y.ensure_gpu_allocated();

    // Perform pooling

    cudnn_check(cudnnPoolingForward(handle.get(), *pooling_desc, alpha, *x_tensor, x.gpu_memory(), beta, *y_tensor, y.gpu_memory()));

    y.validate_gpu();
    y.invalidate_cpu();
}

/*!
 * \brief Apply the functor on sub and store the result in m
 * \param sub The sub expression
 * \param m The storage matrix
 * \param c1 The first dimension pooling ratio
 * \param c2 The second dimension pooling ratio
 */
template <typename X, typename Y>
void pool_3d(cudnnPoolingMode_t mode, const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
    using type = std::remove_const_t<value_t<X>>;

    decltype(auto) handle = start_cudnn();

    auto pooling_desc = create_pooling_descriptor(mode, c1, c2, c3, s1, s2, s3, p1, p2, p3);

    auto x_tensor = create_tensor_5d(x);
    auto y_tensor = create_tensor_5d(y);

    type alpha[] = {1.0f};
    type beta[] = {0.0f};

    // Allocate GPU memory, if necessary

    x.ensure_gpu_up_to_date();
    y.ensure_gpu_allocated();

    // Perform pooling

    cudnn_check(cudnnPoolingForward(handle.get(), *pooling_desc, alpha, *x_tensor, x.gpu_memory(), beta, *y_tensor, y.gpu_memory()));

    y.validate_gpu();
    y.invalidate_cpu();
}

/*!
 * \brief Functor for 2D Max Pooling
 */
struct max_pool_2d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        pool_2d(CUDNN_POOLING_MAX, x, y, c1, c2, s1, s2, p1, p2);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, s1, s2, p1, p2);
        }
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct avg_pool_2d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        pool_2d(CUDNN_POOLING_AVERAGE_COUNT_INCLUDE_PADDING, x, y, c1, c2, s1, s2, p1, p2);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, s1, s2, p1, p2);
        }
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct max_pool_3d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        pool_3d(CUDNN_POOLING_MAX, x, y, c1, c2, c3, s1, s2, s3, p1, p2, p3);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, c3, s1, s2, s3, p1, p2, p3);
        }
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct avg_pool_3d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        pool_3d(CUDNN_POOLING_AVERAGE_COUNT_INCLUDE_PADDING, x, y, c1, c2, c3, s1, s2, s3, p1, p2, p3);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, c3, s1, s2, s3, p1, p2, p3);
        }
    }
};

#else

//COVERAGE_EXCLUDE_BEGIN

//TODO

//COVERAGE_EXCLUDE_END

#endif

} //end of namespace cudnn

} //end of namespace impl

} //end of namespace etl
<commit_msg>Fix compatibility without CUDNN<commit_after>//=======================================================================
// Copyright (c) 2014-2017 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#pragma once

#ifdef ETL_CUDNN_MODE

#include "etl/impl/cublas/cuda.hpp"
#include "etl/impl/cudnn/cudnn.hpp"

#endif

namespace etl {

namespace impl {

namespace cudnn {

#ifdef ETL_CUDNN_MODE

/*!
 * \brief Create a CUDNN pooling descriptor for the given input matrix
 * \return a cudnn_wrapper around a created CUDNN filter tensor
 */
inline cudnn_wrapper<cudnnPoolingDescriptor_t> create_pooling_descriptor(cudnnPoolingMode_t mode, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2){
    cudnnPoolingDescriptor_t pooling_desc;
    cudnn_check(cudnnCreatePoolingDescriptor(&pooling_desc));
    cudnn_check(cudnnSetPooling2dDescriptor(pooling_desc, mode, CUDNN_PROPAGATE_NAN, c1, c2, p1, p2, s1, s2));

    return cudnn_wrapper<cudnnPoolingDescriptor_t>{pooling_desc};
}

/*!
 * \brief Create a CUDNN pooling descriptor for the given input matrix
 * \return a cudnn_wrapper around a created CUDNN filter tensor
 */
inline cudnn_wrapper<cudnnPoolingDescriptor_t> create_pooling_descriptor(cudnnPoolingMode_t mode, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3){
    int c[] = {int(c1), int(c2), int(c3)};
    int s[] = {int(s1), int(s2), int(s3)};
    int p[] = {int(p1), int(p2), int(p3)};

    cudnnPoolingDescriptor_t pooling_desc;
    cudnn_check(cudnnCreatePoolingDescriptor(&pooling_desc));
    cudnn_check(cudnnSetPoolingNdDescriptor(pooling_desc, mode, CUDNN_PROPAGATE_NAN, 3, c, p, s));

    return cudnn_wrapper<cudnnPoolingDescriptor_t>{pooling_desc};
}

/*!
 * \brief Apply the functor on sub and store the result in m
 * \param sub The sub expression
 * \param m The storage matrix
 * \param c1 The first dimension pooling ratio
 * \param c2 The second dimension pooling ratio
 */
template <typename X, typename Y>
void pool_2d(cudnnPoolingMode_t mode, const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
    using type = std::remove_const_t<value_t<X>>;

    decltype(auto) handle = start_cudnn();

    auto pooling_desc = create_pooling_descriptor(mode, c1, c2, s1, s2, p1, p2);

    auto x_tensor = create_tensor(x);
    auto y_tensor = create_tensor(y);

    type alpha[] = {1.0f};
    type beta[] = {0.0f};

    // Allocate GPU memory, if necessary

    x.ensure_gpu_up_to_date();
    y.ensure_gpu_allocated();

    // Perform pooling

    cudnn_check(cudnnPoolingForward(handle.get(), *pooling_desc, alpha, *x_tensor, x.gpu_memory(), beta, *y_tensor, y.gpu_memory()));

    y.validate_gpu();
    y.invalidate_cpu();
}

/*!
 * \brief Apply the functor on sub and store the result in m
 * \param sub The sub expression
 * \param m The storage matrix
 * \param c1 The first dimension pooling ratio
 * \param c2 The second dimension pooling ratio
 */
template <typename X, typename Y>
void pool_3d(cudnnPoolingMode_t mode, const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
    using type = std::remove_const_t<value_t<X>>;

    decltype(auto) handle = start_cudnn();

    auto pooling_desc = create_pooling_descriptor(mode, c1, c2, c3, s1, s2, s3, p1, p2, p3);

    auto x_tensor = create_tensor_5d(x);
    auto y_tensor = create_tensor_5d(y);

    type alpha[] = {1.0f};
    type beta[] = {0.0f};

    // Allocate GPU memory, if necessary

    x.ensure_gpu_up_to_date();
    y.ensure_gpu_allocated();

    // Perform pooling

    cudnn_check(cudnnPoolingForward(handle.get(), *pooling_desc, alpha, *x_tensor, x.gpu_memory(), beta, *y_tensor, y.gpu_memory()));

    y.validate_gpu();
    y.invalidate_cpu();
}

/*!
 * \brief Functor for 2D Max Pooling
 */
struct max_pool_2d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        pool_2d(CUDNN_POOLING_MAX, x, y, c1, c2, s1, s2, p1, p2);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, s1, s2, p1, p2);
        }
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct avg_pool_2d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        pool_2d(CUDNN_POOLING_AVERAGE_COUNT_INCLUDE_PADDING, x, y, c1, c2, s1, s2, p1, p2);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, s1, s2, p1, p2);
        }
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct max_pool_3d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        pool_3d(CUDNN_POOLING_MAX, x, y, c1, c2, c3, s1, s2, s3, p1, p2, p3);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, c3, s1, s2, s3, p1, p2, p3);
        }
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct avg_pool_3d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y, cpp_enable_if((decay_traits<X>::dimensions() < 5))>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        pool_3d(CUDNN_POOLING_AVERAGE_COUNT_INCLUDE_PADDING, x, y, c1, c2, c3, s1, s2, s3, p1, p2, p3);
    }

    // Deep handling

    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename A, typename M, cpp_enable_if((decay_traits<A>::dimensions > 4))>
    static void apply(const A& sub, M&& m, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        for(size_t i = 0; i < etl::dim<0>(sub); ++i){
            apply(sub(i), m(i), c1, c2, c3, s1, s2, s3, p1, p2, p3);
        }
    }
};

#else

//COVERAGE_EXCLUDE_BEGIN

/*!
 * \brief Functor for 2D Max Pooling
 */
struct max_pool_2d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        cpp_unused(x);
        cpp_unused(y);
        cpp_unused(c1);
        cpp_unused(c2);
        cpp_unused(s1);
        cpp_unused(s2);
        cpp_unused(p1);
        cpp_unused(p2);

        cpp_unreachable("Unsupported feature called: cudnn pool");
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct avg_pool_2d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t s1, size_t s2, size_t p1, size_t p2) {
        cpp_unused(x);
        cpp_unused(y);
        cpp_unused(c1);
        cpp_unused(c2);
        cpp_unused(s1);
        cpp_unused(s2);
        cpp_unused(p1);
        cpp_unused(p2);

        cpp_unreachable("Unsupported feature called: cudnn pool");
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct max_pool_3d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        cpp_unused(x);
        cpp_unused(y);
        cpp_unused(c1);
        cpp_unused(c2);
        cpp_unused(c3);
        cpp_unused(s1);
        cpp_unused(s2);
        cpp_unused(s3);
        cpp_unused(p1);
        cpp_unused(p2);
        cpp_unused(p3);

        cpp_unreachable("Unsupported feature called: cudnn pool");
    }
};

/*!
 * \brief Functor for 2D Max Pooling
 */
struct avg_pool_3d {
    /*!
     * \brief Apply the functor on sub and store the result in m
     * \param sub The sub expression
     * \param m The storage matrix
     * \param c1 The first dimension pooling ratio
     * \param c2 The second dimension pooling ratio
     */
    template <typename X, typename Y>
    static void apply(const X& x, Y&& y, size_t c1, size_t c2, size_t c3, size_t s1, size_t s2, size_t s3, size_t p1, size_t p2, size_t p3) {
        cpp_unused(x);
        cpp_unused(y);
        cpp_unused(c1);
        cpp_unused(c2);
        cpp_unused(c3);
        cpp_unused(s1);
        cpp_unused(s2);
        cpp_unused(s3);
        cpp_unused(p1);
        cpp_unused(p2);
        cpp_unused(p3);

        cpp_unreachable("Unsupported feature called: cudnn pool");
    }
};

//COVERAGE_EXCLUDE_END

#endif

} //end of namespace cudnn

} //end of namespace impl

} //end of namespace etl
<|endoftext|>


<commit_before>/// @author Andrew Hundt <athundt@gmail.com>
#ifndef _VREP_VF_CONTROLLER_
#define _VREP_VF_CONTROLLER_

/// @todo remove IGNORE_ROTATION or make it a runtime configurable parameter
// #define IGNORE_ROTATION

#include <string>
#include <tuple>
#include <boost/format.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sawConstraintController/mtsVFController.h>

#include "grl/cisst/GrlVFController.hpp"
#include "grl/vrep/Vrep.hpp"
#include "grl/vrep/VrepRobotArmDriver.hpp"
#include "grl/vrep/VrepRobotArmJacobian.hpp"
#include "grl/vrep/Eigen.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/range/algorithm/copy.hpp>

namespace grl {

/// @todo verify Robotiiwa is the correct base, and RobotMillTipTarget is the right target, because if the transform doesn't match the one in the jacobian the algorithm will break
//template<typename DesiredKinematics = DesiredKinematicsObject>
class VrepInverseKinematicsController : public grl::InverseKinematicsController
{
public:
    typedef grl::InverseKinematicsController parent_type;

    //using parent_type::currentKinematicsStateP_;
    //using parent_type::parent_type::InitializeKinematicsState;

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName,
        IKGroupName
    };

    typedef std::tuple<
             std::string // DesiredKinematicsObjectName
            ,std::string // IKGroupName
        > Params;

    static Params defaultParams()
    {
       return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa");
    }

    /// @todo need to call parent constructor:
        /*! Constructor
    */
    VrepInverseKinematicsController(size_t num_joints = 7, mtsVFBase::CONTROLLERMODE cm = mtsVFBase::CONTROLLERMODE::JPOS):
     InverseKinematicsController(num_joints,cm)
    {
    }

    void construct(Params params = defaultParams()){
        // get kinematics group name
        // get number of joints
        VrepRobotArmDriverP_ = std::make_shared<vrep::VrepRobotArmDriver>();
        VrepRobotArmDriverP_->construct();

        ikGroupHandle_ = simGetIkGroupHandle(std::get<IKGroupName>(params).c_str());
        simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik

        this->currentKinematicsStateP_->Name = std::get<IKGroupName>(params);

        this->desiredKinematicsStateP_->Name = std::get<DesiredKinematicsObjectName>(params);

        /// @todo set desiredKinematicsStateP name, INITIALIZE ALL MEMBER OBJECTS, & SET NAMES
        //
        positionLimitsName = std::get<IKGroupName>(params)+"_PositionLimits";
        this->jointPositionLimitsVFP_->Name = positionLimitsName;

        velocityLimitsName = std::get<IKGroupName>(params)+"_VelocityLimits";
        this->jointVelocityLimitsVFP_->Name = velocityLimitsName;

//            /// This will hold the Jacobian
//    std::unique_ptr<prmKinematicsState> currentKinematicsStateP_;
//
//    /// This will hold the xyz position and the rotation of where I want to go
//    std::unique_ptr<prmKinematicsState> desiredKinematicsStateP_;
//
//    // want to follow a goal position
//    /// @todo will want to use an addVFFollow member function once it is added in
//    std::unique_ptr<mtsVFDataBase> followVFP_;
//
//    /// need velocity limits for the joints
//    std::unique_ptr<mtsVFDataJointLimits> jointVelocityLimitsVFP_;
//    /// joints cannot go to certain position
//    std::unique_ptr<mtsVFDataJointLimits> jointPositionLimitsVFP_;

        /// @todo verify object lifetimes
        //parent_type::parent_type::InitializeKinematicsState(this->currentKinematicsStateP_)

            // for each virtual fixture need names and number of rows

        /// @todo read objective rows from vrep

        // Initialize Variables for update
        //jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        //int numJoints =jointHandles_.size();

#ifndef IGNORE_ROTATION
        /// @todo objectiveRows seems to currently be a 3 vector, may eventually want a rotation and translation, perhaps with quaternion rotation
        followVFP_->ObjectiveRows = 6;
#else
        followVFP_->ObjectiveRows = 3;
#endif
        // set the names once for each object, only once
        followVFP_->KinNames.push_back(currentKinematicsStateP_->Name);
        followVFP_->KinNames.push_back(desiredKinematicsStateP_->Name);

        AddVFFollowPath(*followVFP_);

        /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object
    }

    /// check out sawConstraintController
    void updateKinematics(){

        jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        auto eigentestJacobian=::grl::vrep::getJacobian(*VrepRobotArmDriverP_);

        /// The row/column major order is swapped between cisst and VREP!
        this->currentKinematicsStateP_->Jacobian.SetSize(eigentestJacobian.cols(),eigentestJacobian.rows());
        Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mckp2(this->currentKinematicsStateP_->Jacobian.Pointer(),this->currentKinematicsStateP_->Jacobian.cols(),this->currentKinematicsStateP_->Jacobian.rows());
        mckp2 = eigentestJacobian.cast<double>();

        //Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mf(eigentestJacobian,eigentestJacobian.cols(),eigentestJacobian.rows());
        //Eigen::MatrixXf eigenJacobian = mf;
        Eigen::MatrixXf eigenJacobian = eigentestJacobian;

        ///////////////////////////////////////////////////////////
        // Copy Joint Interval, the range of motion for each joint

        // lower limits
        auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
        std::vector<double> llimits(llim.begin(),llim.end());
        jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);

        // upper limits
        auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
        std::vector<double> ulimits(ulim.begin(),ulim.end());
        jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);

        // update limits
        /// @todo does this leak memory when called every time around?
        UpdateAbsoluteJointLimitsVF(llim.size(),positionLimitsName,jointPositionLimitsVFP_->UpperLimits,jointPositionLimitsVFP_->LowerLimits);

        const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();
        Eigen::Affine3d currentEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  currentEigenT = currentEndEffectorPose.translation();
        auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
        currentCisstT[0] = currentEigenT(0);
        currentCisstT[1] = currentEigenT(1);
        currentCisstT[2] = currentEigenT(2);
#ifndef IGNORE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
        ccr = currentEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
        /// @todo set rotation component of current position

        Eigen::Affine3d desiredEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  desiredEigenT = desiredEndEffectorPose.translation();
        auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
        desiredCisstT[0] = desiredEigenT(0);
        desiredCisstT[1] = desiredEigenT(1);
        desiredCisstT[2] = desiredEigenT(2);
#ifndef IGNORE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
        dcr = desiredEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
        /// @todo set rotation component of desired position

        // for debugging, the translation between the current and desired position in cartesian coordinates
        auto inputDesired_dx = desiredCisstT - currentCisstT;

        vct3 dx_translation, dx_rotation;

        // Rotation part
        vctAxAnRot3 dxRot;
        vct3 dxRotVec;
        dxRot.FromNormalized((currentKinematicsStateP_->Frame.Inverse() * desiredKinematicsStateP_->Frame).Rotation());
        dxRotVec = dxRot.Axis() * dxRot.Angle();
        dx_rotation[0] = dxRotVec[0];
        dx_rotation[1] = dxRotVec[1];
        dx_rotation[2] = dxRotVec[2];
        //dx_rotation.SetAll(0.0);
        dx_rotation = currentKinematicsStateP_->Frame.Rotation() * dx_rotation;

        Eigen::AngleAxis<float> tipToTarget_cisstToEigen;

        Eigen::Matrix3f rotmat;
        double theta = std::sqrt(dx_rotation[0]*dx_rotation[0]+dx_rotation[1]*dx_rotation[1]+dx_rotation[2]*dx_rotation[2]);
        rotmat= Eigen::AngleAxisf(theta,Eigen::Vector3f(dx_rotation[0]/theta,dx_rotation[1]/theta,dx_rotation[2]/theta));

//        std::cout << "\ntiptotarget     \n" << tipToTarget.matrix() << "\n";
//        std::cout << "\ntiptotargetcisst\n" << rotmat.matrix() << "\n";

        //BOOST_LOG_TRIVIAL(trace) << "\n   test         desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
        SetKinematics(*currentKinematicsStateP_);  // replaced by name of object
        // fill these out in the desiredKinematicsStateP_
        //RotationType RotationMember; // vcRot3
        //TranslationType TranslationMember; // vct3

        SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
        // call setKinematics with the new kinematics
        // sawconstraintcontroller has kinematicsState
        // set the jacobian here

        //////////////////////
        /// @todo move code below here back under run_one updateKinematics() call

       /// @todo need to provide tick time in double seconds and get from vrep API call
       float simulationTimeStep = simGetSimulationTimeStep();
       UpdateOptimizer(simulationTimeStep);

       vctDoubleVec jointAngles_dt;
       auto returncode = Solve(jointAngles_dt);

       /// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
       if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));

       /// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
        std::string str;
       // str = "";
       for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
       {
          float currentAngle;
          auto ret = simGetJointPosition(jointHandles_[i],&currentAngle);
          BOOST_VERIFY(ret!=-1);
          float futureAngle = currentAngle + jointAngles_dt[i];
          //simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
          //simSetJointTargetPosition(jointHandles_[i],futureAngle);
          simSetJointPosition(jointHandles_[i],futureAngle);
                str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
                if (i<jointHandles_.size()-1)
                    str+=", ";
       }
        BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;

        auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;

        BOOST_LOG_TRIVIAL(trace) << "\n            desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
    }

    /// may not need this it is in the base class
    /// blocking call, call in separate thread, just allocates memory
    void run_one(){
       updateKinematics();

    }

    /// may not need this it is in the base class
    /// this will have output
    /// blocking call, call in separate thread, just allocates memory
    /// this runs the actual optimization algorithm
    void solve(){

    }

    std::vector<int> jointHandles_; ///< @todo move this item back into VrepRobotArmDriver
    int ikGroupHandle_;
    std::shared_ptr<vrep::VrepRobotArmDriver> VrepRobotArmDriverP_;
    vrep::VrepRobotArmDriver::State currentArmState_;
    std::string positionLimitsName;
    std::string velocityLimitsName;
};

} // namespace grl

#endif<commit_msg>Joint position limits are now supported. However, there is a vibration bug that may have been present for a long time when using the grl supplied IK<commit_after>/// @author Andrew Hundt <athundt@gmail.com>
#ifndef _VREP_VF_CONTROLLER_
#define _VREP_VF_CONTROLLER_

/// @todo remove IGNORE_ROTATION or make it a runtime configurable parameter
// #define IGNORE_ROTATION

#include <string>
#include <tuple>
#include <boost/format.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sawConstraintController/mtsVFController.h>

#include "grl/cisst/GrlVFController.hpp"
#include "grl/vrep/Vrep.hpp"
#include "grl/vrep/VrepRobotArmDriver.hpp"
#include "grl/vrep/VrepRobotArmJacobian.hpp"
#include "grl/vrep/Eigen.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/range/algorithm/copy.hpp>

namespace grl {

/// @todo verify Robotiiwa is the correct base, and RobotMillTipTarget is the right target, because if the transform doesn't match the one in the jacobian the algorithm will break
//template<typename DesiredKinematics = DesiredKinematicsObject>
class VrepInverseKinematicsController : public grl::InverseKinematicsController
{
public:
    typedef grl::InverseKinematicsController parent_type;

    //using parent_type::currentKinematicsStateP_;
    //using parent_type::parent_type::InitializeKinematicsState;

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName,
        IKGroupName
    };

    typedef std::tuple<
             std::string // DesiredKinematicsObjectName
            ,std::string // IKGroupName
        > Params;

    static Params defaultParams()
    {
       return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa");
    }

    /// @todo need to call parent constructor:
        /*! Constructor
    */
    VrepInverseKinematicsController(size_t num_joints = 7, mtsVFBase::CONTROLLERMODE cm = mtsVFBase::CONTROLLERMODE::JPOS):
     InverseKinematicsController(num_joints,cm)
    {
    }

    void construct(Params params = defaultParams()){
        // get kinematics group name
        // get number of joints
        VrepRobotArmDriverP_ = std::make_shared<vrep::VrepRobotArmDriver>();
        VrepRobotArmDriverP_->construct();

        ikGroupHandle_ = simGetIkGroupHandle(std::get<IKGroupName>(params).c_str());
        simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik

        this->currentKinematicsStateP_->Name = std::get<IKGroupName>(params);

        this->desiredKinematicsStateP_->Name = std::get<DesiredKinematicsObjectName>(params);

        /// @todo set desiredKinematicsStateP name, INITIALIZE ALL MEMBER OBJECTS, & SET NAMES
        //
        positionLimitsName = std::get<IKGroupName>(params)+"_PositionLimits";
        this->jointPositionLimitsVFP_->Name = positionLimitsName;

        velocityLimitsName = std::get<IKGroupName>(params)+"_VelocityLimits";
        this->jointVelocityLimitsVFP_->Name = velocityLimitsName;

//            /// This will hold the Jacobian
//    std::unique_ptr<prmKinematicsState> currentKinematicsStateP_;
//
//    /// This will hold the xyz position and the rotation of where I want to go
//    std::unique_ptr<prmKinematicsState> desiredKinematicsStateP_;
//
//    // want to follow a goal position
//    /// @todo will want to use an addVFFollow member function once it is added in
//    std::unique_ptr<mtsVFDataBase> followVFP_;
//
//    /// need velocity limits for the joints
//    std::unique_ptr<mtsVFDataJointLimits> jointVelocityLimitsVFP_;
//    /// joints cannot go to certain position
//    std::unique_ptr<mtsVFDataJointLimits> jointPositionLimitsVFP_;

        /// @todo verify object lifetimes
        //parent_type::parent_type::InitializeKinematicsState(this->currentKinematicsStateP_)

            // for each virtual fixture need names and number of rows

        /// @todo read objective rows from vrep

        // Initialize Variables for update
        //jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        //int numJoints =jointHandles_.size();

#ifndef IGNORE_ROTATION
        /// @todo objectiveRows seems to currently be a 3 vector, may eventually want a rotation and translation, perhaps with quaternion rotation
        followVFP_->ObjectiveRows = 6;
#else
        followVFP_->ObjectiveRows = 3;
#endif
        // set the names once for each object, only once
        followVFP_->KinNames.push_back(currentKinematicsStateP_->Name);
        followVFP_->KinNames.push_back(desiredKinematicsStateP_->Name);

        AddVFFollowPath(*followVFP_);

        /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object
    }

    /// check out sawConstraintController
    void updateKinematics(){

        jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        auto eigentestJacobian=::grl::vrep::getJacobian(*VrepRobotArmDriverP_);

        /// The row/column major order is swapped between cisst and VREP!
        this->currentKinematicsStateP_->Jacobian.SetSize(eigentestJacobian.cols(),eigentestJacobian.rows());
        Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mckp2(this->currentKinematicsStateP_->Jacobian.Pointer(),this->currentKinematicsStateP_->Jacobian.cols(),this->currentKinematicsStateP_->Jacobian.rows());
        mckp2 = eigentestJacobian.cast<double>();

        //Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mf(eigentestJacobian,eigentestJacobian.cols(),eigentestJacobian.rows());
        //Eigen::MatrixXf eigenJacobian = mf;
        Eigen::MatrixXf eigenJacobian = eigentestJacobian;

        ///////////////////////////////////////////////////////////
        // Copy Joint Interval, the range of motion for each joint

        // lower limits
        auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
        std::vector<double> llimits(llim.begin(),llim.end());
        jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);

        // upper limits
        auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
        std::vector<double> ulimits(ulim.begin(),ulim.end());
        jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);

        // current position
        auto & currentJointPos = std::get<vrep::VrepRobotArmDriver::JointPosition>(currentArmState_);
        std::vector<double> currentJointPosVec(currentJointPos.begin(),currentJointPos.end());
        vctDoubleVec vctDoubleVecCurrentJoints(currentJointPosVec.size(),&currentJointPosVec[0]);

        // update limits
        /// @todo does this leak memory when called every time around?
        UpdateJointPosLimitsVF(positionLimitsName,jointPositionLimitsVFP_->UpperLimits,jointPositionLimitsVFP_->LowerLimits,vctDoubleVecCurrentJoints);

        const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();
        Eigen::Affine3d currentEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  currentEigenT = currentEndEffectorPose.translation();
        auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
        currentCisstT[0] = currentEigenT(0);
        currentCisstT[1] = currentEigenT(1);
        currentCisstT[2] = currentEigenT(2);
#ifndef IGNORE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
        ccr = currentEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
        /// @todo set rotation component of current position

        Eigen::Affine3d desiredEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  desiredEigenT = desiredEndEffectorPose.translation();
        auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
        desiredCisstT[0] = desiredEigenT(0);
        desiredCisstT[1] = desiredEigenT(1);
        desiredCisstT[2] = desiredEigenT(2);
#ifndef IGNORE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
        dcr = desiredEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
        /// @todo set rotation component of desired position

        // for debugging, the translation between the current and desired position in cartesian coordinates
        auto inputDesired_dx = desiredCisstT - currentCisstT;

        vct3 dx_translation, dx_rotation;

        // Rotation part
        vctAxAnRot3 dxRot;
        vct3 dxRotVec;
        dxRot.FromNormalized((currentKinematicsStateP_->Frame.Inverse() * desiredKinematicsStateP_->Frame).Rotation());
        dxRotVec = dxRot.Axis() * dxRot.Angle();
        dx_rotation[0] = dxRotVec[0];
        dx_rotation[1] = dxRotVec[1];
        dx_rotation[2] = dxRotVec[2];
        //dx_rotation.SetAll(0.0);
        dx_rotation = currentKinematicsStateP_->Frame.Rotation() * dx_rotation;

        Eigen::AngleAxis<float> tipToTarget_cisstToEigen;

        Eigen::Matrix3f rotmat;
        double theta = std::sqrt(dx_rotation[0]*dx_rotation[0]+dx_rotation[1]*dx_rotation[1]+dx_rotation[2]*dx_rotation[2]);
        rotmat= Eigen::AngleAxisf(theta,Eigen::Vector3f(dx_rotation[0]/theta,dx_rotation[1]/theta,dx_rotation[2]/theta));

//        std::cout << "\ntiptotarget     \n" << tipToTarget.matrix() << "\n";
//        std::cout << "\ntiptotargetcisst\n" << rotmat.matrix() << "\n";

        //BOOST_LOG_TRIVIAL(trace) << "\n   test         desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
        SetKinematics(*currentKinematicsStateP_);  // replaced by name of object
        // fill these out in the desiredKinematicsStateP_
        //RotationType RotationMember; // vcRot3
        //TranslationType TranslationMember; // vct3

        SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
        // call setKinematics with the new kinematics
        // sawconstraintcontroller has kinematicsState
        // set the jacobian here

        //////////////////////
        /// @todo move code below here back under run_one updateKinematics() call

       /// @todo need to provide tick time in double seconds and get from vrep API call
       float simulationTimeStep = simGetSimulationTimeStep();
       UpdateOptimizer(simulationTimeStep);

       vctDoubleVec jointAngles_dt;
       auto returncode = Solve(jointAngles_dt);

       /// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
       if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));

       /// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
        std::string str;
       // str = "";
       for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
       {
          float currentAngle;
          auto ret = simGetJointPosition(jointHandles_[i],&currentAngle);
          BOOST_VERIFY(ret!=-1);
          float futureAngle = currentAngle + jointAngles_dt[i];
          //simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
          //simSetJointTargetPosition(jointHandles_[i],futureAngle);
          simSetJointPosition(jointHandles_[i],futureAngle);
                str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
                if (i<jointHandles_.size()-1)
                    str+=", ";
       }
        BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;

        auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;

        BOOST_LOG_TRIVIAL(trace) << "\n            desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
    }

    /// may not need this it is in the base class
    /// blocking call, call in separate thread, just allocates memory
    void run_one(){
       updateKinematics();

    }

    /// may not need this it is in the base class
    /// this will have output
    /// blocking call, call in separate thread, just allocates memory
    /// this runs the actual optimization algorithm
    void solve(){

    }

    std::vector<int> jointHandles_; ///< @todo move this item back into VrepRobotArmDriver
    int ikGroupHandle_;
    std::shared_ptr<vrep::VrepRobotArmDriver> VrepRobotArmDriverP_;
    vrep::VrepRobotArmDriver::State currentArmState_;
    std::string positionLimitsName;
    std::string velocityLimitsName;
};

} // namespace grl

#endif<|endoftext|>


<commit_before>/// @author Andrew Hundt <athundt@gmail.com>

#ifndef _VREP_VF_CONTROLLER_
#define _VREP_VF_CONTROLLER_

#include <string>
#include <tuple>
#include <boost/format.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sawConstraintController/mtsVFController.h>

#include "grl/cisst/GrlVFController.hpp"
#include "grl/vrep/Vrep.hpp"
#include "grl/vrep/VrepRobotArmDriver.hpp"
#include "grl/vrep/Eigen.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/range/algorithm/copy.hpp>

namespace grl {

/// This handles a whole vrep path object
class DesiredKinematicsPath {

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName
    };

    typedef std::tuple<
            std::string // DesiredKinematicsObjectName
        > VrepVFControllerParams;

    void construct()
    {

    }

    Eigen::Affine3d getDesiredPose(){

    }
};

/// This handles a specific vrep pose
class DesiredKinematicsObject {

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName
    };

    std::tuple<
            std::string // DesiredKinematicsObjectName
        > Params;

    void construct()
    {

    }

    Eigen::Affine3d getDesiredPose(){

    }
};

/// @todo verify Robotiiwa is the correct base, and RobotMillTipTarget is the right target, because if the transform doesn't match the one in the jacobian the algorithm will break
//template<typename DesiredKinematics = DesiredKinematicsObject>
class VrepInverseKinematicsController : public grl::InverseKinematicsController
{
public:
    typedef InverseKinematicsController parent_type;

    //using parent_type::currentKinematicsStateP_;
    //using parent_type::parent_type::InitializeKinematicsState;

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName,
        IKGroupName
    };

    typedef std::tuple<
             std::string // DesiredKinematicsObjectName
            ,std::string // IKGroupName
        > Params;

    static Params defaultParams()
    {
       return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa");
    }

    /// @todo need to call parent constructor:
        /*! Constructor
    */
    VrepInverseKinematicsController(size_t num_joints = 7, mtsVFBase::CONTROLLERMODE cm = mtsVFBase::CONTROLLERMODE::JPOS):
     InverseKinematicsController(num_joints,cm)
    {
    }

    void construct(Params params = defaultParams()){
        // get kinematics group name
        // get number of joints
        VrepRobotArmDriverP_ = std::make_shared<vrep::VrepRobotArmDriver>();
        VrepRobotArmDriverP_->construct();

        ikGroupHandle_ = simGetIkGroupHandle(std::get<IKGroupName>(params).c_str());
        simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik

        this->currentKinematicsStateP_->Name = std::get<IKGroupName>(params);

        this->desiredKinematicsStateP_->Name = std::get<DesiredKinematicsObjectName>(params);

        /// @todo set desiredKinematicsStateP name, INITIALIZE ALL MEMBER OBJECTS, & SET NAMES
        //
        positionLimitsName = std::get<IKGroupName>(params)+"_PositionLimits";
        this->jointPositionLimitsVFP_->Name = positionLimitsName;

        velocityLimitsName = std::get<IKGroupName>(params)+"_VelocityLimits";
        this->jointVelocityLimitsVFP_->Name = velocityLimitsName;

//            /// This will hold the Jacobian
//    std::unique_ptr<prmKinematicsState> currentKinematicsStateP_;
//
//    /// This will hold the xyz position and the rotation of where I want to go
//    std::unique_ptr<prmKinematicsState> desiredKinematicsStateP_;
//
//    // want to follow a goal position
//    /// @todo will want to use an addVFFollow member function once it is added in
//    std::unique_ptr<mtsVFDataBase> followVFP_;
//
//    /// need velocity limits for the joints
//    std::unique_ptr<mtsVFDataJointLimits> jointVelocityLimitsVFP_;
//    /// joints cannot go to certain position
//    std::unique_ptr<mtsVFDataJointLimits> jointPositionLimitsVFP_;

        /// @todo verify object lifetimes
        //parent_type::parent_type::InitializeKinematicsState(this->currentKinematicsStateP_)

            // for each virtual fixture need names and number of rows

        /// @todo read objective rows from vrep

        // Initialize Variables for update
        //jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        //int numJoints =jointHandles_.size();

#ifdef HANDLE_ROTATION
        /// @todo objectiveRows seems to currently be a 3 vector, may eventually want a rotation and translation, perhaps with quaternion rotation
        followVFP_->ObjectiveRows = 6;
#else
        followVFP_->ObjectiveRows = 3;
#endif
        // set the names once for each object, only once
        followVFP_->KinNames.push_back(currentKinematicsStateP_->Name);
        followVFP_->KinNames.push_back(desiredKinematicsStateP_->Name);

        AddVFFollowPath(*followVFP_);

        /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object
    }

    /// check out sawConstraintController
    void updateKinematics(){

        // Initialize Variables for update
        jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        int numJoints =jointHandles_.size();
        std::vector<float> ikCalculatedJointValues(numJoints,0);
        VrepRobotArmDriverP_->getState(currentArmState_);

        /// Run inverse kinematics, but all we really want is the jacobian
        /// @todo find version that only returns jacobian
        /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simCheckIkGroup
        auto ikcalcResult =
        simCheckIkGroup(ikGroupHandle_
                       ,numJoints
                       ,&jointHandles_[0]
                       ,&ikCalculatedJointValues[0]
                       ,NULL /// @todo do we need to use these options?
                       );
        /// @see http://www.coppeliarobotics.com/helpFiles/en/apiConstants.htm#ikCalculationResults
        if(ikcalcResult!=sim_ikresult_success && ikcalcResult != sim_ikresult_not_performed)
        {
            BOOST_LOG_TRIVIAL(error) << "VrepInverseKinematicsController: didn't run inverse kinematics";
            return;
        }

        // Get the Jacobian
        int jacobianSize[2];
        float* jacobian=simGetIkGroupMatrix(ikGroupHandle_,0,jacobianSize);
        /// @todo FIX HACK jacobianSize include orientation component, should be 7x6 instead of 7x3

#ifndef HANDLE_ROTATION
        jacobianSize[1] = 3;
#endif

        /// The row/column major order is swapped between cisst and VREP!
        this->currentKinematicsStateP_->Jacobian.SetSize(jacobianSize[1],jacobianSize[0]);

        // Transfer the Jacobian to cisst

        // jacobianSize[0] represents the row count of the Jacobian
        // (i.e. the number of equations or the number of joints involved
        // in the IK resolution of the given kinematic chain)
        // Joints appear from tip to base.

        // jacobianSize[1] represents the column count of the Jacobian
        // (i.e. the number of constraints, e.g. x,y,z,alpha,beta,gamma,jointDependency)

        // The Jacobian data is ordered row-wise, i.e.:
        // [row0,col0],[row1,col0],..,[rowN,col0],[row0,col1],[row1,col1],etc.

        std::string str;
        str+="\n";
        for (int i=0;i<jacobianSize[0];i++)
        {
            for (int j=0;j<jacobianSize[1];j++)
            {
                str+=boost::str(boost::format("%.1e") % jacobian[static_cast<int>(j*jacobianSize[0]+i)]);
                if (j<jacobianSize[1]-1)
                    str+=", ";
                float currentValue = jacobian[static_cast<int>(j*jacobianSize[0]+i)];

                /// The row/column major order is swapped between cisst and VREP!
                this->currentKinematicsStateP_->Jacobian[j][jacobianSize[0]-i-1] = currentValue;
            }
            str+="\n";
        }
        BOOST_LOG_TRIVIAL(trace) << str;

        ///////////////////////////////////////////////////////////
        /// @todo Set Current Joint State

        // currentKinematicsStateP_->JointState = ...
        // prmJointState* JointState;

        ///////////////////////////////////////////////////////////
        // Copy Joint Interval, the range of motion for each joint

        // lower limits
        auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
        std::vector<double> llimits(llim.begin(),llim.end());
        jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);

        // upper limits
        auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
        std::vector<double> ulimits(ulim.begin(),ulim.end());
        jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);

        /// @todo get target position, probably relative to base
        const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();

        Eigen::Affine3d currentEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  currentEigenT = currentEndEffectorPose.translation();
        auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
        currentCisstT[0] = currentEigenT(0);
        currentCisstT[1] = currentEigenT(1);
        currentCisstT[2] = currentEigenT(2);
#ifdef HANDLE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
        ccr = currentEndEffectorPose.rotation();
#endif // HANDLE_ROTATION
        /// @todo set rotation component of current position

        Eigen::Affine3d desiredEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  desiredEigenT = desiredEndEffectorPose.translation();
        auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
        desiredCisstT[0] = desiredEigenT(0);
        desiredCisstT[1] = desiredEigenT(1);
        desiredCisstT[2] = desiredEigenT(2);
#ifdef HANDLE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
        dcr = desiredEndEffectorPose.rotation();
#endif // HANDLE_ROTATION
        /// @todo set rotation component of desired position

        // for debugging, the translation between the current and desired position in cartesian coordinates
        auto inputDesired_dx = desiredCisstT - currentCisstT;

        SetKinematics(*currentKinematicsStateP_);  // replaced by name of object
        // fill these out in the desiredKinematicsStateP_
        //RotationType RotationMember; // vcRot3
        //TranslationType TranslationMember; // vct3

        SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
        // call setKinematics with the new kinematics
        // sawconstraintcontroller has kinematicsState
        // set the jacobian here

        //////////////////////
        /// @todo move code below here back under run_one updateKinematics() call

       /// @todo need to provide tick time in double seconds and get from vrep API call
       UpdateOptimizer(0.01);

       vctDoubleVec jointAngles_dt;
       auto returncode = Solve(jointAngles_dt);

       /// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
       if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));

       /// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
        //std::string str;
        str = "";
       for (int i=0 ; i < jointHandles_.size() ; i++)
       {
          float currentAngle;
          auto ret = simGetJointPosition(jointHandles_[i],&currentAngle);
          BOOST_VERIFY(ret!=-1);
          float futureAngle = currentAngle + jointAngles_dt[i];
          //simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
          //simSetJointTargetPosition(jointHandles_[i],futureAngle);
          simSetJointPosition(jointHandles_[i],futureAngle);
                str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
                if (i<jointHandles_.size()-1)
                    str+=", ";
       }
        BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;

        auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;

        BOOST_LOG_TRIVIAL(trace) << "desired dx: " << inputDesired_dx << " optimizer Calculated dx: " << optimizerCalculated_dx;
    }

    /// may not need this it is in the base class
    /// blocking call, call in separate thread, just allocates memory
    void run_one(){
       updateKinematics();

    }

    /// may not need this it is in the base class
    /// this will have output
    /// blocking call, call in separate thread, just allocates memory
    /// this runs the actual optimization algorithm
    void solve(){

    }

    std::vector<int> jointHandles_; ///< @todo move this item back into VrepRobotArmDriver
    int ikGroupHandle_;
    std::shared_ptr<vrep::VrepRobotArmDriver> VrepRobotArmDriverP_;
    vrep::VrepRobotArmDriver::State currentArmState_;
    std::string positionLimitsName;
    std::string velocityLimitsName;
};

} // namespace grl

#endif<commit_msg>Jacobian now calculates successfully for griInverseKinematics test. see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=3967 for details.<commit_after>/// @author Andrew Hundt <athundt@gmail.com>

#ifndef _VREP_VF_CONTROLLER_
#define _VREP_VF_CONTROLLER_

#include <string>
#include <tuple>
#include <boost/format.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sawConstraintController/mtsVFController.h>

#include "grl/cisst/GrlVFController.hpp"
#include "grl/vrep/Vrep.hpp"
#include "grl/vrep/VrepRobotArmDriver.hpp"
#include "grl/vrep/Eigen.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/range/algorithm/copy.hpp>

namespace grl {

/// This handles a whole vrep path object
class DesiredKinematicsPath {

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName
    };

    typedef std::tuple<
            std::string // DesiredKinematicsObjectName
        > VrepVFControllerParams;

    void construct()
    {

    }

    Eigen::Affine3d getDesiredPose(){

    }
};

/// This handles a specific vrep pose
class DesiredKinematicsObject {

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName
    };

    std::tuple<
            std::string // DesiredKinematicsObjectName
        > Params;

    void construct()
    {

    }

    Eigen::Affine3d getDesiredPose(){

    }
};

/// @todo verify Robotiiwa is the correct base, and RobotMillTipTarget is the right target, because if the transform doesn't match the one in the jacobian the algorithm will break
//template<typename DesiredKinematics = DesiredKinematicsObject>
class VrepInverseKinematicsController : public grl::InverseKinematicsController
{
public:
    typedef InverseKinematicsController parent_type;

    //using parent_type::currentKinematicsStateP_;
    //using parent_type::parent_type::InitializeKinematicsState;

    enum  VrepVFControllerParamsIndex {
        DesiredKinematicsObjectName,
        IKGroupName
    };

    typedef std::tuple<
             std::string // DesiredKinematicsObjectName
            ,std::string // IKGroupName
        > Params;

    static Params defaultParams()
    {
       return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa");
    }

    /// @todo need to call parent constructor:
        /*! Constructor
    */
    VrepInverseKinematicsController(size_t num_joints = 7, mtsVFBase::CONTROLLERMODE cm = mtsVFBase::CONTROLLERMODE::JPOS):
     InverseKinematicsController(num_joints,cm)
    {
    }

    void construct(Params params = defaultParams()){
        // get kinematics group name
        // get number of joints
        VrepRobotArmDriverP_ = std::make_shared<vrep::VrepRobotArmDriver>();
        VrepRobotArmDriverP_->construct();

        ikGroupHandle_ = simGetIkGroupHandle(std::get<IKGroupName>(params).c_str());
        simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik

        this->currentKinematicsStateP_->Name = std::get<IKGroupName>(params);

        this->desiredKinematicsStateP_->Name = std::get<DesiredKinematicsObjectName>(params);

        /// @todo set desiredKinematicsStateP name, INITIALIZE ALL MEMBER OBJECTS, & SET NAMES
        //
        positionLimitsName = std::get<IKGroupName>(params)+"_PositionLimits";
        this->jointPositionLimitsVFP_->Name = positionLimitsName;

        velocityLimitsName = std::get<IKGroupName>(params)+"_VelocityLimits";
        this->jointVelocityLimitsVFP_->Name = velocityLimitsName;

//            /// This will hold the Jacobian
//    std::unique_ptr<prmKinematicsState> currentKinematicsStateP_;
//
//    /// This will hold the xyz position and the rotation of where I want to go
//    std::unique_ptr<prmKinematicsState> desiredKinematicsStateP_;
//
//    // want to follow a goal position
//    /// @todo will want to use an addVFFollow member function once it is added in
//    std::unique_ptr<mtsVFDataBase> followVFP_;
//
//    /// need velocity limits for the joints
//    std::unique_ptr<mtsVFDataJointLimits> jointVelocityLimitsVFP_;
//    /// joints cannot go to certain position
//    std::unique_ptr<mtsVFDataJointLimits> jointPositionLimitsVFP_;

        /// @todo verify object lifetimes
        //parent_type::parent_type::InitializeKinematicsState(this->currentKinematicsStateP_)

            // for each virtual fixture need names and number of rows

        /// @todo read objective rows from vrep

        // Initialize Variables for update
        //jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        //int numJoints =jointHandles_.size();

#ifdef HANDLE_ROTATION
        /// @todo objectiveRows seems to currently be a 3 vector, may eventually want a rotation and translation, perhaps with quaternion rotation
        followVFP_->ObjectiveRows = 6;
#else
        followVFP_->ObjectiveRows = 3;
#endif
        // set the names once for each object, only once
        followVFP_->KinNames.push_back(currentKinematicsStateP_->Name);
        followVFP_->KinNames.push_back(desiredKinematicsStateP_->Name);

        AddVFFollowPath(*followVFP_);

        /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object
    }

    /// check out sawConstraintController
    void updateKinematics(){

        // Initialize Variables for update
        jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
        int numJoints =jointHandles_.size();
        std::vector<float> ikCalculatedJointValues(numJoints,0);
        VrepRobotArmDriverP_->getState(currentArmState_);

        /// @todo get target position, probably relative to base
        const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();

        auto target = std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams);
        auto tip = std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams);

        // save the current tip to target transform
        Eigen::Affine3d tipToTarget =getObjectTransform( target,tip);

        // set the current transform to the identity so simCheckIkGroup won't fail
        // @see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=3967
        // for details about this issue.
        setObjectTransform( target,tip,Eigen::Affine3d::Identity());

        // debug:
        // std::cout << "TipToTargetTransform:\n" << tipToTarget.matrix() << "\n";

        /// Run inverse kinematics, but all we really want is the jacobian
        /// @todo find version that only returns jacobian
        /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simCheckIkGroup
        auto ikcalcResult =
        simCheckIkGroup(ikGroupHandle_
                       ,numJoints
                       ,&jointHandles_[0]
                       ,&ikCalculatedJointValues[0]
                       ,NULL /// @todo do we need to use these options?
                       );
        /// @see http://www.coppeliarobotics.com/helpFiles/en/apiConstants.htm#ikCalculationResults
        if(ikcalcResult!=sim_ikresult_success && ikcalcResult != sim_ikresult_not_performed)
        {
            BOOST_LOG_TRIVIAL(error) << "VrepInverseKinematicsController: didn't run inverse kinematics";
            return;
        }

        setObjectTransform( target,tip,tipToTarget);

        // Get the Jacobian
        int jacobianSize[2];
        float* jacobian=simGetIkGroupMatrix(ikGroupHandle_,0,jacobianSize);
        /// @todo FIX HACK jacobianSize include orientation component, should be 7x6 instead of 7x3

#ifndef HANDLE_ROTATION
        jacobianSize[1] = 3;
#endif

        /// The row/column major order is swapped between cisst and VREP!
        this->currentKinematicsStateP_->Jacobian.SetSize(jacobianSize[1],jacobianSize[0]);

        // Transfer the Jacobian to cisst

        // jacobianSize[0] represents the row count of the Jacobian
        // (i.e. the number of equations or the number of joints involved
        // in the IK resolution of the given kinematic chain)
        // Joints appear from tip to base.

        // jacobianSize[1] represents the column count of the Jacobian
        // (i.e. the number of constraints, e.g. x,y,z,alpha,beta,gamma,jointDependency)

        // The Jacobian data is ordered row-wise, i.e.:
        // [row0,col0],[row1,col0],..,[rowN,col0],[row0,col1],[row1,col1],etc.

        std::string str;
        str+="\n";
        for (int i=0;i<jacobianSize[0];i++)
        {
            for (int j=0;j<jacobianSize[1];j++)
            {
                str+=boost::str(boost::format("%.1e") % jacobian[static_cast<int>(j*jacobianSize[0]+i)]);
                if (j<jacobianSize[1]-1)
                    str+=", ";
                float currentValue = jacobian[static_cast<int>(j*jacobianSize[0]+i)];

                /// The row/column major order is swapped between cisst and VREP!
                this->currentKinematicsStateP_->Jacobian[j][jacobianSize[0]-i-1] = currentValue;
            }
            str+="\n";
        }
        BOOST_LOG_TRIVIAL(trace) << str;

        ///////////////////////////////////////////////////////////
        /// @todo Set Current Joint State

        // currentKinematicsStateP_->JointState = ...
        // prmJointState* JointState;

        ///////////////////////////////////////////////////////////
        // Copy Joint Interval, the range of motion for each joint

        // lower limits
        auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
        std::vector<double> llimits(llim.begin(),llim.end());
        jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);

        // upper limits
        auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
        std::vector<double> ulimits(ulim.begin(),ulim.end());
        jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);

        Eigen::Affine3d currentEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  currentEigenT = currentEndEffectorPose.translation();
        auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
        currentCisstT[0] = currentEigenT(0);
        currentCisstT[1] = currentEigenT(1);
        currentCisstT[2] = currentEigenT(2);
#ifdef HANDLE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
        ccr = currentEndEffectorPose.rotation();
#endif // HANDLE_ROTATION
        /// @todo set rotation component of current position

        Eigen::Affine3d desiredEndEffectorPose =
        getObjectTransform(
                             std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
                            ,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
                          );
        auto  desiredEigenT = desiredEndEffectorPose.translation();
        auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
        desiredCisstT[0] = desiredEigenT(0);
        desiredCisstT[1] = desiredEigenT(1);
        desiredCisstT[2] = desiredEigenT(2);
#ifdef HANDLE_ROTATION
        Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
        dcr = desiredEndEffectorPose.rotation();
#endif // HANDLE_ROTATION
        /// @todo set rotation component of desired position

        // for debugging, the translation between the current and desired position in cartesian coordinates
        auto inputDesired_dx = desiredCisstT - currentCisstT;

        SetKinematics(*currentKinematicsStateP_);  // replaced by name of object
        // fill these out in the desiredKinematicsStateP_
        //RotationType RotationMember; // vcRot3
        //TranslationType TranslationMember; // vct3

        SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
        // call setKinematics with the new kinematics
        // sawconstraintcontroller has kinematicsState
        // set the jacobian here

        //////////////////////
        /// @todo move code below here back under run_one updateKinematics() call

       /// @todo need to provide tick time in double seconds and get from vrep API call
       UpdateOptimizer(0.01);

       vctDoubleVec jointAngles_dt;
       auto returncode = Solve(jointAngles_dt);

       /// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
       if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));

       /// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
        //std::string str;
        str = "";
       for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
       {
          float currentAngle;
          auto ret = simGetJointPosition(jointHandles_[i],&currentAngle);
          BOOST_VERIFY(ret!=-1);
          float futureAngle = currentAngle + jointAngles_dt[i];
          //simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
          //simSetJointTargetPosition(jointHandles_[i],futureAngle);
          simSetJointPosition(jointHandles_[i],futureAngle);
                str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
                if (i<jointHandles_.size()-1)
                    str+=", ";
       }
        BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;

        auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;

        BOOST_LOG_TRIVIAL(trace) << "desired dx: " << inputDesired_dx << " optimizer Calculated dx: " << optimizerCalculated_dx;
    }

    /// may not need this it is in the base class
    /// blocking call, call in separate thread, just allocates memory
    void run_one(){
       updateKinematics();

    }

    /// may not need this it is in the base class
    /// this will have output
    /// blocking call, call in separate thread, just allocates memory
    /// this runs the actual optimization algorithm
    void solve(){

    }

    std::vector<int> jointHandles_; ///< @todo move this item back into VrepRobotArmDriver
    int ikGroupHandle_;
    std::shared_ptr<vrep::VrepRobotArmDriver> VrepRobotArmDriverP_;
    vrep::VrepRobotArmDriver::State currentArmState_;
    std::string positionLimitsName;
    std::string velocityLimitsName;
};

} // namespace grl

#endif<|endoftext|>


<commit_before>/**
 * @file main.cpp
 *
 * @author CNR
 */

#include "LolaAdaptor.h"

#include <csignal>
#include <unistd.h>
#include <chrono>
#include <sys/stat.h>

using namespace naoth;
using namespace std;

// std::atomic_int framesSinceCognitionLastSeen;
std::atomic_bool running;
std::atomic_bool already_got_signal;

#define TO_STRING_INT(x) #x
#define TO_STRING(x) TO_STRING_INT(x)

std::string getSignalDescription(int sigid)
{
  switch (sigid)
  {
    case SIGABRT:
      return "Caught Signal Abort: Abnormal termination, such as is initiated by the abort function.";
    case SIGFPE:
      return "Caught Signal Floating-Point Exception: Erroneous arithmetic operation, such as zero divide or an operation resulting in overflow (not necessarily with a floating-point operation).";
    case SIGILL:
      return "Caught Signal Illegal Instruction: Invalid function image, such as an illegal instruction. This is generally due to a corruption in the code or to an attempt to execute data.";
    case SIGINT:
      return "Caught Signal Interrupt: Interactive attention signal. Generally generated by the application user.";
    case SIGSEGV:
      return "Caught Signal Segmentation Violation: Invalid access to storage: When a program tries to read or write outside the memory it has allocated.";
    case SIGTERM:
      return "Caught Signal Terminate: Termination request sent to program.";
    default:
      return "Caught unknown signal.";
  }
}

// handle signals to stop the binary
void got_signal(int sigid)
{
  // notify all threads to stop
  running = false;

  system("paplay /opt/aldebaran/share/naoqi/wav/shutdown.wav");

  std::cout << getSignalDescription(sigid) << std::endl;

  if(sigid == SIGTERM || sigid == SIGINT) // graceful stop
  {
    std::cout << "shutdown requested by kill signal" << sigid << std::endl;

    if (already_got_signal) {
      std::cout << "WARNING: received repeated kill signals. Graceful stop was not possible. Will kill." << std::endl;
    } else {
      // remember that we got a signal in case we don't manage to stop the binary gracefully
      already_got_signal = true;
      // stop signal handling for now and give the binary time to stop gracefully
      return;
    }
  }
  else if(sigid == SIGSEGV) // segmentation fault
  {
    std::cerr << "SEGMENTATION FAULT" << std::endl;

    std::cout << "dumping traces" << std::endl;
    Trace::getInstance().dump();
    //StopwatchManager::getInstance().dump("cognition");

    std::cout << "syncing file system..." ;
    sync();
    std::cout << " finished." << std::endl;
  }

  // set the default handler for the signal and forward the signal
  std::signal(sigid, SIG_DFL);
  std::raise(sigid);

}//end got_signal

bool fileExists (const std::string& filename) {
  struct stat buffer;
  return (stat (filename.c_str(), &buffer) == 0);
}

int main(int /*argc*/, char **/*argv[]*/)
{
  std::cout << "=========================================="  << std::endl;
  std::cout << "LoalAdaptor compiled on: " << __DATE__ << " at " << __TIME__ << std::endl;

  system("paplay /opt/aldebaran/share/naoqi/wav/bip_power_on.wav");

  #ifdef REVISION
  std::cout << "Revision number: " << TO_STRING(REVISION) << std::endl;
  #endif
  #ifdef USER_NAME
  std::cout << "Owner: " << TO_STRING(USER_NAME) << std::endl;
  #endif
  #ifdef BRANCH_PATH
  std::cout << "Branch path: " << TO_STRING(BRANCH_PATH) << std::endl;
  #endif
  std::cout << "==========================================\n"  << std::endl;

  // react on "kill" and segmentation fault:
  // Signal     Value     Action   Comment
  // --------------------------------------------------------
  // SIGSEGV      11       Core    Invalid memory reference
  // SIGINT        2       Term    Interrupt from keyboard
  // SIGQUIT       3       Core    Quit from keyboard
  // SIGKILL       9       Term    Kill signal
  //

  // from http://www.cplusplus.com/reference/csignal/signal/:
  // SIGABRT	(Signal Abort)                    Abnormal termination, such as is initiated by the abort function.
  // SIGFPE	  (Signal Floating-Point Exception) Erroneous arithmetic operation, such as zero divide or an operation resulting in overflow (not necessarily with a floating-point operation).
  // SIGILL	  (Signal Illegal Instruction)      Invalid function image, such as an illegal instruction. This is generally due to a corruption in the code or to an attempt to execute data.
  // SIGINT	  (Signal Interrupt)                Interactive attention signal. Generally generated by the application user.
  // SIGSEGV	(Signal Segmentation Violation)   Invalid access to storage: When a program tries to read or write outside the memory it has allocated.
  // SIGTERM	(Signal Terminate)                Termination request sent to program.

  // important
  std::signal(SIGTERM, got_signal);
  std::signal(SIGINT,  got_signal);
  std::signal(SIGSEGV, got_signal);

  std::signal(SIGABRT, got_signal);
  std::signal(SIGFPE,  got_signal);
  std::signal(SIGILL, got_signal);

  // create the controller
  if(fileExists("/usr/bin/lola") || fileExists("/opt/aldebaran/bin/lola"))
  {
    LolaAdaptor theLolaAdaptor;

    // try to start lola
    theLolaAdaptor.start();
    running = true;

    while(running && theLolaAdaptor.isRunning())
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(125));
    }
  }

  return 0;
}//end main
<commit_msg>play shutdown sound only once<commit_after>/**
 * @file main.cpp
 *
 * @author CNR
 */

#include "LolaAdaptor.h"

#include <csignal>
#include <unistd.h>
#include <chrono>
#include <sys/stat.h>

using namespace naoth;
using namespace std;

// std::atomic_int framesSinceCognitionLastSeen;
std::atomic_bool running;
std::atomic_bool already_got_signal;

#define TO_STRING_INT(x) #x
#define TO_STRING(x) TO_STRING_INT(x)

std::string getSignalDescription(int sigid)
{
  switch (sigid)
  {
    case SIGABRT:
      return "Caught Signal Abort: Abnormal termination, such as is initiated by the abort function.";
    case SIGFPE:
      return "Caught Signal Floating-Point Exception: Erroneous arithmetic operation, such as zero divide or an operation resulting in overflow (not necessarily with a floating-point operation).";
    case SIGILL:
      return "Caught Signal Illegal Instruction: Invalid function image, such as an illegal instruction. This is generally due to a corruption in the code or to an attempt to execute data.";
    case SIGINT:
      return "Caught Signal Interrupt: Interactive attention signal. Generally generated by the application user.";
    case SIGSEGV:
      return "Caught Signal Segmentation Violation: Invalid access to storage: When a program tries to read or write outside the memory it has allocated.";
    case SIGTERM:
      return "Caught Signal Terminate: Termination request sent to program.";
    default:
      return "Caught unknown signal.";
  }
}

// handle signals to stop the binary
void got_signal(int sigid)
{
  // notify all threads to stop
  running = false;

  std::cout << getSignalDescription(sigid) << std::endl;

  if(sigid == SIGTERM || sigid == SIGINT) // graceful stop
  {
    std::cout << "shutdown requested by kill signal" << sigid << std::endl;

    if (already_got_signal) {
      std::cout << "WARNING: received repeated kill signals. Graceful stop was not possible. Will kill." << std::endl;
    } else {
      // remember that we got a signal in case we don't manage to stop the binary gracefully
      already_got_signal = true;
      system("paplay /opt/aldebaran/share/naoqi/wav/shutdown.wav");
    // stop signal handling for now and give the binary time to stop gracefully
      return;
    }
  }
  else if(sigid == SIGSEGV) // segmentation fault
  {
    std::cerr << "SEGMENTATION FAULT" << std::endl;

    std::cout << "dumping traces" << std::endl;
    Trace::getInstance().dump();
    //StopwatchManager::getInstance().dump("cognition");

    std::cout << "syncing file system..." ;
    sync();
    std::cout << " finished." << std::endl;
  }

  // set the default handler for the signal and forward the signal
  std::signal(sigid, SIG_DFL);
  std::raise(sigid);

}//end got_signal

bool fileExists (const std::string& filename) {
  struct stat buffer;
  return (stat (filename.c_str(), &buffer) == 0);
}

int main(int /*argc*/, char **/*argv[]*/)
{
  std::cout << "=========================================="  << std::endl;
  std::cout << "LoalAdaptor compiled on: " << __DATE__ << " at " << __TIME__ << std::endl;

  system("paplay /opt/aldebaran/share/naoqi/wav/bip_power_on.wav");

  #ifdef REVISION
  std::cout << "Revision number: " << TO_STRING(REVISION) << std::endl;
  #endif
  #ifdef USER_NAME
  std::cout << "Owner: " << TO_STRING(USER_NAME) << std::endl;
  #endif
  #ifdef BRANCH_PATH
  std::cout << "Branch path: " << TO_STRING(BRANCH_PATH) << std::endl;
  #endif
  std::cout << "==========================================\n"  << std::endl;

  // react on "kill" and segmentation fault:
  // Signal     Value     Action   Comment
  // --------------------------------------------------------
  // SIGSEGV      11       Core    Invalid memory reference
  // SIGINT        2       Term    Interrupt from keyboard
  // SIGQUIT       3       Core    Quit from keyboard
  // SIGKILL       9       Term    Kill signal
  //

  // from http://www.cplusplus.com/reference/csignal/signal/:
  // SIGABRT	(Signal Abort)                    Abnormal termination, such as is initiated by the abort function.
  // SIGFPE	  (Signal Floating-Point Exception) Erroneous arithmetic operation, such as zero divide or an operation resulting in overflow (not necessarily with a floating-point operation).
  // SIGILL	  (Signal Illegal Instruction)      Invalid function image, such as an illegal instruction. This is generally due to a corruption in the code or to an attempt to execute data.
  // SIGINT	  (Signal Interrupt)                Interactive attention signal. Generally generated by the application user.
  // SIGSEGV	(Signal Segmentation Violation)   Invalid access to storage: When a program tries to read or write outside the memory it has allocated.
  // SIGTERM	(Signal Terminate)                Termination request sent to program.

  // important
  std::signal(SIGTERM, got_signal);
  std::signal(SIGINT,  got_signal);
  std::signal(SIGSEGV, got_signal);

  std::signal(SIGABRT, got_signal);
  std::signal(SIGFPE,  got_signal);
  std::signal(SIGILL, got_signal);

  // create the controller
  if(fileExists("/usr/bin/lola") || fileExists("/opt/aldebaran/bin/lola"))
  {
    LolaAdaptor theLolaAdaptor;

    // try to start lola
    theLolaAdaptor.start();
    running = true;

    while(running && theLolaAdaptor.isRunning())
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(125));
    }
  }

  return 0;
}//end main
<|endoftext|>