Skip to content

Commit 139dff5

Browse files
juanjqojuanjqo
authored
[DQ_Kinematics and subclasses] pose_jacobian_derivative method is now abstract in DQ_Kinematics. (#48)
* [DQ_Kinematics.h, .cpp] Added the pose_jacobian_derivative method. * [DQ_HolonomicBase.h, .cpp] Added the pose_jacobian_derivative method. * [DQ_SerialWholeBody.h, .cpp] Added the pose_jacobian_derivative method. However, it is not implemented yet. * [DQ_DifferentialDriveRobot.h, .cpp] Added the pose_jacobian_derivative method. * [DQ_SerialManipulator.h, .cpp] Added the pose_jacobian_derivative method. * [DQ_SerialManipulatorDH.h, .cpp] Added the pose_jacobian_derivative method. * [DQ_SerialManipulatorMDH.h, .cpp] Added the pose_jacobian_derivative method. * [DQ_WholeBody.h, .cpp] Added the pose_jacobian_derivative method in the class WholeBody. However, the method is not implemented yet. * [DQ_Kinematics.h, .cpp] Renamed the variables joint_configurations and joint_velocity_configurations. * [DQ_Kinematics.cpp] Updated the documentation of the method pose_jacobian_derivative. * [DQ_DifferentialDriveRobot.h, .cpp] Renamed the variables joint_configurations and joint_velocity_configurations. * [DQ_HolonomicBase.h, .cpp] Renamed the variables joint_configurations and joint_velocity_configurations. * [DQ_MobileBase.h] Added a comment about the virtual method pose_jacobian_derivative. * [DQ_SerialWholeBody.h, .cpp] Renamed the variables joint_configurations and joint_velocity_configurations. * [DQ_WholeBody.h, .cpp] Renamed the variables joint_configurations and joint_velocity_configurations. * [DQ_Kinematics.cpp] Updated the documentation of the pose_jacobian_derivative() method. * [DQ_HolonomicBase.cpp] Updated the documentation of the pose_jacobian_derivative() method. * [DQ_DifferentialDriveRobot.h, .cpp] Renamed the variables joint_configurations and joint_velocity_configurations. I had forgotten to make the changes in one of the methods. * [DQ_Kinematics.h, .cpp] Renamed the variables of pose_jacobian_derivative * [DQ_DifferentialDriveRobot.h, .cpp] Renamed the variables of pose_jacobian_derivative * [DQ_HolonomicBase.h, .cpp] Renamed the variables of pose_jacobian_derivative * [DQ_MobileBase.h] Renamed the variables of pose_jacobian_derivative. * [DQ_WholeBody.h,.cpp] Renamed the variables of pose_jacobian_derivative. * [DQ_SerialWholeBody.h,.cpp] Renamed the variables of pose_jacobian_derivative. * [DQ_SerialManipulator.h,.cpp] Renamed the variables of pose_jacobian_derivative. * [DQ_SerialManipulatorDH.h,.cpp] Renamed the variables of pose_jacobian_derivative. * [DQ_SerialManipulatorMDH.h,.cpp] Renamed the variables of pose_jacobian_derivative.
1 parent 2317a3c commit 139dff5

17 files changed

+381
-100
lines changed

‎include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/**
2-
(C) Copyright 2019 DQ Robotics Developers
2+
(C) Copyright 2019-2022 DQ Robotics Developers
33
44
This file is part of DQ Robotics.
55
@@ -38,9 +38,14 @@ class DQ_DifferentialDriveRobot : public DQ_HolonomicBase
3838
DQ_DifferentialDriveRobot(const double& wheel_radius, const double& distance_between_wheels);
3939

4040
MatrixXd constraint_jacobian(const double& phi) const;
41-
41+
MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const;
4242
MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
4343
MatrixXd pose_jacobian(const VectorXd &q) const override;
44+
MatrixXd pose_jacobian_derivative(const VectorXd& q,
45+
const VectorXd& q_dot,
46+
const int& to_link) const override;
47+
MatrixXd pose_jacobian_derivative (const VectorXd& q,
48+
const VectorXd& q_dot) const override;
4449
};
4550

4651
}

‎include/dqrobotics/robot_modeling/DQ_HolonomicBase.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,15 @@ class DQ_HolonomicBase: public DQ_MobileBase
4040
virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override;
4141
virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override;
4242
virtual MatrixXd pose_jacobian(const VectorXd& q) const override;
43+
virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
44+
const VectorXd& q_dot, const int& to_link) const override;
45+
virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
46+
const VectorXd& q_dot) const override;
4347

4448
DQ raw_fkm(const VectorXd& q) const;
4549
MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const;
50+
MatrixXd raw_pose_jacobian_derivative(const VectorXd& q,
51+
const VectorXd& q_dot, const int& to_link = 2) const;
4652
};
4753

4854
}

‎include/dqrobotics/robot_modeling/DQ_Kinematics.h‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,8 +64,13 @@ class DQ_Kinematics
6464
virtual DQ fkm (const VectorXd& joint_configurations) const = 0;
6565
virtual DQ fkm (const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
6666
virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0;
67+
virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
68+
const VectorXd& q_dot,
69+
const int& to_ith_link) const = 0;
6770
//Virtual methods
6871
virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const;
72+
virtual MatrixXd pose_jacobian_derivative(const VectorXd& q,
73+
const VectorXd& q_dot) const;
6974
virtual int get_dim_configuration_space() const;
7075

7176
//Static methods

‎include/dqrobotics/robot_modeling/DQ_MobileBase.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ class DQ_MobileBase : public DQ_Kinematics
4242
//virtual int get_dim_configuration_space() const = 0;
4343
//virtual DQ fkm(const VectorXd& joint_configurations) const = 0;
4444
//virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations,const int& to_link) const = 0;
45+
//virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link) const = 0;
4546

4647
void set_frame_displacement(const DQ& pose);
4748
DQ frame_displacement();

‎include/dqrobotics/robot_modeling/DQ_SerialManipulator.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ This file is part of DQ Robotics.
2020
Contributors:
2121
- Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp)
2222
- Mateus Rodrigues Martins (martinsrmateus@gmail.com)
23+
- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
2324
*/
2425

2526
#include <dqrobotics/robot_modeling/DQ_Kinematics.h>
@@ -49,10 +50,12 @@ class DQ_SerialManipulator: public DQ_Kinematics
4950

5051
//Virtual
5152
virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const;
53+
virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const;
5254
virtual DQ raw_fkm(const VectorXd& q_vec) const;
5355

5456
//Pure virtual
5557
virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0;
58+
virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0;
5659
virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0;
5760

5861
//Overrides from DQ_Kinematics
@@ -63,6 +66,9 @@ class DQ_SerialManipulator: public DQ_Kinematics
6366

6467
virtual MatrixXd pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics
6568
virtual MatrixXd pose_jacobian(const VectorXd& q_vec) const override; //Override from DQ_Kinematics
69+
virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics
70+
virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const override; //Override from DQ_Kinematics
71+
6672
};
6773

6874
}

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ This file is part of DQ Robotics.
1919
2020
Contributors:
2121
- Murilo M. Marinho (murilo@nml.t.u-tokyo.ac.jp)
22+
- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
2223
*/
2324

2425

@@ -47,13 +48,12 @@ class DQ_SerialManipulatorDH: public DQ_SerialManipulator
4748
DQ_SerialManipulatorDH()=delete;
4849
DQ_SerialManipulatorDH(const MatrixXd& dh_matrix);
4950

50-
MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const;
51-
MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const;
52-
5351
using DQ_SerialManipulator::raw_pose_jacobian;
52+
using DQ_SerialManipulator::raw_pose_jacobian_derivative;
5453
using DQ_SerialManipulator::raw_fkm;
5554

5655
MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
56+
MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override;
5757
DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
5858
};
5959

‎include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -47,13 +47,12 @@ class DQ_SerialManipulatorMDH: public DQ_SerialManipulator
4747
DQ_SerialManipulatorMDH()=delete;
4848
DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix);
4949

50-
MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot, const int& to_ith_link) const;
51-
MatrixXd pose_jacobian_derivative(const VectorXd& q_vec, const VectorXd& q_vec_dot) const;
52-
5350
using DQ_SerialManipulator::raw_pose_jacobian;
51+
using DQ_SerialManipulator::raw_pose_jacobian_derivative;
5452
using DQ_SerialManipulator::raw_fkm;
5553

5654
MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
55+
MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override;
5756
DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
5857
};
5958

‎include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/**
2-
(C) Copyright 2020 DQ Robotics Developers
2+
(C) Copyright 2020-2022 DQ Robotics Developers
33
44
This file is part of DQ Robotics.
55
@@ -58,12 +58,21 @@ class DQ_SerialWholeBody : public DQ_Kinematics
5858
DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const;
5959
DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const;
6060
MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const;
61+
MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& q,
62+
const VectorXd& q_dot,
63+
const int& to_ith_chain,
64+
const int& to_jth_link) const; //To be implemented.
6165

6266
//Abstract methods' implementation
6367
DQ fkm(const VectorXd& q) const override;
6468
DQ fkm(const VectorXd&, const int& to_ith_link) const override;
6569
MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override;
6670
MatrixXd pose_jacobian(const VectorXd& q) const override;
71+
MatrixXd pose_jacobian_derivative(const VectorXd& q,
72+
const VectorXd& q_dot,
73+
const int& to_ith_link) const override; //To be implemented.
74+
MatrixXd pose_jacobian_derivative (const VectorXd& q,
75+
const VectorXd& q_dot) const override; //To be implemented.
6776
};
6877

6978
}

‎include/dqrobotics/robot_modeling/DQ_WholeBody.h‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,11 @@ class DQ_WholeBody : public DQ_Kinematics
5555
DQ fkm(const VectorXd&, const int& to_chain) const override;
5656
MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override;
5757
MatrixXd pose_jacobian(const VectorXd& q) const override;
58+
MatrixXd pose_jacobian_derivative(const VectorXd& q,
59+
const VectorXd& q_dot,
60+
const int& to_ith_link) const override; //To be implemented.
61+
MatrixXd pose_jacobian_derivative (const VectorXd& q,
62+
const VectorXd& q_dot) const override; //To be implemented.
5863
};
5964

6065
}

‎src/robot_modeling/DQ_DifferentialDriveRobot.cpp‎

Lines changed: 56 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/**
2-
(C) Copyright 2019 DQ Robotics Developers
2+
(C) Copyright 2019-2022 DQ Robotics Developers
33
44
This file is part of DQ Robotics.
55
@@ -45,6 +45,25 @@ MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian(const double &phi) const
4545
return J;
4646
}
4747

48+
/**
49+
* @brief returns the time derivative of the constraint Jacobian
50+
* @param phi The orientation of the robot on the plane.
51+
* @param phi_dot The time derivative of phi.
52+
* @return a MatrixXd representing the desired Jacobian derivative
53+
*/
54+
MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian_derivative(const double &phi, const double &phi_dot) const
55+
{
56+
const double& r = wheel_radius_;
57+
double c = cos(phi);
58+
double s = sin(phi);
59+
60+
MatrixXd J_dot(3,2);
61+
J_dot << -(r/2)*s, -(r/2)*s,
62+
(r/2)*c, (r/2)*c,
63+
0, 0;
64+
return J_dot*phi_dot;
65+
}
66+
4867
MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q, const int &to_link) const
4968
{
5069
if(to_link!=0 && to_link!=1)
@@ -63,4 +82,40 @@ MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q) const
6382
return pose_jacobian(q,1);
6483
}
6584

85+
86+
/**
87+
* @brief returns the pose Jacobian derivative
88+
* @param q The VectorXd representing the robot configuration.
89+
* @param q_dot The VectorXd representing the robot configuration velocities.
90+
* @param to_link The ith link which we want to compute the Jacobian derivative.
91+
* @return a MatrixXd representing the desired Jacobian
92+
*/
93+
MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const
94+
{
95+
if(to_link!=0 && to_link!=1)
96+
throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian_derivative(q,q_dot, to_link) only accepts to_link in {0,1}.");
97+
98+
/// Requirements
99+
MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2);
100+
MatrixXd J_holonomic_dot = DQ_HolonomicBase::pose_jacobian_derivative(q,q_dot,2);
101+
MatrixXd J_c = constraint_jacobian(q(2));
102+
MatrixXd J_c_dot = constraint_jacobian_derivative(q(2), q_dot(2));
103+
MatrixXd J_dot = J_holonomic_dot*J_c + J_holonomic*J_c_dot;
104+
return J_dot.block(0,0,8,to_link+1);
105+
}
106+
107+
/**
108+
* @brief returns the pose Jacobian derivative
109+
* @param q The VectorXd representing the robot configuration.
110+
* @param q_dot The VectorXd representing the robot configuration velocities.
111+
* @return a MatrixXd representing the desired Jacobian
112+
*/
113+
MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const
114+
{
115+
// The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics
116+
// The size of the configuration space is three but there is one constraint, so there are only
117+
// to columns in the pose_jacobian
118+
return pose_jacobian_derivative(q, q_dot, 1);
119+
}
120+
66121
}

0 commit comments

Comments
 (0)