dqrobotics / cpp Goto Github PK

View Code? Open in Web Editor NEW

35.0 35.0 11.0 1.19 MB

The DQ Robotics library in C++

Home Page: https://dqrobotics.github.io

License: GNU Lesser General Public License v3.0

CMake 2.28% C++ 97.72%

cpp's People

Contributors

Stargazers

Watchers

Forkers

tgn3000 wdfalfred hitersyw maripaf juanjqo marcos-pereira 0000duck andreicostinescu techniccontroller qlin960618 dansjohnson

cpp's Issues

FKM of the DQ_WholeBody does not return intermediate transformations of one of the chains [BUG]

Bug description
Hello, @bvadorno,

I am currently working with the DQ_WholeBody serializing two legs in order to perform a walking algorithm. One of my steps is to calculate the center of mass of the serialized chain. My approach for such calculation starts with the intermediate transformations of each link of the serialized chain. However, when I use functionality fkm(q,joint) it does not work properly.

To Reproduce
I used the code in the two_legs_example to create a minimal example.

Code

    % Include the DQ namespace to use i_ instead of DQ.i, etc.
    include_namespace_dq
    
    % The legs are composed of two KUKA LWR4 robots coupled together    
    right_leg = KukaLwr4Robot.kinematics();
    right_leg.name = 'Right leg';
    left_leg = KukaLwr4Robot.kinematics();
    left_leg.name = 'Left leg';
    
   
    % The left foot will be located 20 cm to the left, along the x axis,
    % but with the z-axis pointing downwards.
    left_foot = (1 - E_ * 0.5 * 0.2 * i_) * i_;
    % The right foot will be located 20 cm to the right, along the x axis,
    % but with the z-axis pointing downwards.
    right_foot = (1 + E_ * 0.5 * 0.2 * i_) * i_;
    
    
    % Physically place the left leg in the workspace
    x_0_to_left_effector = left_leg.fkm(zeros(7,1));
    new_base_frame = left_foot .* T(x_0_to_left_effector);
    left_leg.set_base_frame(new_base_frame);
    % Physically place the right leg in the workspace
    x_0_to_right_effector = right_leg.fkm(zeros(7,1));
    new_base_frame = right_foot .* T(x_0_to_right_effector);
    right_leg.set_base_frame(new_base_frame);
    
    %% Initializes the robot serially coupled kinematic chain from the mobile base
    left_to_right = DQ_WholeBody(left_leg, 'reversed');
    % The base frame is the left foot. The reference frame, also located at
    % the left foot, provides the transformation with respect to the global
    % frame. Therefore, any element in the chain will be given with respect
    % to the global frame.
    left_to_right.set_reference_frame(left_foot);
    left_to_right.set_base_frame(left_foot);
    
    % Determines the rigid transformation between the base frames of the
    % two legs
    base_left_to_base_right = left_leg.base_frame'*right_leg.base_frame;
    % Add this constant transformation to the chain
    left_to_right.add(base_left_to_base_right);    
    % Finally, add the right frame
    left_to_right.add(right_leg);
    
    q = zeros(14,1);
    x1 = left_to_right.fkm(q,1)
    x2 = left_to_right.fkm(q,2)
    x3 = left_to_right.fkm(q,3)
    x4 = left_to_right.fkm(q,4)
    x5 = left_to_right.fkm(q,5)
    x6 = left_to_right.fkm(q,6)

Output

x1 = 
         (1i) + E*(0.1 + 0.55j)
x2 = 
         (1i) + E*( - 0.1 + 0.55j)
x3 = 
         (1i) + E*( - 0.1)
Index exceeds matrix dimensions.

Error in DQ_WholeBody/raw_fkm (line 170)
                if isa(obj.chain{i}, 'DQ_Kinematics')

Error in DQ_WholeBody/fkm (line 142)
                x = obj.reference_frame * raw_fkm(obj,q,ith);

Error in issue_FKM_minimal_example (line 52)
    x4 = left_to_right.fkm(q,4)

Expected behavior

I was expecting the frames of each joint of the first chain (left leg in the minimal example)

Expected output
This is the output of the left leg frames without serialization.

x1 = 
         (0.70711 + 0.70711i) + E*(0.1096j + 0.1096k)
x2 = 
         (1) + E*(0.155k)
x3 = 
         (0.70711 - 0.70711i) + E*( - 0.25102j + 0.25102k)
x4 = 
         (1) + E*(0.355k)
x5 = 
         (0.70711 + 0.70711i) + E*(0.38891j + 0.38891k)
x6 = 
         (1) + E*(0.55k)

Environment:

OS: [Ubuntu 16.04]
dqrobotics last pull date: 28/04/2019
MATLAB version [R2016b]

Implementation of utility functions

Context: There are several utility functions that might be useful in the current development as they increase code readability, brevity, and interoperability between DQ objects and standard types.

For instance, suppose that we have a DQ object dq such that dq = 1.5. If a given function accepts double parameters, instead of doing dq.q(1) in Matlab (or dq.q(0) in C++), it is more appropriate to cast that object to double (i.e.,double(dq)). In addition to providing a cleaner code, we end up with an exact code for both Matlab and C++ without having to worry about indexes, which are different across different languages. (This link shows how to overload typecasts in C++).

For sure, such a cast only makes sense (at least from a semantic point of view), if the DQ objectdq corresponds to a 'strict' real number; that is, a real number with imaginary part equal to zero and dual part equal to zero too. Therefore, in order to increase readability we should also implement functions to verify if a dual quaternion is real, pure, a quaternion, etc.

Suggestion:

implement the casting of a dual-quaternion real-number to double
implement is_pure (1 if real part is 0 and 0 otherwise)
implement is_real (1 if imaginary part is 0 and 1 otherwise)
implement is_real_number (1 if imaginary and dual parts are 0 and 0 otherwise)
implement is_quaternion (1 if dual part is 0 and 0 otherwise)
implement is_pure_quaternion (1 if is_pure and is_quaternion, 0 otherwise)
implement is_line (1 if is_pure and is_unit, 0 otherwise)
implement is_plane (1 if is_unit and is_real(D(x)))

[BUG] Small typo in ([DQ_QuadraticProgrammingController.cpp](line 70)

Hi @mmmarinho, I spotted a small typo. I'm certain that I will forget about it if I don't write about it here.

Bug description

There is a small typo in DQ_QuadraticProgrammingController.cpp (line 70). It is written "feedoforward" instead of "feedforward".
Current:
throw std::runtime_error("Incompatible sizes between task error and feedoforward in compute_tracking_control_signal");
Instead of:
throw std::runtime_error("Incompatible sizes between task error and feedforward in compute_tracking_control_signal");

Code

 DQ_QuadraticProgrammingController.cpp

Environment:

dqrobotics version: C++ (master branch)

[BUG] Rename of is_stable_ and is_stable()

Hi, @mmmarinho,

The variable is_stable_ and the method is_stable() in DQ_KinematicController are not appropriate as 'stability' is an expression in control theory with a very precise meaning. To reflect in C++ the renaming of those variable and method that I did in Matlab, could you please change them to system_reached_stable_region_ and system_reached_stable_region(), respectively?

Cheers,
Bruno

[BUG] Controller method get_task_variable() is always returning zero when using ControlObjective::DistanceToPlane

Hello @mmmarinho, I am having the following bug.

Bug description

The DQ_ClassicQPController controller method get_task_variable() is always returning zero when the ControlObjective::DistanceToPlane is used.

To Reproduce

Code
issue_get_task_variable.cpp

#include <iostream>
#include <vector>

#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include "Jaco.h"

#include <dqrobotics/robot_control/DQ_QuadraticProgrammingController.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <dqrobotics/solvers/DQ_CPLEXSolver.h>

using namespace DQ_robotics;

void test_issue()
{
    // Measured joint angles
    VectorXd qm(6);
    qm << 7.60284,  3.46782,  1.38851,  3.56359,  11.2361, -4.54074;
 
    DQ_SerialManipulator jaco = Jaco::kinematics();

    // Constrained QP controller
    DQ_CPLEXSolver cplex_solver;
    DQ_ClassicQPController controller(&jaco, &cplex_solver);       
    controller.set_stability_threshold(0.0001);
    controller.set_damping(0.01);
    controller.set_control_objective(ControlObjective::DistanceToPlane);
    controller.set_stability_counter_max(100);

    // Target plane
    DQ p_plane1;
    p_plane1 = -0.4897*DQ_robotics::k_;
    const double phi_plane1 = 0.0;
    const DQ n_plane1(0.0, 1.0, 1.0, 1.0);
    const DQ r_plane1 = cos(phi_plane1/2) + n_plane1*sin(phi_plane1/2);
    const DQ kplane1 = r_plane1 + DQ_robotics::E_*(0.5)*p_plane1*r_plane1;
    const DQ plane1 = Adsharp(kplane1,DQ_robotics::k_);

    controller.set_target_primitive(plane1);

    const DQ task_variable = controller.get_task_variable(qm);

    // Task reference
    const DQ task_reference(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);    

    std::cout << "task_variable " << controller.get_task_variable(qm) << "\n";

    std::cout << "task_variable DQ " << task_variable << "\n";

    const VectorXd u_qdot = controller.compute_setpoint_control_signal(qm, VectorXd::Zero(1));

}

int main(void)
{
    test_issue();
    return 0;
}

Jaco.cpp

#ifndef DQ_ROBOTICS_JACO_DH_H
#define DQ_ROBOTICS_JACO_DH_H

#include "Jaco.h"
#include<dqrobotics/utils/DQ_Constants.h>

namespace DQ_robotics
{

DQ_SerialManipulator Jaco::kinematics()
{
    const double pi2 = pi/2.0;

    // Parameters reference: 
    // https://www.kinovarobotics.com/sites/default/files/ULWS-RA-JAC-UG-INT-EN%20201804-1.0%20%28KINOVA%E2%84%A2%20Ultra%20lightweight%20robotic%20arm%20user%20guide%29_0.pdf

    double D1 = 0.2755;
    double D2 = 0.4100;
    double D3 = 0.2073;
    double D4 = 0.0741;
    double D5 = 0.0741;
    double D6 = 0.1600;
    double e2 = 0.0098;

    double aa = (30.0*pi)/180.0;
    double sa = sin(aa);
    double s2a = sin(2*aa);
    double d4b = D3 + (sa/s2a)*D4;
    double d5b = (sa/s2a)*D4 + (sa/s2a)*D5;
    double d6b = (sa/s2a)*D5 + D6;

    Matrix<double,4,6> jaco_dh(4,6);

    jaco_dh <<  0,     pi2,     pi2,      0,      0,    0,  
                -0.1188,    0,     0, -0.252,   -0.079, -0.04,  
                 0,     -0.41,    0,      0,      0,    0, 
                -pi2,   -pi,  -pi2,   -0.3056*pi,   0.3056*pi,   pi; 
    DQ_SerialManipulator jaco(jaco_dh,"standard");

    return jaco;
}

}

#endif

Jaco.h

#ifndef DQ_ROBOTS_JACO_H
#define DQ_ROBOTS_JACO_H

#include<dqrobotics/robot_modeling/DQ_SerialManipulator.h>

namespace DQ_robotics
{

class Jaco
{
public:
    static DQ_SerialManipulator kinematics();
};

}

#endif

CMakeLists.txt

cmake_minimum_required(VERSION 3.1)
project(issue_get_task_variable)

# find_package (Threads REQUIRED)

set (CMAKE_CXX_STANDARD 11)
# add_compile_options(-std=c++14)

set( CPLEX_PATH /opt/ibm/ILOG/CPLEX_Studio129 )

add_definitions(-DIL_STD)

link_directories(
    ${CPLEX_PATH}/cplex/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/concert/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/bin/glnxa64
    )

include_directories(
    ${CPLEX_PATH}/concert/include
    ${CPLEX_PATH}/cplex/include
    )

add_executable(${PROJECT_NAME} issue_get_task_variable.cpp)

add_library(Jaco
  Jaco.cpp
)

target_link_libraries(${PROJECT_NAME}  
  # pthread
  # Threads::Threads
  Jaco
  -ldqrobotics  
  ilocplex
  cplex
  concert
  m
  pthread
  ${CMAKE_DL_LIBS}
  # dl  
)

Output

task_variable  0
task_variable DQ 0 0i 0j 0k +E( 0 0i 0j 0k )
Going to solve quadratic program!

Expected behavior
Since the ControlObjective::DistanceToPlane is being used, the get_task_variable() should return a VectorXd with a size of 1. The returning value should be the point_to_plane_distance() between the robot effector and the target_primitive_. This is expected, as it can be seen in the get_task_variable() method in DQ_KinematicController.cpp.
However, I tried different values of p_plane1 and the get_task_variable() is always returning zero for the given qm.

Expected output

task variable <value possibly different from zero>
task_variable DQ <non-zero DQ>
Going to solve quadratic program!

Environment:

OS: Ubuntu 18.04
dqrobotics version 19.10.1-0~~202002140719~~ubuntu18.04.1

Best regards,

Marcos

[LANGUAGE INCOMPATIBILITY] The DistanceToPlane control objective is missing.

Hello @mmmarinho, would it be possible to add the DistanceToPlane control objective to the C++ implementation?

Describe the missing/unexpected functionality
The DistanceToPlane control objective is missing on the C++ implementation.

Matlab behavior (if applicable)
No execution errors occur.

MATLAB SCRIPT

clc
close all
clear all

include_namespace_dq

kuka = KukaLwr4Robot.kinematics();

solver = DQ_QuadprogSolver;

plane = k_;

controller = DQ_ClassicQPController(kuka,solver);
controller.set_gain(10);
controller.set_stability_threshold(0.0001);
controller.set_damping(0.01);
controller.set_control_objective(ControlObjective.DistanceToPlane);

controller.set_max_stability_counter(100);
controller.set_target_primitive(plane);

MATLAB OUTPUT

<No output expected>

C++ behavior (if applicable)
The control objective DistanceToPlane is not found.
The method set_max_stability_counter is not found.
The method set_target_primitive is not found.

C++ Code

#include <iostream>

#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
#include <dqrobotics/robot_control/DQ_QuadraticProgrammingController.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <dqrobotics/solvers/DQ_CPLEXSolver.h>

using namespace DQ_robotics;

void test_issue()
{
    const DQ plane = DQ_robotics::k_;
 
    DQ_SerialManipulator robot = KukaLw4Robot::kinematics();

    // Constrained QP controller
    DQ_CPLEXSolver cplex_solver;
    DQ_ClassicQPController controller(&robot, &cplex_solver);       
    controller.set_control_objective(ControlObjective::DistanceToPlane);
    controller.set_gain(10.0);
    controller.set_damping(0.01);
    controller.set_max_stability_counter(100);
    controller.set_target_primitive(plane);

}

int main(void)
{
    test_issue();
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.1)
project(issue_DistanceToPlane)

# find_package (Threads REQUIRED)

set (CMAKE_CXX_STANDARD 11)
# add_compile_options(-std=c++14)

set( CPLEX_PATH /opt/ibm/ILOG/CPLEX_Studio129 )

add_definitions(-DIL_STD)

link_directories(
    ${CPLEX_PATH}/cplex/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/concert/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/bin/glnxa64
    )

include_directories(
    ${CPLEX_PATH}/concert/include
    ${CPLEX_PATH}/cplex/include
    )

add_executable(${PROJECT_NAME} issue_DistanceToPlane.cpp)

target_link_libraries(${PROJECT_NAME}  
  # pthread
  # Threads::Threads
  -ldqrobotics  
  ilocplex
  cplex
  concert
  m
  pthread
  ${CMAKE_DL_LIBS}
  # dl  
)

C++ OUTPUT
The compilation throws the following errors.

issue_DistanceToPlane.cpp:20:56: error: ‘DistanceToPlane’ is not a member of ‘DQ_robotics::ControlObjective’
     controller.set_control_objective(ControlObjective::DistanceToPlane);

...

issue_DistanceToPlane.cpp:23:16: error: ‘class DQ_robotics::DQ_ClassicQPController’ has no member named ‘set_max_stability_counter’
     controller.set_max_stability_counter(100);

...
issue_DistanceToPlane.cpp:24:16: error: ‘class DQ_robotics::DQ_ClassicQPController’ has no member named ‘set_target_primitive’; did you mean ‘attached_primitive_’?
     controller.set_target_primitive(plane);
                ^~~~~~~~~~~~~~~~~~~~
                attached_primitive_

Environment:

OS: ubuntu 18.04
C++ dqrobotics version: 19.10.0-0 201910290536 ubuntu18.04.1
MATLAB version 2018a

Additional context
The DistanceToPlane control objective was recently added to the DQ Robotics in Matlab.
The DistanceToPlane is defined on the DQ_KinematicController.m, but it is not defined on the DQ_KinematicController.h.

[QUESTION] Eigen UnalignedArrayAssert because of DQ_SerialManipulator during runtime on development PPA

Bug description

Eigen is throwing an UnalignedArrayAssert when a DQ_SerialManipulator is declared using the development PPA

To Reproduce

Code

robot_interface_node.cpp

...
#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
...
ROS_INFO_STREAM("TEST");
DQ_SerialManipulator robot_kinematics = KukaLw4Robot::kinematics();
ROS_INFO_STREAM("TEST");

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(robot_interface)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++14)
...
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
...
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/robot_interface_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
 add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )
target_link_libraries(${PROJECT_NAME}_node
					 ${catkin_LIBRARIES}
					 -ldqrobotics
)

Output
rosrun robot_interface robot_interface_node

[ INFO] [1563287024.186883840]: TEST
robot_interface_node: /usr/local/include/Eigen/src/Core/DenseStorage.h:109: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]: Assertion `(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.
Aborted (core dumped)

Expected behavior

The DQ_SerialManipulator should be declared and no runtime errors should appear.

Expected output

[ INFO] [1563287024.186883840]: TEST
[ INFO] [1563287024.186883840]: TEST

Environment:

OS: Ubuntu 18.04
dqrobotics version - development PPA: 0.1-0~ 201906250359 ~ubuntu18.04.1
ROSdistro: melodic
ROSversion: 1.14.3
Eigen version: libeigen3-dev (3.3.4-4)

Additional context

Eigen suggests to read this and run the code with gdb.
I ran the code on gdb and obtained the following results with the gdb command bt.
I also tried to compile the code with c++17, but it did not work.

Reading symbols from ./devel/lib/robot_interface/robot_interface_node...(no debugging symbols found)...done.

(gdb) run

Starting program: /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/robot_interface/robot_interface_node 
[Thread debugging using libthread_db enabled]
Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1".
[New Thread 0x7ffff0019700 (LWP 25634)]
[New Thread 0x7fffef818700 (LWP 25635)]
[New Thread 0x7fffef017700 (LWP 25636)]
[New Thread 0x7fffee816700 (LWP 25637)]
[ WARN] [1563292402.495467043]: Robot interface initializing...
[ INFO] [1563292402.500789607]: TESTE
robot_interface_node: /usr/local/include/Eigen/src/Core/DenseStorage.h:109: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]: Assertion `(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.

Thread 1 "robot_interface" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
51	../sysdeps/unix/sysv/linux/raise.c: No such file or directory.

(gdb) bt

#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1  0x00007ffff5c6d801 in __GI_abort () at abort.c:79
#2  0x00007ffff5c5d39a in __assert_fail_base (fmt=0x7ffff5de47d8 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", 
    assertion=assertion@entry=0x7ffff7aa5910 "(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && \"this assertion is explained here: \" \"http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.htm"..., 
    file=file@entry=0x7ffff7aa58d8 "/usr/local/include/Eigen/src/Core/DenseStorage.h", line=line@entry=109, 
    function=function@entry=0x7ffff7aa5b20 <Eigen::internal::plain_array<double, 8, 0, 16>::plain_array()::__PRETTY_FUNCTION__> "Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]")
    at assert.c:92
#3  0x00007ffff5c5d412 in __GI___assert_fail (
    assertion=0x7ffff7aa5910 "(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && \"this assertion is explained here: \" \"http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.htm"..., 
    file=0x7ffff7aa58d8 "/usr/local/include/Eigen/src/Core/DenseStorage.h", line=109, 
    function=0x7ffff7aa5b20 <Eigen::internal::plain_array<double, 8, 0, 16>::plain_array()::__PRETTY_FUNCTION__> "Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 8; int MatrixOrArrayOptions = 0]") at assert.c:101
#4  0x00007ffff79cf0b9 in Eigen::internal::plain_array<double, 8, 0, 16>::plain_array() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#5  0x00007ffff79ce790 in Eigen::DenseStorage<double, 8, 8, 1, 0>::DenseStorage() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#6  0x00007ffff79cd9da in Eigen::PlainObjectBase<Eigen::Matrix<double, 8, 1, 0, 8, 1> >::PlainObjectBase() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#7  0x00007ffff79cd240 in Eigen::Matrix<double, 8, 1, 0, 8, 1>::Matrix() ()
   from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
#8  0x00007ffff79c0dfa in DQ_robotics::DQ::DQ(double const&, double const&, double const&, double const&, double const&, double const&, double const&, double const&) () from /home/marcos/git/mastermsp/manipulation-planning-v2/catkin_ws/devel/lib/libdq_robotics.so
---Type <return> to continue, or q <return> to quit---
#9  0x00007ffff6aed704 in DQ_robotics::DQ_Kinematics::DQ_Kinematics (this=0x7fffffffcf50)
    at /usr/src/dqrobotics/robot_modeling/DQ_Kinematics.cpp:32
#10 0x00007ffff6af4439 in DQ_robotics::DQ_SerialManipulator::DQ_SerialManipulator (this=0x7fffffffcf50, dh_matrix=..., 
    convention="standard") at /usr/src/dqrobotics/robot_modeling/DQ_SerialManipulator.cpp:51
#11 0x00007ffff6afe909 in DQ_robotics::KukaLw4Robot::kinematics () at /usr/src/dqrobotics/robots/KukaLw4Robot.cpp:41
#12 0x00005555555a6b58 in ControllingNode::ROSControlLoop(int) ()
#13 0x00005555555ae4f0 in main ()

I followed the instructions on how to run the cpp-examples and the performance evaluation example worked without errors. It also uses the DQ_SerialManipulator. However, as you know, it includes the DQ header from the source code and not from the PPA.

[BUG] rotation_angle is returning "nan"

Bug description

When running a kinematic controller, after the system stabilizes with the error close to zero (1-invariant error), the rotation angle of the dual quaternion error is becoming "nan".
If a dual quaternion has a unit norm, and the real part of the rotation is equal to 1, the rotation angle should be equal to zero. However, the function rotation_angle is returning "nan".

To Reproduce

Running the following code you will see that the running stops after some iterations, when the rotation angle becomes "nan".

Code

#include <Eigen/Dense>
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <iostream>
#include <cmath>

using namespace DQ_robotics;

int main()
{
    // robot definition
    Matrix<double, 4, 7> kuka_dh;
    kuka_dh << 0, 0, 0, 0, 0, 0, 0, // theta
               0.3105, 0, 0.4, 0, 0.39, 0, 0, // d
               0, 0, 0, 0, 0, 0, 0, // a
               0, M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, M_PI_2, -M_PI_2; // alpha
    DQ_SerialManipulator kuka(kuka_dh,"modified");

    int num_dof = kuka.get_dim_configuration_space();
    VectorXd q(num_dof), q_dot(num_dof);
    q << 0., 0.5, 0., -1.2, 0., 0., 0.;
    q_dot << 0, 0, 0, 0, 0, 0, 0;

    Eigen::MatrixXd J = kuka.pose_jacobian(q);
    Eigen::MatrixXd N(8,num_dof);
    DQ x = kuka.fkm(q);

    VectorXd q_desired(num_dof);
    q_desired << 0., 0.5, 0.5, -1.2, 0., 0., 0.;
    DQ xd = kuka.fkm(q_desired);

    DQ x_til = x.conj()*xd;

    double k = 5;
    double sampling_time = 0.005;
    int i = 0;

    // initialize rotation angle
    double phi = 0.;

    while (i < 2000) {

        ++i;

        x = kuka.fkm(q);
        J = kuka.pose_jacobian(q);

        // invariant error
        x_til = x.conj()*xd;

        // check rotation angle
        phi = x_til.rotation_angle();
        if (std::isnan(phi)) {
            std::cout << "phi is not real" << std::endl;
            std::cout << "phi = " << phi << std::endl;
            std::cout << "is unit: " << is_unit(x_til) << std::endl;
            std::cout << "real: " << Re(P(x_til)) << std::endl;
            return 1;
        }

        // first order kinematic control
        N = haminus8(xd)*C8()*J;
        q_dot = pinv(N)*(k*vec8(1-x_til));
        q = sampling_time*q_dot + q;

        // error
        std::cout << (1 - x.conj()*xd).vec8().norm() << std::endl;
    }

    // exit program
    return 0;
}

Output

phi = nan

Expected behavior

Since the dual quaternion has a unit norm, and Re(P(x_til)) is equal to one, I expected the rotation angle to be zero.

Expected output

phi = 0

Environment:

OS: Ubuntu 16.04
dqrobotics version 0.1.0

Additional context

This problem does not affect the classic kinematic control, but it affects anything that uses rotation, as the logarithm for example. A similar problem occurs in MATLAB version.

Implementation of set_reference_frame() and set_base_frame()

Context: In the class DQ_kinematics, the method set_base is always given with respect to the identity frame (i.e., the global reference one). This has been used to define the physical location of the robot with respect to the global reference frame (in Matlab, for instance, set_base is extensively used in the function plot(), but the issue I'll describe below is independent of the programming language). This is particularly important when we have several robots in the workspace and we want to determine their physical location with respect to a common frame, but we want to perform motion control on a local frame (for example, each robot uses the frame located at its base as the reference frame).

Therefore, sometimes it is useful to use a reference frame that does not coincide with the base frame, which means that the robot is physically located in one place, but the reference frame can be another one (e.g., for example, a camera reference frame). This is particularly important in serially coupled kinematic chains (i.e., complete kinematic chains coupled together to form a larger one) because a particular kinematic chain's end-effector pose is given with respect to the previous kinematic chain's end-effector, not with respect to the global reference frame.

Proposed solution: Use two distinct variables, namely base_frame and reference_frame, where the former is used to physically determine the robot in the workspace and the latter is the reference frame used to determine the end-effector pose. Furthermore, we need to implement the methods

set_reference_frame(reference_frame);
set_base_frame(base_frame).

Since both fkm() and pose_jacobian() methods use the transformation given by base_frame, they should be adapted accordingly to use reference_frame instead.

Side-effects: Small, but we need to implement the deprecated function set_base, which must refer to set_reference_frame.

I already implemented all those changes in the mobile_base branch. I intend to merge them with the master branch after we decide on the naming conventions related to this issue and issue #8.

Cheers,
Bruno

Directory structure

Context: Directory structure should be the same across all DQ Robotics languages.

The current Matlab folder structure is given as follows:

Folder name	Description
`@DQ`	Contains the DQ class and all the associated methods spread across different files. Since this class is quite complex and have several methods and mathematical operations, in Matlab we need to use the @-type folder to split the methods across different files.
examples	Contains several examples on how to use DQ Robotics in Matlab. The internal folder structure depends on the language because some examples do not make so much sense in some languages (for instance, plot function usage).
legacy	Legacy classes and methods that will be removed in the next major library improvement.
robot_control	Classes for robot control. Since the architecture of the `DQ_KinematicController` will be changed in the future, such that we have one abstract superclass and several concrete subclasses, each one implementing a specific controller, it is a good idea to place all of them in a specific folder.
robot_modeling	Classes for robot modeling. Since now we have several robot classes, and not only Serial Manipulators anymore, it is a good idea to place all of them in one separate directory. I used the American spelling for 'modeling'. Currently, the C++ version is written in British spelling. I suggest we stick with just one. 😃
robots	Robot definitions currently supported by DQ Robotics
utils	Utility functions and classes.
vrep_interface	V-Rep classes

Suggestion: Replicate this directory structure in C++.

[QUESTION] Ubuntu 14.04 Compatibility

Bug description

I am unable to compile at Ubuntu 14.04 due to minimum cmake version requirement.

Hotfix:

It was quite simple to solve. I just changed from 3.4 to 2.8 as show bellow and worked just fine.

cpp/CMakeLists.txt

Line 1 in c2154c0

CMAKE_MINIMUM_REQUIRED(VERSION 3.4)

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)

Then I tested using travis for manual compilation with something like:

language: generic
matrix:
  include:
  - name: "Trusty indigo"
    dist: trusty
  - name: "Trusty jade"
    dist: trusty
  - name: "Xenial kinetic"
    dist: xenial

before_script:
  - sudo apt-get install -y cmake libeigen3-dev # DQRobotics

script:
  - mkdir build
  - cd build
  - cmake ..
  - make

To ensure it would not have any trouble.

Environment:

OS: [Ubuntu 14.04, ,Ubuntu 16.04, Ubuntu 18.04]
dqrobotics current

Implementation of mobile base classes

@mmmarinho ,

I finished the implementation of DQ_MobileBase, DQ_HolonomicBase, DQ_DifferentialDriveRobot, and DQ_WholeBody in Matlab (branch mobile_base).
Could you please implement those classes in C++?

Thanks,
Bruno

Bug in line_to_line_squared_distance

Bug description

In file DQ_Geometry.cpp, function line_to_line_squared_distance, the squared distance only works for non-parallel lines. It lacks the implementation of Eq. (46) of M. M. Marinho, B. V. Adorno, K. Harada, and M. Mitsuishi, “Dynamic Active Constraints for Surgical Robots using Vector Field Inequalities,” Apr. 2018.

Furthermore, according to Eq. (41), line 131 should be
return std::pow(static_cast<double>(b/a),2);
instead of
return std::pow(static_cast<double>(a/b),2);

Cheers,
Bruno

[QUESTION] Is there a way to get the ith link x of a DQ_SerialManipulatorDH during runtime?

Hello,

I would like to know if there is a way to get the ith link x(pose) of a DQ_SerialManipulatorDH during runtime.
（In the case of 7th link manipulator, I would like to get 2th link x or 5th link x.）

I know to get top pose from fkm.

Thank you

DQ_Kinematics::point_to_line_segment / line_segment_to_point

Murilo sensei, I think having these jacobians can make many problems simpler. I wish these methods could be added if you have any time and are pleased to do so. :>

static MatrixXd point_to_line_segment_squared_distance_jacobian(const MatrixXd& robot_point_jacobian,
    const DQ& robot_point,
    const DQ& workspace_line,
    const DQ& workspace_point_1,
    const DQ& workspace_point_2)
{
    DQ projection_point = workspace_point_1 + P(workspace_line) * dot(P(workspace_line), (robot_point - workspace_point_1));
    double d_projection_point_to_line_point1 = norm(vec3(projection_point - workspace_point_1));
    double d_projection_point_to_line_point2 = norm(vec3(projection_point - workspace_point_2));
    double line_segment_length = norm(vec3(workspace_point_1 - workspace_point_2));
    if (d_projection_point_to_line_point1 <= line_segment_length && d_projection_point_to_line_point2 <= line_segment_length)
        // line_state = body
        return DQ_Kinematics::point_to_line_distance_jacobian(robot_point_jacobian, robot_point, workspace_line);
    else if (d_projection_point_to_line_point1 < d_projection_point_to_line_point2)
        // point 1 
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_jacobian, robot_point, workspace_point_1);
    else
        // point 2
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_jacobian, robot_point, workspace_point_2);
}

static MatrixXd line_segement_to_point_squared_distance_jacobian(const MatrixXd& robot_line_jacobian,
    const MatrixXd& robot_point_1_translation_jacobian,
    const MatrixXd& robot_point_2_translation_jacobian,
    const DQ& robot_line,
    const DQ& robot_point_1,
    const DQ& robot_point_2,
    const DQ& workspace_point)
{
    DQ projection_point = robot_point_1 + P(robot_line) * dot(P(robot_line), (workspace_point - robot_point_1));
    double d_projection_point_to_line_point1 = norm(vec3(projection_point - robot_point_1));
    double d_projection_point_to_line_point2 = norm(vec3(projection_point - robot_point_2));
    double line_segment_length = norm(vec3(robot_point_1 - robot_point_2));
    if (d_projection_point_to_line_point1 <= line_segment_length && d_projection_point_to_line_point2 <= line_segment_length)
        // line_state = body
        return DQ_Kinematics::line_to_point_distance_jacobian(robot_line_jacobian, robot_line, workspace_point);
    else if (d_projection_point_to_line_point1 < d_projection_point_to_line_point2)
        // point 1 
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_1_translation_jacobian, robot_point_1, workspace_point);
    else
        // point 2
        return DQ_Kinematics::point_to_point_distance_jacobian(robot_point_2_translation_jacobian, robot_point_2, workspace_point);
}

[QUESTION] Up-to-date DQ Robotics headers not being found when building with catkin_make

Bug description

The up-to-date DQ Robotics headers are not being found during compilation with catkin_make.

To Reproduce

Code

robot_interface_node.cpp

...
// For example the DQ_Kinematics.h
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
...

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(robot_interface)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++14)
...
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
...
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/robot_interface_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
 add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )
target_link_libraries(${PROJECT_NAME}_node
					 ${catkin_LIBRARIES}
					 -ldqrobotics
)

Compilation command

catkin_make

Output

fatal error: dqrobotics/robot_modeling/DQ_Kinematics.h: No such file or directory
 #include <dqrobotics/robot_modeling/DQ_Kinematics.h>

Expected behavior

The DQ Robotics headers should be found.

Environment:

OS: Ubuntu 18.04
dqrobotics version 0.1-0 69 ubuntu18.04.1
ROSdistro: melodic
ROSversion: 1.14.3

Additional context

I installed the DQ Robotics through the PPA with the instructions available on the DQ Robotics webpage. That is, I used the following commands:

sudo add-apt-repository ppa:dqrobotics-dev/release
sudo apt-get update
sudo apt-get install libdqrobotics

When I run the install command line, the following appears and indicate the libdqrobotics is installed:

Reading package lists... Done
Building dependency tree       
Reading state information... Done
libdqrobotics is already the newest version (0.1-0~69~ubuntu18.04.1).
0 upgraded, 0 newly installed, 0 to remove and 12 not upgraded.

Cooperative framework is missing in C++.

Hi Murilo,

I realized that the cooperative dual space framework is implemented on Matlab (@DQ_cdts) but is not available for C++. It is possible add this functionality in the C++ version?

Cheers,

Juancho.

Implementation of raw_jacobian

Hi, Murilo

I've just implemented the function raw_jacobian on Matlab and adapted the Jacobian function accordingly. The raw_jacobian calculates the Jacobian matrix without taking into consideration any constant base and end-effector displacements. In addition, following Juancho's request (Ticket 27 on SourceForge), now we have the option to use robot.jacobian(q, ith), which calculates the Jacobian up to the i-th link. Could you please implement those functionalities in C++?

Cheers,
Bruno

[QUESTION] Is there a way to get the finite line from Plucker line?

Hello,

I would like to know if there is a way to get the finite line from Plucker line.
I image not to collide like between just only real shaft part (line) and other real shaft part (line).

double DQ_Geometry::line_to_line_squared_distance(const DQ& line1, const DQ& line2)

https://github.com/dqrobotics/cpp/blob/77acf9a42875ffb69e9f48f98f3950f9d7242c0e/src/utils/DQ_Geometry.cpp#L118C21-L118C50

Thank you !

[BUG] Logarithm of a unit dual quaternion is returning `nan`

Bug description

Similar to #25 :
When running a kinematic controller, after the system stabilizes with the error close to zero (1-invariant error), the logarithm of the dual quaternion error is returning "nan".

To Reproduce

Running the following code you will see that the running stops after some iterations, when the logarithm becomes "nan".

Code

#include <Eigen/Dense>
#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <iostream>
#include <cmath>

using namespace DQ_robotics;

int main()
{
    // robot definition
    Matrix<double, 4, 7> kuka_dh;
    kuka_dh << 0, 0, 0, 0, 0, 0, 0, // theta
               0.3105, 0, 0.4, 0, 0.39, 0, 0, // d
               0, 0, 0, 0, 0, 0, 0, // a
               0, M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, M_PI_2, -M_PI_2; // alpha
    DQ_SerialManipulator kuka(kuka_dh,"modified");

    int num_dof = kuka.get_dim_configuration_space();
    VectorXd q(num_dof), q_dot(num_dof);
    q << 0., 0.5, 0., -1.2, 0., 0., 0.;
    q_dot << 0, 0, 0, 0, 0, 0, 0;

    Eigen::MatrixXd J = kuka.pose_jacobian(q);
    Eigen::MatrixXd N(8,num_dof);
    DQ x = kuka.fkm(q);

    VectorXd q_desired(num_dof);
    q_desired << 0., 0.5, 0.5, -1.2, 0., 0., 0.;
    DQ xd = kuka.fkm(q_desired);

    DQ x_til = x.conj()*xd;

    double k = 5;
    double sampling_time = 0.005;
    int i = 0;

    // initialize rotation angle
    DQ test;

    while (i < 2000) {

        ++i;

        x = kuka.fkm(q);
        J = kuka.pose_jacobian(q);

        // invariant error
        x_til = x.conj()*xd;

        // check log
        test = log(x_til);
        if (std::isnan(test.q(0))) {
            std::cout << test << std::endl;
            return 1;
        }

        // first order kinematic control
        N = haminus8(xd)*C8()*J;
        q_dot = pinv(N)*(k*vec8(1-x_til));
        q = sampling_time*q_dot + q;

        // error
        std::cout << (1 - x.conj()*xd).vec8().norm() << std::endl;
    }

    // exit program
    return 0;
}

Output

nan nani nanj nank +E( 0 0i -2.26944e-09j 0k )

Expected behavior

I expected the rotation angle to be zero.

Expected output

0 0i 0j 0k +E( 0 0i -2.26944e-09j 0k )

Environment:

OS: Ubuntu 16.04
dqrobotics version 0.1.0

Additional Information:

the log function in C++ is using the acos function instead of rotation_angle.

Implementation of abstract class DQ_Kinematics

Following the discussion https://github.com/orgs/dqrobotics/teams/developers/discussions/1, we must implement the abstract class DQ_Kinematics, change the name of DQ_kinematics to DQ_SerialManipulator and adapt it according to the new architecture. The implementation in Matlab is currently in branch _mobile_base. I suggest creating a new branch in C++ to implement those functionalities in order to ensure that the master branch remains stable.

Cheers,
Bruno

[QUESTION] Is there a way to change the robot of a DQ_SerialManipulator during runtime?

Hello,

I would like to know if there is a way to change the robot of a DQ_SerialManipulator during runtime.

Something like:

DQ_SerialManipulator robot_serial_manipulator();
...
robot_serial_manipulator = KukaLw4Robot::kinematics();
...
robot_serial_manipulator = AnotherRobot::kinematics();
...
robot_serial_manipulator = MyOtherRobot::kinematics();
...

This yields an error of the following type

error: cannot convert ‘DQ_robotics::DQ_SerialManipulator’ to ‘DQ_robotics::DQ_SerialManipulator()’ in assignment

My goal is to be able to use the robot_serial_manipulator overall in the code without having to declare a DQ_SerialManipulator for each robot. I want to use the same code for different robots chosen during runtime.

Stable PPA
DQ Robotics version: 19.10.0-0~~201910030916~~ubuntu18.04.1

Thank you,

Marcos

distance_jacobians and residuals

Context: Signature parameters are unnecessarily verbose, the implementation is not "language-proof", and utility functions should be used to obtain a cleaner code.

Suggestions:

Change variables such as robot_point_translation to robot_point.
Instead of using return dq.q(0), use the casting to double proposed in #14 to obtain a "language-proof" solution.
Use the utility functions to verify if, for instance, a given dual quaternion is just a pure quaternion (is_pure_quaternion(dq)).
Verify if all geometric primitives passed as parameters fulfill their respective requirements. For instance, a point must be a pure quaternion whereas a plane must have unit norm and the imaginary component of its dual part must be zero, etc.
In point_to_plane_distance_jacobian, it is not necessary to compute const DQ d = D(workspace_plane);
plane_to_point_distance_jacobian does not need the robot_plane argument. Furthermore, DQ n_pi = P(robot_plane) and DQ d_pi = D(robot_plane) can be removed;
Fix the bugs listed below.

Specific Points (I may edit the issue in the very near future to accommodate other bugs, in case I encounter more of them):

I think that point_to_point_residual is returning the wrong value (it should be q(0) instead of q(1), right?). The casting to double will prevent those kinds of minor-but-very-annoying mistakes.
line_to_line_distance_jacobian currently only implements the Jacobian for the case where the lines are not parallel.
line_to_line_residual currently only implements the residual for the case where the lines are not parallel.

Implementation of jacobian_dot and the corresponding example

Hi Murilo,

I've just implemented the function jacobian_dot to calculate the time derivative of the Jacobian matrix in Matlab (Ticket 30 of Sourceforge). I also implemented an example (jacobian_time_derivative.m) to compare the analytical expression with a numerical one. Could you please implement the jacobian_dot method, which in the Matlab version is inside @DQ_kinematics, and also a simple example like the one I mentioned?

Thanks,
Bruno

Geometric primitives Jacobians for C++

Hi @mmmarinho and @bvadorno !

There are plans (in the very near future) to include the geometric primitives Jacobians (line-line, point-line, plane-point...etc, as described in the paper Active Constraints Using Vector Field Inequalities for Surgical Robots in the DQ Robotics (For Matlab, C++ and Python)?
I have my personal implementation on Matlab but I want to use the Python version.

Best regards!

Juancho

Robot naming

Context: Naming convention should follow the Google Python Style Guide, but currently in both C++ and Matlab, functions for creating robots are not following the convention.

For instance, in C++, a KUKA robot is currently declared as DQ_kinematics kuka = KukaKinematics()
and in Matlab it is declared as kuka = DQ_KUKA.

Suggestion: Use the following convention for functions used to create new robots:
new_robotname_robot

Therefore, a Kuka robot in C++ would be declared as DQ_kinematics kuka = new_kuka_robot() and in Matlab as kuka = new_kuka_robot.

Cheers,
Bruno

[BUG] DQ_KinematicController::get_task_variable() is returning a VectorXd with wrong size

Bug description

The DQ_KinematicController::get_task_variable() is returning a VectorXd with wrong size in comparison to the task_reference. Thus, DQ_QuadraticProgrammingController::compute_setpoint_control_signal() is throwing a runtime_error saying task_reference and task_variable have incompatible sizes.

To Reproduce

Code
CMakeLists.txt

cmake_minimum_required(VERSION 3.1)
project(issue_get_task_variable)

# find_package (Threads REQUIRED)

set (CMAKE_CXX_STANDARD 11)
# add_compile_options(-std=c++14)

set( CPLEX_PATH /opt/ibm/ILOG/CPLEX_Studio129 )

add_definitions(-DIL_STD)

link_directories(
    ${CPLEX_PATH}/cplex/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/concert/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/bin/glnxa64
    )

include_directories(
    ${CPLEX_PATH}/concert/include
    ${CPLEX_PATH}/cplex/include
    )

add_executable(${PROJECT_NAME} issue_get_task_variable.cpp)

add_library(Jaco
  Jaco.cpp
)

target_link_libraries(${PROJECT_NAME}  
  # pthread
  # Threads::Threads
  Jaco
  -ldqrobotics  
  ilocplex
  cplex
  concert
  m
  pthread
  ${CMAKE_DL_LIBS}
  # dl  
)

Jaco.h

#ifndef DQ_ROBOTS_JACO_H
#define DQ_ROBOTS_JACO_H

#include<dqrobotics/robot_modeling/DQ_SerialManipulator.h>

namespace DQ_robotics
{

class Jaco
{
public:
    static DQ_SerialManipulator kinematics();
};

}

#endif

Jaco.cpp

#ifndef DQ_ROBOTICS_JACO_DH_H
#define DQ_ROBOTICS_JACO_DH_H

#include "Jaco.h"
#include<dqrobotics/utils/DQ_Constants.h>

namespace DQ_robotics
{

DQ_SerialManipulator Jaco::kinematics()
{
    const double pi2 = pi/2.0;

    // Parameters reference: 
    // https://www.kinovarobotics.com/sites/default/files/ULWS-RA-JAC-UG-INT-EN%20201804-1.0%20%28KINOVA%E2%84%A2%20Ultra%20lightweight%20robotic%20arm%20user%20guide%29_0.pdf

    double D1 = 0.2755;
    double D2 = 0.4100;
    double D3 = 0.2073;
    double D4 = 0.0741;
    double D5 = 0.0741;
    double D6 = 0.1600;
    double e2 = 0.0098;

    double aa = (30.0*pi)/180.0;
    double sa = sin(aa);
    double s2a = sin(2*aa);
    double d4b = D3 + (sa/s2a)*D4;
    double d5b = (sa/s2a)*D4 + (sa/s2a)*D5;
    double d6b = (sa/s2a)*D5 + D6;

    Matrix<double,4,6> jaco_dh(4,6);

    jaco_dh <<  0,     pi2,     pi2,      0,      0,    0,  
                -0.1188,    0,     0, -0.252,   -0.079, -0.04,  
                 0,     -0.41,    0,      0,      0,    0, 
                -pi2,   -pi,  -pi2,   -0.3056*pi,   0.3056*pi,   pi; 
    DQ_SerialManipulator jaco(jaco_dh,"standard");

    return jaco;
}

}

#endif

issue_get_task_variable.cpp

#include <iostream>
#include <vector>

#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include "Jaco.h"

#include <dqrobotics/robot_control/DQ_QuadraticProgrammingController.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <dqrobotics/solvers/DQ_CPLEXSolver.h>

using namespace DQ_robotics;

void test_issue()
{
    // Measured joint angles
    VectorXd qm(6);
    qm << 2.17982, 3.65219, 1.456, 3.50779, 5.10436, -3.73721;
 
    DQ_SerialManipulator jaco = Jaco::kinematics();

    // Constrained QP controller
    DQ_CPLEXSolver cplex_solver;
    DQ_ClassicQPController controller(&jaco, &cplex_solver);       
    controller.set_control_objective(ControlObjective::Plane);
    controller.set_primitive_to_effector(DQ_robotics::k_);
    controller.set_gain(10.0);
    controller.set_damping(0.001);

    const DQ task_variable = controller.get_task_variable(qm);

    // Task reference
    const DQ task_reference(1, 0, 0, 0, 0, 0.1, 0.1, 0.1);    

    std::cout << "task_variable " << controller.get_task_variable(qm) << "\n";

    std::cout << "task_variable DQ " << task_variable << "\n";

    const VectorXd u_qdot = controller.compute_setpoint_control_signal(qm, vec8(task_reference));

}

int main(void)
{
    test_issue();
    return 0;
}

Output

./issue_get_task_variable 
task_variable            0
-0.000112916
 0.000183813
           1
task_variable DQ 0 -0.000112916i 0.000183813j 1k +E( 0 0i 0j 0k )
terminate called after throwing an instance of 'std::runtime_error'
  what():  Incompatible sizes between task variable and task reference in compute_setpoint_control_signal
Aborted (core dumped)

Expected behavior

The compute_setpoint_control_signal() should calculate a control signal or throw an error saying it is unfeasible.

Environment:

OS: Ubuntu 18.04
dqrobotics version 19.10.0-0 201910230920 ubuntu18.04.1
CPLEX version: cplex_studio129

Additional context

Note that if we print the output of get_task_variable(), it returns a VectorXd with four elements, that is, the primary part of the quaternion. If we declare a DQ which receives the return of get_task_variable(), it returns the DQ with primary and dual parts. Shouldn't the call to get_task_variable() return a VectorXd with eight elements?
I tried some other joint configuration values for qm, but the same problem occurs.

[BUG] in DQ_SerialWholeBody::pose_jacobian method

Code of Conduct

By submitting this report you automatically agree that you've read and accepted the following conditions.

Support for DQ Robotics is given voluntarily and it's not the developers' role to educate and/or convince anyone of their vision.
Any possible response and its timeliness will be based on the relevance, accuracy, and politeness of a request and the following discussion.
If a DQ Robotics member replies, the user must let them know if their response solves their issue or not.
Any suggestion/advice/request made by anyone, as well intentioned as they might be, might not be incorporated into DQ Robotics.

Bug description

@dqrobotics/developers

Hi @mmmarinho, the method DQ_SerialWholeBody::pose_jacobian returns a result that is different from the Matlab implementation. I think this is related to the use of raw_methodssince the bug is present only when I set the reference frame to a value different from DQ(1).

Minimal Example

#include <iostream>
#include <dqrobotics/robot_modeling/DQ_SerialWholeBody.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
#include <memory>
#include <iomanip>

using namespace DQ_robotics;
using namespace Eigen;

int main()
{
    auto arm = std::make_shared<DQ_SerialManipulatorMDH>(FrankaEmikaPandaRobot::kinematics());
    DQ_SerialWholeBody robot = DQ_SerialWholeBody(arm);
    robot.set_reference_frame(1 + 0.5*E_*k_);
    VectorXd q = VectorXd::Zero(robot.get_dim_configuration_space());
    MatrixXd J = robot.pose_jacobian(q);
    std::cout<<std::fixed<<std::setprecision(4)<<J<<std::endl;
    return 0;
}

Output

 0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0000
 0.0000  0.0000  0.0000  0.0000  0.0000  0.0000  0.0000
 0.5000  0.0000  0.5000 -0.0000  0.5000 -0.0000 -0.5000
 0.0000 -0.5000  0.0000  0.5000 -0.0000  0.5000  0.0000
-0.0000 -0.0650 -0.0000 -0.0930 -0.0000 -0.2850 -0.0000
-0.2315  0.0000 -0.2315 -0.0000 -0.2315  0.0000  0.2315
 0.0000 -0.0117  0.0000 -0.0296  0.0000  0.0117 -0.0000
-0.0117 -0.0000 -0.0117  0.0000 -0.0117  0.0000 -0.0323

Expected behavior (Matlab)

clear all
close all
clc

robot = DQ_SerialWholeBody(FrankaEmikaPandaRobot.kinematics());
robot.set_reference_frame(1 + 0.5*DQ.E*DQ.k);
q = zeros(robot.get_dim_configuration_space());
J = robot.pose_jacobian(q)

Expected output

J =

         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
    0.5000    0.0000    0.5000   -0.0000    0.5000   -0.0000   -0.5000
    0.0000   -0.5000    0.0000    0.5000         0    0.5000         0
   -0.0000    0.1850   -0.0000   -0.3430   -0.0000   -0.5350    0.0000
   -0.4815   -0.0000   -0.4815    0.0000   -0.4815    0.0000    0.4815
   -0.0000   -0.0117    0.0000   -0.0296   -0.0000    0.0117    0.0000
   -0.0117    0.0000   -0.0117   -0.0000   -0.0117   -0.0000   -0.0323

Environment:

OS: [e.g. Ubuntu 22.04.3 LTS]
dqrobotics version (Matlab: e455c66, C++ 77acf9a)
MATLAB version [e.g., R2022b]

About the DQ_Geometry::point_to_line/plane_(squared)_distance() function

Professor Murilo, every time I use these two functions in conjunction with DQ_Kinematics::line/plane_to_point_(squared)_distance, I always have to remind myself to switch the positions of the point and the line/plane. During such moments, I can't help but think how convenient it would be if there were functions named line_to_point_squared_distance and plane_to_point_distance. I am wondering if you have time and are interested in adding these two delegation functions to improve coding efficiency and readability. :)

[BUG] Unable to access link index of the second chain in DQ_SerialWholeBody.fkm()

@dqrobotics/developers

Bug description

I am using the DQ_SerialWholeBody class to model a kinematic chain composed of two Kuka manipulators. However, I do not have access to the link index of the second chain when I use the fkm() method. I am able to compute the intermediary poses only for the first chain.

I think that this bug is responsible for the bug reported here.

Code

#include <iostream>
#include <dqrobotics/DQ.h>
#include <dqrobotics/robot_modeling/DQ_SerialWholeBody.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
#include <memory>
#include <Eigen/Dense>

using namespace DQ_robotics;

int main()
{
    //-----------Robot definition------------------------------------------------------
    auto arm1 = std::make_shared<DQ_SerialManipulatorDH>(KukaLw4Robot::kinematics());
    auto arm2 = std::make_shared<DQ_SerialManipulatorDH>(KukaLw4Robot::kinematics());
    DQ_SerialWholeBody super_arm(std::static_pointer_cast<DQ_Kinematics>(arm1));
    super_arm.add(std::static_pointer_cast<DQ_Kinematics>(arm2));
    //---------------------------------------------------------------------------------
    int n = arm1->get_dim_configuration_space()+arm2->get_dim_configuration_space();
    VectorXd q = VectorXd::Zero(n);

    for (int i=0; i<n; i++)
    {
            std::cout << "x"<<i<<": "<<super_arm.fkm(q,i)<<std::endl;
    }

    return 0;
}

CmakeLists.

cmake_minimum_required(VERSION 3.5)

project(serial_whole_body LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package (Threads REQUIRED)
FIND_PACKAGE(Eigen3 REQUIRED)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})

add_executable(serial_whole_body main.cpp)
TARGET_LINK_LIBRARIES(${PROJECT_NAME}
    pthread
    dqrobotics
)

Output

x0: 0.707107 + 0.707107i + E*(0.109602j + 0.109602k)
x1: 1 + E*(0.155k)
x2: 0.707107 - 0.707107i + E*( - 0.251023j + 0.251023k)
x3: 1 + E*(0.355k)
x4: 0.707107 + 0.707107i + E*(0.388909j + 0.388909k)
x5: 1 + E*(0.55k)
x6: 1 + E*(0.55k)
terminate called after throwing an instance of 'std::runtime_error'
  what():  Tried to access link index 7 which is unnavailable.

Expected behavior

The Matlab version of the code is working (master branch).

clear all
close all
clc

arm1 = KukaLwr4Robot.kinematics();
arm2 = KukaLwr4Robot.kinematics();
super_arm = DQ_SerialWholeBody(arm1);
super_arm.add(arm2);
n = arm1.get_dim_configuration_space() + arm2.get_dim_configuration_space();
q = zeros(n,1);
for i=1:n    
    super_arm.fkm(q, i)    
end

Expected output

ans = 
         (0.70711 + 0.70711i) + E*(0.1096j + 0.1096k)
ans = 
         (1) + E*(0.155k)
ans = 
         (0.70711 - 0.70711i) + E*( - 0.25102j + 0.25102k)
ans = 
         (1) + E*(0.355k)
ans = 
         (0.70711 + 0.70711i) + E*(0.38891j + 0.38891k)
ans = 
         (1) + E*(0.55k)
ans = 
         (1) + E*(0.55k)
ans = 
         (0.70711 + 0.70711i) + E*(0.49851j + 0.49851k)
ans = 
         (1) + E*(0.705k)
ans = 
         (0.70711 - 0.70711i) + E*( - 0.63993j + 0.63993k)
ans = 
         (1) + E*(0.905k)
ans = 
         (0.70711 + 0.70711i) + E*(0.77782j + 0.77782k)
ans = 
         (1) + E*(1.1k)
ans = 
         (1) + E*(1.1k)

Environment:

OS: Ubuntu 20.04
dqrobotics version
C++ (development branch) 20.04.0-0-202206300608-ubuntu20.04.1.

[QUESTION] Change repo name to something more specific?

Acceptance of the Code of Conduct

Leave these in your question, read, and check them.

I understand that support for DQRobotics is given voluntarily.
In understand that any possible response and its timeliness will be based on the relevance, accuracy, and politeness of my request and the following discussion.
If a DQRobotics member replies, I will let them know, politely, if their response solves my issue or not.

Question

Would it make sense to change the repo name to something like dqrobotics-cpp? cpp just sounds too generic.

New option for the Pseudoinverse method

Hi Murilo!

The pseudoinverse method (MatrixXd pseudoInverse( const MatrixXd& matrix)) is implemented in the current version. It is a very useful function. But there is no option to compute the pseudoinverse with damped factor. It is possible to add it?

Thanks!

Juancho.

[LANGUAGE INCOMPATIBILITY] The matrices Q4 and Q8 are not implemented

Acceptance of the Code of Conduct

Leave these in your bug report, read, and check them.

I understand that support for DQRobotics is given voluntarily.
In understand that any possible response and its timeliness will be based on the relevance, accuracy, and politeness of my request and the following discussion.
If a DQRobotics member replies, I will let them know, politely, if their response solves my issue or not.

Hello guys,

Describe the missing/unexpected functionality
The matrices Q4 and Q8 are not implemented in the DQ class as they are implemented in the MATLAB version of the DQ class for Matlab.

Would it be possible for you to implement them? Or should I implement them and do a pull request?

Environment:

OS: Ubuntu 22.04
dqrobotics version 20.04.0-0~~202005090156~~ubuntu22.04.1

Kind regards,
Marcos Pereira

[BUG] Gcc throws an "undefined reference to" some of DQ_robotics methods and types since the last dev-PPA update?

Question description

This error appeared after the last update to the development PPA.
Why does gcc throws an "undefined reference to" some of DQ_robotics methods and types?

To Reproduce

The code used was the code proposed on issue #22.

Code

mkdir issue22
cd issue22
cat <<EOT >> issue22.cpp
#include<iostream>
#include<dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include<dqrobotics/robots/KukaLw4Robot.h>

using namespace DQ_robotics;

int main(void)
{

DQ_SerialManipulator robot;
DQ_SerialManipulator kuka = KukaLw4Robot::kinematics();
std::cout << "Number of links = " << kuka.get_dim_configuration_space() << std::endl;

return 0;
}
EOT
g++ -o issue22 issue22.cpp -ldqrobotics
./issue22

Output

/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/libdqrobotics.so: undefined reference to `DQ_robotics::DQ_KinematicController::compute_tracking_control_signal(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)'
/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/libdqrobotics.so: undefined reference to `DQ_robotics::DQ_TaskspaceQuadraticProgrammingController::compute_tracking_control_signal(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)'
/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/libdqrobotics.so: undefined reference to `typeinfo for DQ_robotics::DQ_KinematicController'
/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/libdqrobotics.so: undefined reference to `vtable for DQ_robotics::DQ_KinematicController'
/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/libdqrobotics.so: undefined reference to `DQ_robotics::DQ_KinematicController::compute_setpoint_control_signal(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)'
collect2: error: ld returned 1 exit status

Expected behavior

The code should compile without errors.

Expected output

Number of links = 7

Environment:

OS: Ubuntu 18.04
dqrobotics version: 0.1-0 201908010354 ubuntu18.04.1

[QUESTION] Would it be possible to add the method line_to_line_angle_jacobian to the DQ in C++?

Hello @mmmarinho,

Would it be possible for you to add the line_to_line_angle_jacobian() method to the DQ Robotics C++ library? The method is not available at the DQ_Kinematics.cpp.

It is already available at the DQ_Kinematics.m.

I am looking forward to use it in C++.

Thanks

Implement the sharp conjugation operation

Murilo, I've just implemented the sharp conjugation operation in Matlab (matlab/@DQ/transpose.m). Could you please add it to the C++ code?

See Eq. (2.5.3) from Adorno, B. V. (2017). Robot Kinematic Modeling and Control Based on Dual Quaternion Algebra -- Part I: Fundamentals, which can be found here

Improvements on DQ_Geometry

Hi, @mmmarinho

I suggest some changes in the file DQ_Geometry.

Following the same reasoning that we used when designing DQ_Kinematics, I think it's better to make DQ_Geometry a class with static methods instead of just a file with functions inside the namespace DQ_Robotics. The reasons for doing so are threefold: first, Matlab does not have the same namespace mechanism as C++; second, we maintain consistency (in terms of our adopted development conventions) inside the overall library; third, we know exactly where those functions are coming from, which helps in the maintainability.
For all squared distances, instead of doing something like pow((vec4(cross(point,l)-m).norm()),2);, it is more efficient writing something like

new_point = cross(point,l)-m;
vec4(new_point)'*vec4(new_point);

[BUG] lack of pure virtual method overload in DQ_SerialManipulatorDense

Acceptance of the Code of Conduct

Leave these in your bug report, read, and check them.

I understand that support for DQRobotics is given voluntarily.
In understand that any possible response and its timeliness will be based on the relevance, accuracy, and politeness of my request and the following discussion.
If a DQRobotics member replies, I will let them know, politely, if their response solves my issue or not.

Bug description

lack of overload of virtual Eigen::MatrixXd DQ_robotics::DQ_SerialManipulator::raw_pose_jacobian_derivative(const VectorXd&, const VectorXd&, const int&) const in DQ_SerialManipulatorDenso

To Reproduce

Please add the code you ran and the output you had, including error messages.
In case of C++, add the #include part of your code as well
In case of Python, add the import part of your code as well
Format it using code tags:

Code

code here

Output

output here

Expected behavior

A clear and concise description of what you expected to happen.

Expected output

expected output here

Environment:

OS: [e.g. Ubuntu 16.04, Ubuntu 18.04]
dqrobotics version
Python version [e.g. 3.6, 3.7 (If applicable)]
MATLAB version [e.g., 2018b, 2019a, (if applicable)]

Additional context

Add any other context about the problem here.

[QUESTION] Compilation issues

Hi, Dear Dev!

When I did

git clone https://github.com/dqrobotics/cpp.git dqrobotics
cd dqrobotics
mkdir build
cd build
cmake ..
make

CMake does not pick up the include path of my Eigen3. I have to modify

build/CMakeFiles/dqrobotics.dir/flags.make

to make it work, replacing

dqrobotics/EIGEN3_INCLUDE_DIR

with my Eigen3 path. Here's what CMake gave me

-- The C compiler identification is GNU 6.3.0
-- The CXX compiler identification is GNU 6.3.0
-- Check for working C compiler: /usr/lib/ccache/cc
-- Check for working C compiler: /usr/lib/ccache/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/lib/ccache/c++
-- Check for working CXX compiler: /usr/lib/ccache/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Configuring done
-- Generating done
-- Build files have been written to: /home/mujin/Repos/dqrobotics/build

so it seems to me your CMakeLists.txt does not pick up the Eigen3's directory.

Also it appears that the unit test is no longer working, as you seem to have removed/moved some "legacy" controller files. I would like to get the unit test working again, so that I could learn the usage of your double quaternion functions. Thank you!

Improve documentation and performance of dqrobotics-cpp

Hi, @mmmarinho,

I've edited this message in order to reflect #13 (comment).

General suggestion: for all Jacobians, put the associated bibliographic reference.

List of things to review:

line_jacobian
plane_jacobian

`line_jacobian`

The comments of DQ_Kinematics::line_jacobian() are outdated as they do not reflect the signature established in dqrobotics/matlab#12 (comment).

Furthermore, I suggest changing
haminus4(line_direction*conj(xr))*Jr + hamiplus4(xr*line_direction)*C4()*Jr
to
(haminus4(line_direction*conj(xr)) + hamiplus4(xr*line_direction)*C4())*Jr.

The former has three matrix multiplications and 1 matrix addition; the latter has 1 matrix addition and only two matrix multiplications. 🤓

`plane_jacobian`

The comments of DQ_Kinematics::plane_jacobian() are outdated as they do not reflect the signature established in dqrobotics/matlab#12 (comment).

Analogously to the line_jacobian, I also suggest changing
Jnz = haminus4(plane_normal*conj(xr))*Jr + hamiplus4(xr*plane_normal)*C4()*Jr
to
Jnz = (haminus4(plane_normal*conj(xr)) + hamiplus4(xr*plane_normal)*C4())*Jr;

Cheers,
Bruno

[BUG] The constructor of DQ does not work for pure dual quaternions

Bug description

It should be possible to create a DQ object from an Eigen vector with the pure dual quaternion coefficients, that is, a vector of size 6.
In fact, the library does not complain when creating a DQ from a vector of size 6. However, instead of creating a pure dual quaternion, it creates a DQ object with the first 6 elements equal to the elements of the vector, that is
q(0) + q(1)i + q(2)j + q(3)k + E*(q(4) + q(5)i + 0j + 0k)
instead of
0 + q(0)i + q(1)j + q(2)k + E*(0 + q(3)i + q(4)j + q(5)k).

To Reproduce

Code

#include <iostream>
#include <Eigen/Dense>
#include <dqrobotics/DQ.h>
#include <iostream>

using namespace DQ_robotics;

int main()
{
    Eigen::VectorXd force(6);
    force << 1, 2, 3, 4, 5, 6;
    std::cout << DQ(force) << std::endl;
   return 0;
}

Output

1 2i 3j 4k +E( 5 6i 0j 0k )

Expected behavior

Expected output

0 1i 2j 3k +E( 0 4i 5j 6k )

Environment:

OS: Ubuntu16.04
dqrobotics version: 0.1.0

Additional context

It works fine in MATLAB version.

C++ Doxygen documentation

Murilo,

Could you please generate the Doxygen documentation files and make them available as an online HTML page? The idea is to automatically update this documentation HTML page at each push. Please see an example here.

dqrobotics / cpp Goto Github PK

cpp's People

Contributors

Stargazers

Watchers

Forkers

cpp's Issues

Bug description

Hotfix:

Environment:

Code of Conduct

Acceptance of the Code of Conduct

Question

Acceptance of the Code of Conduct

Acceptance of the Code of Conduct

line_jacobian

plane_jacobian

Recommend Projects

Recommend Topics

Recommend Org

`line_jacobian`

`plane_jacobian`