diff --git a/robot_modeling/DQ_DifferentialDriveRobot.m b/robot_modeling/DQ_DifferentialDriveRobot.m index 39f59bfb..c89e66d7 100644 --- a/robot_modeling/DQ_DifferentialDriveRobot.m +++ b/robot_modeling/DQ_DifferentialDriveRobot.m @@ -14,7 +14,7 @@ % See also DQ_HolonomicBase. -% (C) Copyright 2011-2019 DQ Robotics Developers +% (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -34,7 +34,11 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ufmg.br +% 1. Bruno Vihena Adorno - adorno@ieee.org +% +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Added the method pose_jacobian_derivative(). +% However, the method is not implemented yet. classdef DQ_DifferentialDriveRobot < DQ_HolonomicBase @@ -78,5 +82,17 @@ J_holonomic = pose_jacobian@DQ_HolonomicBase(obj,q); J = J_holonomic*obj.constraint_jacobian(q(3)); end + + function J_dot = pose_jacobian_derivative(obj, q, q_dot) + % J_dot = pose_jacobian_derivative(q, q_dot) returns the time + % derivative of the Jacobian matrix that satisfies + % vec8(h_dot_dot) = J_dot*[wr,wl]' + J*[wr_dot,wl_dot]', where h_dot is the time + % derivative of the unit dual quaternion that represent the + % mobile base pose and wr and wl are the angular velocities of the + % right and left wheels, respectively. + % q = [x,y,phi] is the robot configuration + + error('pose_jacobian_derivative is not implemented yet'); + end end end \ No newline at end of file diff --git a/robot_modeling/DQ_HolonomicBase.m b/robot_modeling/DQ_HolonomicBase.m index 19fad259..b48d29f0 100644 --- a/robot_modeling/DQ_HolonomicBase.m +++ b/robot_modeling/DQ_HolonomicBase.m @@ -16,7 +16,7 @@ % % See also DQ_MobileBase, DQ_DifferentialDriveRobot -% (C) Copyright 2011-2019 DQ Robotics Developers +% (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -36,7 +36,11 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ufmg.br +% 1. Bruno Vihena Adorno - adorno@ieee.org +% +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Added the method pose_jacobian_derivative(). +% However, the method is not implemented yet. classdef DQ_HolonomicBase < DQ_MobileBase @@ -137,6 +141,16 @@ J = haminus8(obj.frame_displacement)*obj.raw_pose_jacobian(q); end end + + function J_dot = pose_jacobian_derivative(obj, q, q_dot, ~) + % POSE_JACOBIAN_DERIVATIVE(q, q_dot) returns, given the configuration + % q = [x,y,phi]', the mobile-base pose Jacobian derivative J_dot that satisfies + % x_dot_dot = J_dot*q_dot + J*q_dot_dot, where x_dot is the time derivative + % of the unit dual quaternion that represents the mobile-base pose. It takes into + % consideration the base frame displacement (e.g., base height). + + error('pose_jacobian_derivative is not implemented yet'); + end function set_base_diameter(obj,diameter) % Set the base diameter, in meters. This function must be diff --git a/robot_modeling/DQ_Kinematics.m b/robot_modeling/DQ_Kinematics.m index 58699549..2bc0635f 100644 --- a/robot_modeling/DQ_Kinematics.m +++ b/robot_modeling/DQ_Kinematics.m @@ -37,7 +37,7 @@ % translation_jacobian - Compute the translation Jacobian. % See also DQ_SerialManipulator, DQ_MobileBase, DQ_CooperativeDualTaskSpace. -% (C) Copyright 2011-2023 DQ Robotics Developers +% (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -60,10 +60,11 @@ % 1. Bruno Vihena Adorno (adorno@ieee.org) % Responsible for the original implementation. % -% 2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) % - Added the property dim_configuration_space. % - Added the method line_to_line_angle_residual(). % - Added the methods check_ith_link() and check_q_vec(). +% - Added the method pose_jacobian_derivative(). classdef DQ_Kinematics < handle % DQ_Kinematics inherits the HANDLE superclass to avoid unnecessary copies @@ -172,6 +173,13 @@ function check_q_vec(obj, q_vec) % POSE_JACOBIAN(q, to_ith_link) calculates the forward kinematic model up to % the ith element in the chain. J = pose_jacobian(obj, q, to_ith_link); + + % POSE_JACOBIAN_DERIVATIVE(q, q_dot) returns the Jacobian derivative 'J_dot' that satisfies + % vec8(x_pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where x_pose = fkm(q), 'x_pose_dot_dot' is + % the second time derivative of the 'x_pose' and 'q_dot' represents the robot configuration velocities. + % J_dot = pose_jacobian_derivative(obj, q, q_dot, to_ith_link) calculates the jacobian derivative up + % to the ith element in the chain. + J_dot = pose_jacobian_derivative(obj, q, q_dot, to_ith_link); end methods(Static) diff --git a/robot_modeling/DQ_SerialWholeBody.m b/robot_modeling/DQ_SerialWholeBody.m index 4ed1aa28..99c40120 100644 --- a/robot_modeling/DQ_SerialWholeBody.m +++ b/robot_modeling/DQ_SerialWholeBody.m @@ -51,7 +51,7 @@ % % See also DQ_kinematics, DQ_MobileBase -% (C) Copyright 2011-2023 DQ Robotics Developers +% (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -71,7 +71,10 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ieee.org +% 1. Bruno Vihena Adorno - adorno@ieee.org +% +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Added the method pose_jacobian_derivative(). However, the method is not implemented yet. classdef DQ_SerialWholeBody < DQ_Kinematics properties (Access = protected) @@ -591,5 +594,17 @@ function set_effector(obj,effector) ' in the kinematic chain is a DQ_SerialManipulator object']); end end + + function J_dot = pose_jacobian_derivative(obj, q, q_dot, ith) + % Returns the whole-body pose Jacobian. + % J_dot = POSE_JACOBIAN_DERIVATIVE(q, q_dot) receives the configuration vector q and configuration + % velocity vector q_dot of the whole kinematic chain and returns the jacobian derivative J_dot that satisfies + % vec8(xdot_dot) = J_dot*q_dot + J*q_dot_dot, where q_dot is the configuration velocity + % and xdot is the time derivative of the unit dual quaternion that represents the end-effector pose. + % J = POSE_JACOBIAN_DERIVATIVE(q, q_dot, ith) calculates the Jacobian derivative up to the ith + % link of the combined kinematic chain. + + error('pose_jacobian_derivative is not implemented yet'); + end end end \ No newline at end of file diff --git a/robot_modeling/DQ_WholeBody.m b/robot_modeling/DQ_WholeBody.m index a9380296..492ad02e 100644 --- a/robot_modeling/DQ_WholeBody.m +++ b/robot_modeling/DQ_WholeBody.m @@ -42,7 +42,7 @@ % % See also DQ_kinematics, DQ_MobileBase -% (C) Copyright 2011-2023 DQ Robotics Developers +% (C) Copyright 2011-2025 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -62,7 +62,11 @@ % DQ Robotics website: dqrobotics.github.io % % Contributors to this file: -% Bruno Vihena Adorno - adorno@ieee.org +% 1. Bruno Vihena Adorno - adorno@ieee.org +% +% 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) +% - Added the method pose_jacobian_derivative(). +% However, the method is not implemented yet. classdef DQ_WholeBody < DQ_Kinematics properties (Access = protected) @@ -478,5 +482,18 @@ function set_effector(obj,effector) ' in the kinematic chain is a DQ_SerialManipulator object']); end end + + + function J_dot = pose_jacobian_derivative(obj, q, q_dot, ith) + % Returns the whole-body pose Jacobian. + % J_dot = POSE_JACOBIAN_DERIVATIVE(q, q_dot) receives the configuration vector q and configuration + % velocity vector q_dot of the whole kinematic chain and returns the jacobian derivative J_dot that satisfies + % vec8(xdot_dot) = J_dot*q_dot + J*q_dot_dot, where q_dot is the configuration velocity + % and xdot is the time derivative of the unit dual quaternion that represents the end-effector pose. + % J = POSE_JACOBIAN_DERIVATIVE(q, q_dot, ith) calculates the Jacobian derivative up to the ith + % link of the combined kinematic chain. + + error('pose_jacobian_derivative is not implemented yet'); + end end end \ No newline at end of file