[LANGUAGE INCOMPATIBILITY] raw_fkm method unavailable for DQ_SerialWholeBody class

ChrisTYuen commented 7 months ago

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.

Describe the missing/unexpected functionality When using the DQ_SerialWholeBody robot modeling class, the raw_fkm() method is not supported via python version, but is supported via C++ and Matlab versions.

JSON FILE denso_vs050.json

{
  "robot_type":"DQ_SerialManipulatorDenso",
  "effector":[1.0],
  "reference_frame":[1.0],
  "angle_mode":"degree",
  "a":    [0,     0,      -0.009968, 0.000174,  -0.00002,  0],
  "b":    [0,     -0.250,   0,      0,      0,      0],
  "d":    [0.345,   0,      0,      0.25501, -0.000223, 0.069963],
  "alpha":[-89.94,0.038,  90.024, -89.9,  89.988, 0],
  "beta": [0,     0.007,  0,      0,      0,      0],
  "gamma":[0,     0.012,  -0.009, -0.083, -0.018, 0],
  "lower_q_limit":[-170, -70, 10, -270, -90, -360],
  "upper_q_limit":[170, 120, 151, 270, 120, 360],
  "lower_q_dot_limit":[-33, -32.1, -59.40, -60.50, -60.50, -90.80],
  "upper_q_dot_limit":[33, 32.1, 59.40, 60.50, 60.50, 90.80]
}

Python behavior (if applicable)

VS050Robot Package

import numpy as np
import math
import os
import logging
# logger init
logging.basicConfig()
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO)

import dqrobotics as dql
from dqrobotics.interfaces.json11 import DQ_JsonReader

import json
# Class definition of VS050 DENSO robot for pathology

class VS050Robot:
    def __init__(self, json_path=None):
        # Standard of VS050

        if json_path is None or not os.path.isfile(json_path):
            raise ValueError("robot.json not specified")

        try:
            with open(json_path) as j_f:
                jdata = json.load(j_f)

        except Exception as e:
            raise ValueError("DH loading file read error")

        reader = DQ_JsonReader()

        if jdata['robot_type']=="DQ_SerialManipulatorDH":
            self.robot = reader.get_serial_manipulator_dh_from_json(json_path)
        elif jdata['robot_type']=="DQ_SerialManipulatorDenso":
            self.robot = reader.get_serial_manipulator_denso_from_json(json_path)
        else:
            raise ValueError("json parameter type definition error: "+str(type))

    # @staticmethod
    def kinematics(self):
        return self.robot

#############################for testing only################################
def main():
    jpath="./denso_vs050_DH_test.json"
    # jpath="./denso_vs050_denso_11U483.json"
    robot=VS050Robot(jpath).kinematics()
    logger.info(str(robot))
    logger.info(str(robot.fkm([0,0,0,0,0,0])))

if __name__=='__main__':
    main()
#############################for testing only################################

MAIN PYTHON SCRIPT

from dqrobotics import *
from dqrobotics.robot_modeling import DQ_SerialWholeBody
from vs_robot import VS050Robot

robot_json = "./denso_vs050.json"
robot_base = VS050Robot(robot_json).kinematics()
robot_si = DQ_SerialWholeBody(robot_base)
effector_t_si = DQ([0, 0, 0, 0])
effector_r_si = DQ([1, 0, 0, 0])
theta = effector_r_si + 0.5 * E_ * effector_t_si * effector_r_si
robot_si.raw_fkm(theta, 5)

PYTHON OUTPUT

'dqrobotics._dqrobotics._robot_modeling.DQ_SerialWh' object has no attribute 'raw_fkm'.

Environment:

OS: Ubuntu 20.04
dqrobotics version 23.4.0a16
Python version 3.8.10

Additional context Add any other context about the problem here.

mmmarinho commented 7 months ago

@ChrisTYuen Thanks!

I understand what you are saying, but for the sake of completeness, could you add a complete minimal working example?

ChrisTYuen commented 7 months ago

@mmmarinho Of course!

I have updated the comment as requested. Let me know if there is anything else.

mmmarinho commented 7 months ago

@ChrisTYuen Thanks!

@juanjqo The theta does not seem correct in the minimal example. Does it also not work with the correct theta?

juanjqo commented 7 months ago

@mmmarinho No, it doesn't. Even using a correct theta, the problem persists, since the method raw_fkm() is not available in the Python version.

I wasn't able to run the minimal example since the module vs_robot was not included. However, I did a minimal example to illustrate the problem reported by @ChrisTYuen.

Minimal Python example:

#!/bin/python3
from dqrobotics.robot_modeling import DQ_SerialWholeBody
from dqrobotics.robots import FrankaEmikaPandaRobot
from dqrobotics.robots import KukaLw4Robot
import numpy as np

def main() -> None:
    try:
        robot = DQ_SerialWholeBody(FrankaEmikaPandaRobot.kinematics())
        robot.add(KukaLw4Robot.kinematics())
        q = np.zeros(robot.get_dim_configuration_space())
        x_raw = robot.raw_fkm(q)
    except (Exception, KeyboardInterrupt) as e:
        print(e)
        pass

if __name__ == "__main__":
    main()

Output:


'dqrobotics._dqrobotics._robot_modeling.DQ_SerialWh' object has no attribute 'raw_fkm'

Process finished with exit code 0

Expected behavior (using #55):

1i + E*( - 0.06465 - 0.087j)

Process finished with exit code 0

Minimal example (Matlab):

clear all
close all
clc

robot = DQ_SerialWholeBody(FrankaEmikaPandaRobot.kinematics());
robot.add(KukaLwr4Robot.kinematics());
q = zeros(robot.get_dim_configuration_space());
x_raw = robot.raw_fkm(q)

Output (Matlab):

x_raw = 

         (1i) + E*( - 0.06465 - 0.087j)

Best regards,

Juancho

ChrisTYuen commented 7 months ago

Sorry! I have included the package if it is still required.

dqrobotics / python