From de7e1b46189f34a810532323d1a4a19c21d3d43d Mon Sep 17 00:00:00 2001 From: Frederico Afonso Date: Fri, 11 Oct 2024 14:26:52 +0100 Subject: [PATCH] [DQRoboticsApiCommandServer.lua] Added support for arbitrary reference frames for 'get_center_of_mass()' and 'get_inertia()'. --- .../DQRoboticsApiCommandServer.lua | 195 ++++++++++++------ 1 file changed, 130 insertions(+), 65 deletions(-) diff --git a/cmake/vrep_interface_tests/DQRoboticsApiCommandServer.lua b/cmake/vrep_interface_tests/DQRoboticsApiCommandServer.lua index f7d5593..5e05e49 100644 --- a/cmake/vrep_interface_tests/DQRoboticsApiCommandServer.lua +++ b/cmake/vrep_interface_tests/DQRoboticsApiCommandServer.lua @@ -1,21 +1,32 @@ ---(C) Copyright 2022 DQ Robotics Developers ---This file is part of DQ Robotics. +-- (C) Copyright 2011-2024 DQ Robotics Developers +-- +-- This file is part of DQ Robotics. +-- +-- DQ Robotics is free software: you can redistribute it and/or modify +-- it under the terms of the GNU Lesser General Public License as published by +-- the Free Software Foundation, either version 3 of the License, or +-- (at your option) any later version. +-- +-- DQ Robotics is distributed in the hope that it will be useful, +-- but WITHOUT ANY WARRANTY; without even the implied warranty of +-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +-- GNU Lesser General Public License for more details. +-- +-- You should have received a copy of the GNU Lesser General Public License +-- along with DQ Robotics. If not, see . -- --- DQ Robotics is free software: you can redistribute it and/or modify --- it under the terms of the GNU Lesser General Public License as published by --- the Free Software Foundation, either version 3 of the License, or --- (at your option) any later version. --- --- DQ Robotics is distributed in the hope that it will be useful, --- but WITHOUT ANY WARRANTY; without even the implied warranty of --- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the --- GNU Lesser General Public License for more details. --- --- You should have received a copy of the GNU Lesser General Public License --- along with DQ Robotics. If not, see . +-- DQ Robotics website: dqrobotics.github.io --Contributors: -- Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp +-- - Responsible for the original implementation. +-- +-- Frederico Fernandes Afonso Silva - frederico.silva@ieee.org +-- - Generalized functions get_center_of_mass() and get_inertia() to allow arbitrary +-- reference frames. +-- - Removed unnused inputs (e.g., inFloats, inStrings, and inBuffer) from functions +-- get_mass(), get_center_of_mass(), and get_inertia(). Also renamed some of their +-- variables and added comments to make them clearer. @@ -45,18 +56,14 @@ end ---------------------------------------------------------------------------------- --/** --- * @brief Returns the mass a handle. --- * @param inInts. The integer value of the handle. Example: {1} --- * @param inFloats. The float input value. Example: {} --- * @param inStrings. The string input value. Example: {"absolute_frame"} or {} --- * @param inBuffer. The buffer input value. Example: {} --- * @returns the mass -function get_mass(inInts,inFloats,inStrings,inBuffer) - local mass = {} - if #inInts>=1 then - mass =sim.getShapeMass(inInts[1]) - --print(mass) - return {},{mass},{},'' +-- * @brief Returns the mass of an object. +-- * @param inInts. The integer value of the object's handle. Example: [1] +-- * @returns the object's mass +function get_mass(inInts) + if #inInts>=1 then + mass = sim.getShapeMass(inInts[1]) + + return {}, {mass}, {}, '' end end ---------------------------------------------------------------------------------- @@ -64,36 +71,76 @@ end ---------------------------------------------------------------------------------- --/** --- * @brief Returns the inertia of a handle. --- * @param inInts. The integer value of the handle. Example: {1} +-- * @brief Returns the inertia tensor of an object. +-- * @param inInts. The integer value of the object's handle. Optionally, the +-- handle of the desired reference frame 'df' (the detault is the shape's +-- reference frame 'sf'). Example: [1] or [1, 2] -- * @param inFloats. The float input value. Example: {} --- * @param inStrings. The string input value. Example: {"absolute_frame"} or {} --- * @param inBuffer. The buffer input value. Example: {} --- * @returns the inertia -function get_inertia(inInts,inFloats,inStrings,inBuffer) - - if #inInts>=1 then - IM, matrixSFCOM=sim.getShapeInertia(inInts[1]) - +-- * @param inStrings. The name of the desired reference frame. Example: {"absolute_frame"} or {} +-- * @returns the inertia tensor of the object +function get_inertia(inInts, inFloats, inStrings) + if #inInts==1 then + -- Retrieves the inertia tensor in 'sf' + I_sf, H_com_sf = sim.getShapeInertia(inInts[1]) + if inStrings[1] == 'absolute_frame' then - matrix0_SF=sim.getObjectMatrix(inInts[1],-1) - M = sim.multiplyMatrices(matrix0_SF,matrixSFCOM) - I= {{IM[1],IM[2],IM[3]}, - {IM[4],IM[5],IM[6]}, - {IM[7],IM[8],IM[9]}} - R_0_COM = {{M[1],M[2],M[3]}, - {M[5],M[6],M[7]}, - {M[9],M[10],M[11]}} - R_0_COM_T = transpose(R_0_COM) - RIR_T = mat_mult(mat_mult(R_0_COM,I), R_0_COM_T) - RIR_T_v = {RIR_T[1][1], RIR_T[1][2], RIR_T[1][3], - RIR_T[2][1], RIR_T[2][2], RIR_T[2][3], - RIR_T[3][1], RIR_T[3][2], RIR_T[3][3]} - resultInertiaMatrix=RIR_T_v + -- Retrieves the homogeneous transformation matrix between 'sf' and 'absolute_frame' + H_sf_0 = sim.getObjectMatrix(inInts[1], -1) + + -- Expresses the center of mass in 'absolute_frame' + H_com_0 = sim.multiplyMatrices(H_sf_0, H_com_sf) + + -- Formats I_sf for LUA matrix multiplication + I_lua = {{I_sf[1],I_sf[2],I_sf[3]}, + {I_sf[4],I_sf[5],I_sf[6]}, + {I_sf[7],I_sf[8],I_sf[9]}} + + -- Retrieves the rotation matrix from H_com_0 + R_com_0 = {{H_com_0[1],H_com_0[2], H_com_0[3]}, + {H_com_0[5],H_com_0[6], H_com_0[7]}, + {H_com_0[9],H_com_0[10],H_com_0[11]}} + + -- Expresses the inertia tensor in 'absolute_frame' (R*I*R^T) + I_0_lua = mat_mult(mat_mult(R_com_0, I_lua), transpose(R_com_0)) + + -- Formats I_0_lua for proper return (as a 1x9 vector) + I_0 = {I_0_lua[1][1], I_0_lua[1][2], I_0_lua[1][3], + I_0_lua[2][1], I_0_lua[2][2], I_0_lua[2][3], + I_0_lua[3][1], I_0_lua[3][2], I_0_lua[3][3]} + + return {}, I_0, {}, '' else - resultInertiaMatrix=IM - end - return {},resultInertiaMatrix,{},'' + return {}, I_sf, {}, '' + end + elseif #inInts==2 then + -- Retrieves the inertia tensor and the center of mass in 'sf' + I_sf, H_com_sf = sim.getShapeInertia(inInts[1]) + + -- Retrieves the homogeneous transformation matrix between 'sf' and 'df' + H_sf_df = sim.getObjectMatrix(inInts[1], inInts[2]) + + -- Expresses the center of mass in 'df' + H_com_df = sim.multiplyMatrices(H_sf_df, H_com_sf) + + -- Formats I_sf for LUA matrix multiplication + I_lua = {{I_sf[1],I_sf[2],I_sf[3]}, + {I_sf[4],I_sf[5],I_sf[6]}, + {I_sf[7],I_sf[8],I_sf[9]}} + + -- Retrieves the rotation matrix from H_com_df + R_com_df = {{H_com_df[1],H_com_df[2], H_com_df[3]}, + {H_com_df[5],H_com_df[6], H_com_df[7]}, + {H_com_df[9],H_com_df[10],H_com_df[11]}} + + -- Expresses the inertia tensor in 'df' (R*I*R^T) + I_df_lua = mat_mult(mat_mult(R_com_df, I_lua), transpose(R_com_df)) + + -- Formats I_df_lua for proper return (as a 1x9 vector) + I_df = {I_df_lua[1][1], I_df_lua[1][2], I_df_lua[1][3], + I_df_lua[2][1], I_df_lua[2][2], I_df_lua[2][3], + I_df_lua[3][1], I_df_lua[3][2], I_df_lua[3][3]} + + return {}, I_df, {}, '' end end ---------------------------------------------------------------------------------- @@ -101,22 +148,40 @@ end ---------------------------------------------------------------------------------- --/** --- * @brief Returns the center of mass of a handle. --- * @param inInts. The integer value of the handle. Example: {1} +-- * @brief Returns the center of mass of an object. +-- * @param inInts. The integer value of the object's handle. Optionally, the +-- handle of the desired reference frame 'df' (the detault is the shape's +-- reference frame 'sf'). Example: [1] or [1, 2] -- * @param inFloats. The float input value. Example: {} --- * @param inStrings. The string input value. Example: {"absolute_frame"} or {} --- * @param inBuffer. The buffer input value. Example: {} --- * @returns the center of mass -function get_center_of_mass(inInts,inFloats,inStrings,inBuffer) - if #inInts>=1 then - IM,matrix_SF_COM=sim.getShapeInertia(inInts[1]) - matrix0_SF=sim.getObjectMatrix(inInts[1],-1) +-- * @param inStrings. The name of the desired reference frame. Example: {"absolute_frame"} or {} +-- * @returns the vector of the object's center of mass +function get_center_of_mass(inInts, inFloats, inStrings) + if #inInts==1 then + -- Retrieves the center of mass in 'sf' + _, H_com_sf = sim.getShapeInertia(inInts[1]) + if inStrings[1] == 'absolute_frame' then - resultMatrix = sim.multiplyMatrices(matrix0_SF,matrix_SF_COM) + -- Retrieves the homogeneous transformation matrix between 'sf' and 'absolute_frame' + H_sf_0 = sim.getObjectMatrix(inInts[1], -1) + + -- Expresses the center of mass in 'absolute_frame' + H_com_0 = sim.multiplyMatrices(H_sf_0, H_com_sf) + + return {}, {H_com_0[4], H_com_0[8],H_com_0[12]}, {}, '' else - resultMatrix = matrix_SF_COM + return {}, {H_com_sf[4], H_com_sf[8],H_com_sf[12]}, {}, '' end - return {},{resultMatrix[4], resultMatrix[8],resultMatrix[12]},{},'' + elseif #inInts==2 then + -- Retrieves the center of mass in 'sf' + _, H_com_sf = sim.getShapeInertia(inInts[1]) + + -- Retrieves the homogeneous transformation matrix between 'sf' and 'df' + H_sf_df = sim.getObjectMatrix(inInts[1], inInts[2]) + + -- Expresses the center of mass in 'df' + H_com_df = sim.multiplyMatrices(H_sf_df, H_com_sf) + + return {}, {H_com_df[4], H_com_df[8], H_com_df[12]}, {}, '' end end ----------------------------------------------------------------------------------