diff --git a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h index 23beabe..33d4988 100644 --- a/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h +++ b/include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h @@ -1,5 +1,5 @@ /** -(C) Copyright 2019 DQ Robotics Developers +(C) Copyright 2019-2022 DQ Robotics Developers This file is part of DQ Robotics. @@ -38,9 +38,14 @@ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase DQ_DifferentialDriveRobot(const double& wheel_radius, const double& distance_between_wheels); MatrixXd constraint_jacobian(const double& phi) const; - + MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const; MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override; MatrixXd pose_jacobian(const VectorXd &q) const override; + MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot, + const int& to_link) const override; + MatrixXd pose_jacobian_derivative (const VectorXd& q, + const VectorXd& q_dot) const override; }; } diff --git a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h index 78b805e..b3304f0 100644 --- a/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h +++ b/include/dqrobotics/robot_modeling/DQ_HolonomicBase.h @@ -40,9 +40,15 @@ class DQ_HolonomicBase: public DQ_MobileBase virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override; virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override; virtual MatrixXd pose_jacobian(const VectorXd& q) const override; + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot, const int& to_link) const override; + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot) const override; DQ raw_fkm(const VectorXd& q) const; MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const; + MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot, const int& to_link = 2) const; }; } diff --git a/include/dqrobotics/robot_modeling/DQ_Kinematics.h b/include/dqrobotics/robot_modeling/DQ_Kinematics.h index 6ca4912..d85b883 100644 --- a/include/dqrobotics/robot_modeling/DQ_Kinematics.h +++ b/include/dqrobotics/robot_modeling/DQ_Kinematics.h @@ -64,8 +64,13 @@ class DQ_Kinematics virtual DQ fkm (const VectorXd& joint_configurations) const = 0; virtual DQ fkm (const VectorXd& joint_configurations, const int& to_ith_link) const = 0; virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0; + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot, + const int& to_ith_link) const = 0; //Virtual methods virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const; + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot) const; virtual int get_dim_configuration_space() const; //Static methods diff --git a/include/dqrobotics/robot_modeling/DQ_MobileBase.h b/include/dqrobotics/robot_modeling/DQ_MobileBase.h index 3ad9295..e6a7882 100644 --- a/include/dqrobotics/robot_modeling/DQ_MobileBase.h +++ b/include/dqrobotics/robot_modeling/DQ_MobileBase.h @@ -42,6 +42,7 @@ class DQ_MobileBase : public DQ_Kinematics //virtual int get_dim_configuration_space() const = 0; //virtual DQ fkm(const VectorXd& joint_configurations) const = 0; //virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations,const int& to_link) const = 0; + //virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link) const = 0; void set_frame_displacement(const DQ& pose); DQ frame_displacement(); diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h index 6b19720..c70e254 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulator.h @@ -20,6 +20,7 @@ This file is part of DQ Robotics. Contributors: - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) - Mateus Rodrigues Martins (martinsrmateus@gmail.com) +- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) */ #include @@ -49,10 +50,12 @@ class DQ_SerialManipulator: public DQ_Kinematics //Virtual virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const; + virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const; virtual DQ raw_fkm(const VectorXd& q_vec) const; //Pure virtual virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0; + virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0; virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0; //Overrides from DQ_Kinematics @@ -63,6 +66,9 @@ class DQ_SerialManipulator: public DQ_Kinematics virtual MatrixXd pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics virtual MatrixXd pose_jacobian(const VectorXd& q_vec) const override; //Override from DQ_Kinematics + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics + virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const override; //Override from DQ_Kinematics + }; } diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h index c035e4a..7e50fad 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h @@ -19,6 +19,7 @@ This file is part of DQ Robotics. Contributors: - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) +- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) */ @@ -47,13 +48,12 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator DQ_SerialManipulatorDH()=delete; DQ_SerialManipulatorDH(const MatrixXd& dh_matrix); - MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const; - MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const; - using DQ_SerialManipulator::raw_pose_jacobian; + using DQ_SerialManipulator::raw_pose_jacobian_derivative; using DQ_SerialManipulator::raw_fkm; MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h index c986425..b9a1c45 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h @@ -47,13 +47,12 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator DQ_SerialManipulatorMDH()=delete; DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix); - MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const; - MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const; - using DQ_SerialManipulator::raw_pose_jacobian; + using DQ_SerialManipulator::raw_pose_jacobian_derivative; using DQ_SerialManipulator::raw_fkm; MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; }; diff --git a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h index 609d7e3..9721ce9 100644 --- a/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h +++ b/include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h @@ -1,5 +1,5 @@ /** -(C) Copyright 2020 DQ Robotics Developers +(C) Copyright 2020-2022 DQ Robotics Developers This file is part of DQ Robotics. @@ -58,12 +58,21 @@ class DQ_SerialWholeBody : public DQ_Kinematics DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const; DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const; MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const; + MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& q, + const VectorXd& q_dot, + const int& to_ith_chain, + const int& to_jth_link) const; //To be implemented. //Abstract methods' implementation DQ fkm(const VectorXd& q) const override; DQ fkm(const VectorXd&, const int& to_ith_link) const override; MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override; MatrixXd pose_jacobian(const VectorXd& q) const override; + MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot, + const int& to_ith_link) const override; //To be implemented. + MatrixXd pose_jacobian_derivative (const VectorXd& q, + const VectorXd& q_dot) const override; //To be implemented. }; } diff --git a/include/dqrobotics/robot_modeling/DQ_WholeBody.h b/include/dqrobotics/robot_modeling/DQ_WholeBody.h index c22bd06..f0bae17 100644 --- a/include/dqrobotics/robot_modeling/DQ_WholeBody.h +++ b/include/dqrobotics/robot_modeling/DQ_WholeBody.h @@ -55,6 +55,11 @@ class DQ_WholeBody : public DQ_Kinematics DQ fkm(const VectorXd&, const int& to_chain) const override; MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override; MatrixXd pose_jacobian(const VectorXd& q) const override; + MatrixXd pose_jacobian_derivative(const VectorXd& q, + const VectorXd& q_dot, + const int& to_ith_link) const override; //To be implemented. + MatrixXd pose_jacobian_derivative (const VectorXd& q, + const VectorXd& q_dot) const override; //To be implemented. }; } diff --git a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp index fcfd175..bb1be09 100644 --- a/src/robot_modeling/DQ_DifferentialDriveRobot.cpp +++ b/src/robot_modeling/DQ_DifferentialDriveRobot.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2019 DQ Robotics Developers +(C) Copyright 2019-2022 DQ Robotics Developers This file is part of DQ Robotics. @@ -45,6 +45,25 @@ MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian(const double &phi) const return J; } +/** + * @brief returns the time derivative of the constraint Jacobian + * @param phi The orientation of the robot on the plane. + * @param phi_dot The time derivative of phi. + * @return a MatrixXd representing the desired Jacobian derivative + */ +MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian_derivative(const double &phi, const double &phi_dot) const +{ + const double& r = wheel_radius_; + double c = cos(phi); + double s = sin(phi); + + MatrixXd J_dot(3,2); + J_dot << -(r/2)*s, -(r/2)*s, + (r/2)*c, (r/2)*c, + 0, 0; + return J_dot*phi_dot; +} + MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q, const int &to_link) const { if(to_link!=0 && to_link!=1) @@ -63,4 +82,40 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q) const return pose_jacobian(q,1); } + +/** + * @brief returns the pose Jacobian derivative + * @param q The VectorXd representing the robot configuration. + * @param q_dot The VectorXd representing the robot configuration velocities. + * @param to_link The ith link which we want to compute the Jacobian derivative. + * @return a MatrixXd representing the desired Jacobian + */ +MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const +{ + if(to_link!=0 && to_link!=1) + throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian_derivative(q,q_dot, to_link) only accepts to_link in {0,1}."); + + /// Requirements + MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2); + MatrixXd J_holonomic_dot = DQ_HolonomicBase::pose_jacobian_derivative(q,q_dot,2); + MatrixXd J_c = constraint_jacobian(q(2)); + MatrixXd J_c_dot = constraint_jacobian_derivative(q(2), q_dot(2)); + MatrixXd J_dot = J_holonomic_dot*J_c + J_holonomic*J_c_dot; + return J_dot.block(0,0,8,to_link+1); +} + +/** + * @brief returns the pose Jacobian derivative + * @param q The VectorXd representing the robot configuration. + * @param q_dot The VectorXd representing the robot configuration velocities. + * @return a MatrixXd representing the desired Jacobian + */ +MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const +{ + // The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics + // The size of the configuration space is three but there is one constraint, so there are only + // to columns in the pose_jacobian + return pose_jacobian_derivative(q, q_dot, 1); +} + } diff --git a/src/robot_modeling/DQ_HolonomicBase.cpp b/src/robot_modeling/DQ_HolonomicBase.cpp index 025edae..d301d6e 100644 --- a/src/robot_modeling/DQ_HolonomicBase.cpp +++ b/src/robot_modeling/DQ_HolonomicBase.cpp @@ -107,4 +107,82 @@ MatrixXd DQ_HolonomicBase::pose_jacobian(const VectorXd &q) const return pose_jacobian(q,get_dim_configuration_space()-1); } +/** + * @brief returns the Jacobian derivative (J_dot) that satisfies + vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time + derivative of the pose and 'q_dot' represents the robot configuration velocities. + This method does not take into account the base displacement. + * @param q The VectorXd representing the robot configurations. + * @param q_dot The VectorXd representing the robot configuration velocities. + * @param to_link The ith link which we want to compute the Jacobian derivative. + * @return a MatrixXd representing the desired Jacobian derivative. + */ +MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const +{ + if(to_link >= 3 || to_link < 0) + { + throw std::runtime_error(std::string("Tried to access link index ") + std::to_string(to_link) + std::string(" which is unnavailable.")); + } + + const double& x = q(0); + const double& y = q(1); + const double& phi = q(2); + + const double& x_dot = q_dot(0); + const double& y_dot = q_dot(1); + const double& phi_dot = q_dot(2); + + const double c = cos(phi/2.0); + const double s = sin(phi/2.0); + + const double j71 = -0.25*c*phi_dot; + const double j62 = -j71; + const double j13 = -j62; + + const double j72 = -0.25*s*phi_dot; + const double j61 = j72; + const double j43 = j61; + + const double j63 = 0.25 * (-x_dot*s - 0.5*x*c*phi_dot + y_dot*c - 0.5*y*s*phi_dot); + const double j73 = 0.25 * (-x_dot*c + 0.5*x*s*phi_dot - y_dot*s - 0.5*y*c*phi_dot); + + MatrixXd J_dot(8,3); + J_dot << 0.0, 0.0, j13, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, j43, + 0.0, 0.0, 0.0, + j61, j62, j63, + j71, j72, j73, + 0.0, 0.0, 0.0; + return J_dot.block(0,0,8,to_link+1); +} + +/** + * @brief returns the Jacobian derivative 'J_dot' that satisfies + vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time + derivative of the pose and 'q_dot' represents the robot configuration velocities. + * @param q The VectorXd representing the robot configurations. + * @param q_dot The VectorXd representing the robot configuration velocities. + * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative. + * @return a MatrixXd representing the desired Jacobian derivative. + */ +MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const +{ + return haminus8(frame_displacement_)*raw_pose_jacobian_derivative(q, q_dot, to_link); +} + +/** + * @brief returns the Jacobian derivative 'J_dot' that satisfies + vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time + derivative of the pose and 'q_dot' represents the robot configuration velocities. + * @param q The VectorXd representing the robot configurations. + * @param q_dot The VectorXd representing the robot configuration velocities. + * @return a MatrixXd representing the desired Jacobian derivative. + */ +MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const +{ + return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1); +} + } diff --git a/src/robot_modeling/DQ_Kinematics.cpp b/src/robot_modeling/DQ_Kinematics.cpp index 3d93d60..f4a1b26 100644 --- a/src/robot_modeling/DQ_Kinematics.cpp +++ b/src/robot_modeling/DQ_Kinematics.cpp @@ -131,6 +131,19 @@ MatrixXd DQ_Kinematics::pose_jacobian(const VectorXd &joint_configurations) cons return pose_jacobian(joint_configurations, get_dim_configuration_space()-1); } +/** + * @brief returns the Jacobian derivative 'J_dot' that satisfies + vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time + derivative of the pose and 'q_dot' represents the robot configuration velocities. + * @param q The VectorXd representing the robot configurations. + * @param q_dot The VectorXd representing the robot configuration velocities. + * @return a MatrixXd representing the desired Jacobian derivative. + */ +MatrixXd DQ_Kinematics::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const +{ + return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1); +} + /** * @brief Returns the dimensions of the configuration space. * @return An int presenting the dimension of the configuration space. diff --git a/src/robot_modeling/DQ_SerialManipulator.cpp b/src/robot_modeling/DQ_SerialManipulator.cpp index 44a79b8..68f1ebb 100644 --- a/src/robot_modeling/DQ_SerialManipulator.cpp +++ b/src/robot_modeling/DQ_SerialManipulator.cpp @@ -1,5 +1,5 @@ /** -(C) Copyright 2011-2018 DQ Robotics Developers +(C) Copyright 2011-2020 DQ Robotics Developers This file is part of DQ Robotics. @@ -19,6 +19,7 @@ This file is part of DQ Robotics. Contributors: - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) - Mateus Rodrigues Martins (martinsrmateus@gmail.com) +- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) */ #include @@ -156,5 +157,63 @@ MatrixXd DQ_SerialManipulator::pose_jacobian(const VectorXd &q_vec) const return this->DQ_Kinematics::pose_jacobian(q_vec); } + +/** + * @brief This method returns the time derivative of the pose Jacobian. + * The base displacement and the effector are not taken into account. + * @param q. VectorXd representing the robot joint configuration. + * @param q_dot. VectorXd representing the robot joint velocities. + * @returns a MatrixXd representing the desired Jacobian derivative. + * + */ +MatrixXd DQ_SerialManipulator::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const +{ + _check_q_vec(q); + _check_q_vec(q_dot); + return raw_pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1); +} + + +/** + * @brief This method returns the first to_ith_link columns of the time derivative of the pose Jacobian. + * @param q. VectorXd representing the robot joint configuration. + * @param q_dot. VectorXd representing the robot joint velocities. + * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative + * will be calculated. + * @returns a MatrixXd representing the first to_ith_link columns of the desired Jacobian derivative. + * + */ +MatrixXd DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const +{ + _check_q_vec(q); + _check_q_vec(q_dot); + _check_to_ith_link(to_ith_link); + MatrixXd J_dot = raw_pose_jacobian_derivative(q, q_dot, to_ith_link); + + if(to_ith_link==this->get_dim_configuration_space()-1) + { + J_dot = hamiplus8(reference_frame_)*haminus8(curr_effector_)*J_dot; + } + else + { + J_dot = hamiplus8(reference_frame_)*J_dot; + } + return J_dot; +} + +/** + * @brief This method returns time derivative of the pose Jacobian. + * @param q. VectorXd representing the robot joint configuration. + * @param q_dot. VectorXd representing the robot joint velocities. + * @returns a MatrixXd representing the desired Jacobian derivative. + * + */ +MatrixXd DQ_SerialManipulator::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const +{ + _check_q_vec(q); + _check_q_vec(q_dot); + return this->DQ_Kinematics::pose_jacobian_derivative(q, q_dot); +} + }//namespace DQ_robotics diff --git a/src/robot_modeling/DQ_SerialManipulatorDH.cpp b/src/robot_modeling/DQ_SerialManipulatorDH.cpp index 2bc9f16..920b28b 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDH.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDH.cpp @@ -18,6 +18,7 @@ This file is part of DQ Robotics. Contributors: - Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp) +- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) */ #include @@ -189,68 +190,6 @@ VectorXd DQ_SerialManipulatorDH::get_types() const return dh_matrix_.row(4); } - -/** - * @brief This method returns the first to_ith_link columns of the pose Jacobian time derivative - * @param q_vec. Vector of joint values. - * @param q_vec_dot. Vector of joint velocity values. - * @param to_ith_link. The index to a link. This defines until which link the pose_jacobian_derivative - * will be calculated. - * @returns The first to_ith_link columns of the pose_jacobian_derivative. - * - */ -MatrixXd DQ_SerialManipulatorDH::pose_jacobian_derivative(const VectorXd &q_vec, const VectorXd &q_vec_dot, const int &to_ith_link) const -{ - _check_q_vec(q_vec); - _check_q_vec(q_vec_dot); - _check_to_ith_link(to_ith_link); - - int n = to_ith_link+1; - DQ x_effector = raw_fkm(q_vec,to_ith_link); - MatrixXd J = raw_pose_jacobian(q_vec,to_ith_link); - VectorXd vec_x_effector_dot = J*q_vec_dot.head(to_ith_link); - - DQ x = DQ(1); - MatrixXd J_dot = MatrixXd::Zero(8,n); - int jth=0; - - for(int i=0;i