diff --git a/examples/sas_core_example.cpp b/examples/sas_core_example.cpp index 766e52d..b462af0 100644 --- a/examples/sas_core_example.cpp +++ b/examples/sas_core_example.cpp @@ -31,39 +31,38 @@ #include #include -using namespace Eigen; using namespace marinholab::sas::core; int main(int,char**) { - //VectorXd concatenate(const VectorXd& a, const VectorXd& b); - VectorXd a(3); a << 1,2,3; - VectorXd b(3); b << 4,5,6; - VectorXd c(6); c << 1,2,3,4,5,6; + //Eigen::VectorXd concatenate(const Eigen::VectorXd& a, const Eigen::VectorXd& b); + Eigen::VectorXd a(3); a << 1,2,3; + Eigen::VectorXd b(3); b << 4,5,6; + Eigen::VectorXd c(6); c << 1,2,3,4,5,6; assert((concatenate(a,b)==c)); - //VectorXd concatenate(const std::vector& as); + //Eigen::VectorXd concatenate(const std::vector& as); auto as = {a,b}; assert((concatenate(as)==c)); - //MatrixXd vstack(const MatrixXd& A, const MatrixXd& B); - MatrixXd A(2,2); A << 1,2,3,4; - MatrixXd B(2,2); B << 5,6,7,8; - MatrixXd C(4,2); C << A,B; + //Eigen::MatrixXd vstack(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); + Eigen::MatrixXd A(2,2); A << 1,2,3,4; + Eigen::MatrixXd B(2,2); B << 5,6,7,8; + Eigen::MatrixXd C(4,2); C << A,B; assert((vstack(A,B)==C)); - //MatrixXd block_diag(const std::vector& As); + //Eigen::MatrixXd block_diag(const std::vector& As); auto As = {A,B}; - MatrixXd C_block_diag(4,4); C_block_diag << A,MatrixXd::Zero(2,2),MatrixXd::Zero(2,2),B; + Eigen::MatrixXd C_block_diag(4,4); C_block_diag << A,Eigen::MatrixXd::Zero(2,2),Eigen::MatrixXd::Zero(2,2),B; assert((block_diag(As)==C_block_diag)); - //std::vector split(const VectorXd& a, const std::vector& ns); - VectorXd a_split(10); a_split << 1,2,3,4,5,6,7,8,9,10; + //std::vector split(const Eigen::VectorXd& a, const std::vector& ns); + Eigen::VectorXd a_split(10); a_split << 1,2,3,4,5,6,7,8,9,10; std::vector ns = {2,5,3}; auto split_result = split(a_split,ns); - assert((split_result[0]==(VectorXd(2)<<1,2).finished())); - assert((split_result[1]==(VectorXd(5)<<3,4,5,6,7).finished())); - assert((split_result[2]==(VectorXd(3)<<8,9,10).finished())); + assert((split_result[0]==(Eigen::VectorXd(2)<<1,2).finished())); + assert((split_result[1]==(Eigen::VectorXd(5)<<3,4,5,6,7).finished())); + assert((split_result[2]==(Eigen::VectorXd(3)<<8,9,10).finished())); return 0; } diff --git a/examples/sas_robot_driver_example_main.cpp b/examples/sas_robot_driver_example_main.cpp index 769838c..f349789 100644 --- a/examples/sas_robot_driver_example_main.cpp +++ b/examples/sas_robot_driver_example_main.cpp @@ -46,7 +46,7 @@ int main(int,char**) auto configuration = marinholab::sas::core::RobotDriverExampleConfiguration(); configuration.name = "Example_Robot_123"; - configuration.initial_joint_positions = VectorXd::Random(7); + configuration.initial_joint_positions = Eigen::VectorXd::Random(7); auto robot_driver_example = marinholab::sas::core::RobotDriverExample(configuration, shutdown_signaler); @@ -55,7 +55,7 @@ int main(int,char**) std::cout << "Initial joint positions: " << robot_driver_example.get_joint_positions().transpose() << std::endl; - VectorXd target_joint_positions = VectorXd::Random(7); + Eigen::VectorXd target_joint_positions = Eigen::VectorXd::Random(7); std::cout << "Target joint positions: " << target_joint_positions.transpose() << std::endl; robot_driver_example.set_target_joint_positions(target_joint_positions); diff --git a/include/marinholab/sas/core/eigen3_std_conversions.hpp b/include/marinholab/sas/core/eigen3_std_conversions.hpp index 3e67861..82676a6 100644 --- a/include/marinholab/sas/core/eigen3_std_conversions.hpp +++ b/include/marinholab/sas/core/eigen3_std_conversions.hpp @@ -35,7 +35,6 @@ #include #include -using namespace Eigen; using namespace DQ_robotics; namespace marinholab::sas::core @@ -45,28 +44,28 @@ namespace marinholab::sas::core * @param vectorxd Source Eigen vector. * @return std::vector containing the same elements in order. */ -std::vector vectorxd_to_std_vector_double(const VectorXd& vectorxd); +std::vector vectorxd_to_std_vector_double(const Eigen::VectorXd& vectorxd); /** * @brief Convert an Eigen::VectorXi to a std::vector. * @param vectorxi Source Eigen integer vector. * @return std::vector containing the same elements in order. */ -std::vector vectorxi_to_std_vector_int(const VectorXi& vectorxi); +std::vector vectorxi_to_std_vector_int(const Eigen::VectorXi& vectorxi); /** * @brief Convert a std::vector to an Eigen::VectorXd. * @param std_vector_double Source std::vector. - * @return VectorXd containing the same elements. + * @return Eigen::VectorXd containing the same elements. */ -VectorXd std_vector_double_to_vectorxd(std::vector std_vector_double); +Eigen::VectorXd std_vector_double_to_vectorxd(std::vector std_vector_double); /** * @brief Convert a std::vector to an Eigen::VectorXi. * @param std_vector_int Source std::vector. - * @return VectorXi containing the same elements. + * @return Eigen::VectorXi containing the same elements. */ -VectorXi std_vector_int_to_vectorxi(std::vector std_vector_int); +Eigen::VectorXi std_vector_int_to_vectorxi(std::vector std_vector_int); /** * @brief Convert a std::vector to a DQ (dual quaternion) object. diff --git a/include/marinholab/sas/core/examples/sas_robot_driver_example.hpp b/include/marinholab/sas/core/examples/sas_robot_driver_example.hpp index 9d3e873..c0d338a 100644 --- a/include/marinholab/sas/core/examples/sas_robot_driver_example.hpp +++ b/include/marinholab/sas/core/examples/sas_robot_driver_example.hpp @@ -35,7 +35,6 @@ #include #include -using namespace Eigen; namespace marinholab::sas::core { @@ -48,8 +47,8 @@ namespace marinholab::sas::core struct RobotDriverExampleConfiguration { std::string name; - VectorXd initial_joint_positions; - std::tuple joint_limits; + Eigen::VectorXd initial_joint_positions; + std::tuple joint_limits; }; /** @@ -63,7 +62,7 @@ struct RobotDriverExampleConfiguration { protected: const RobotDriverExampleConfiguration configuration_; - VectorXd joint_positions_; + Eigen::VectorXd joint_positions_; public: RobotDriverExample(RobotDriverExample&) = delete; @@ -82,14 +81,14 @@ struct RobotDriverExampleConfiguration * @brief Get the current joint positions * @return Vector of joint positions (radians) */ - virtual VectorXd get_joint_positions() override; + virtual Eigen::VectorXd get_joint_positions() override; /** * @brief Set target joint positions * @param set_target_joint_positions_rad Target joint positions in radians * @throws std::runtime_error if the input vector has incorrect size */ - virtual void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) override; + virtual void set_target_joint_positions(const Eigen::VectorXd& set_target_joint_positions_rad) override; /** * @brief Connect the example driver (establish resources) diff --git a/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp b/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp index fe54d05..216f9f3 100644 --- a/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp +++ b/include/marinholab/sas/core/modeling/serial_manipulator_simulator_friendly.hpp @@ -39,7 +39,7 @@ namespace marinholab::sas::core::modeling { // The dqrobotics types this model builds on. (The Eigen types used in the -// signatures, e.g. MatrixXd/VectorXd, are injected into the global namespace +// signatures, e.g. Eigen::MatrixXd/Eigen::VectorXd, are injected into the global namespace // by the dqrobotics headers.) using DQ_robotics::DQ; using DQ_robotics::DQ_SerialManipulator; @@ -106,7 +106,7 @@ class SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator * @param to_ith_link Index of the terminal link. * @return An 8 x (to_ith_link+1) dual-quaternion pose Jacobian. */ - MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; + Eigen::MatrixXd raw_pose_jacobian(const Eigen::VectorXd& q_vec, const int& to_ith_link) const override; /** * @brief Time derivative of the raw pose Jacobian. * @param q Joint configuration vector. @@ -114,14 +114,14 @@ class SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator * @param to_ith_link Index of the terminal link. * @return An 8 x (to_ith_link+1) Jacobian-derivative matrix. */ - MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; + Eigen::MatrixXd raw_pose_jacobian_derivative(const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, const int& to_ith_link) const override; /** * @brief Raw forward kinematics of the chain up to a given link. * @param q_vec Joint configuration vector. * @param to_ith_link Index of the terminal link. * @return The dual-quaternion pose of the terminal link. */ - DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; + DQ raw_fkm(const Eigen::VectorXd &q_vec, const int &to_ith_link) const override; }; } diff --git a/include/marinholab/sas/core/sas_core.hpp b/include/marinholab/sas/core/sas_core.hpp index 34d33cf..a664bc3 100644 --- a/include/marinholab/sas/core/sas_core.hpp +++ b/include/marinholab/sas/core/sas_core.hpp @@ -34,7 +34,6 @@ #include -using namespace Eigen; namespace marinholab::sas::core { @@ -73,14 +72,14 @@ constexpr T incremental_mean(const T ¤t_mean, const int ¤t_number_of * @param b Second vector. * @return Concatenated vector containing all elements of a followed by b. */ -VectorXd concatenate(const VectorXd& a, const VectorXd& b); +Eigen::VectorXd concatenate(const Eigen::VectorXd& a, const Eigen::VectorXd& b); /** * @brief Concatenate a list of vectors into a single vector. * @param as Vector of vectors to concatenate in order. * @return Concatenated vector containing the elements of each input vector in order. */ -VectorXd concatenate(const std::vector& as); +Eigen::VectorXd concatenate(const std::vector& as); /** * @brief Stack two matrices vertically (A above B). @@ -89,21 +88,21 @@ VectorXd concatenate(const std::vector& as); * @return Matrix formed by stacking A on top of B. Columns must match. * @throws std::range_error if A and B have different numbers of columns. */ -MatrixXd vstack(const MatrixXd& A, const MatrixXd& B); +Eigen::MatrixXd vstack(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); /** * @brief Create a block-diagonal matrix from a list of matrices. * @param As Vector of matrices to place on the block diagonal. * @return Block-diagonal matrix containing the input matrices along its diagonal. */ -MatrixXd block_diag(const std::vector& As); +Eigen::MatrixXd block_diag(const std::vector& As); /** * @brief Split a vector into pieces with sizes specified by ns. * @param a Vector to split. * @param ns Sizes of each piece; their sum must equal a.size(). - * @return Vector containing the split VectorXd pieces. + * @return Vector containing the split Eigen::VectorXd pieces. */ -std::vector split(const VectorXd& a, const std::vector& ns); +std::vector split(const Eigen::VectorXd& a, const std::vector& ns); } diff --git a/include/marinholab/sas/core/sas_robot_driver.hpp b/include/marinholab/sas/core/sas_robot_driver.hpp index 4a720c2..20eca4a 100644 --- a/include/marinholab/sas/core/sas_robot_driver.hpp +++ b/include/marinholab/sas/core/sas_robot_driver.hpp @@ -46,7 +46,6 @@ #include #include -using namespace Eigen; namespace marinholab::sas::core { @@ -61,9 +60,9 @@ class RobotDriver protected: std::atomic_bool* break_loops_; //Deprecated std::shared_ptr shutdown_signaler_; - std::tuple joint_limits_; - VectorXd joint_velocities_; - VectorXd joint_torques_; + std::tuple joint_limits_; + Eigen::VectorXd joint_velocities_; + Eigen::VectorXd joint_torques_; std::unique_ptr clock_; std::unique_ptr watchdog_thread_; @@ -111,53 +110,53 @@ class RobotDriver * @brief Get current joint positions * @return Vector of joint positions (radians) */ - virtual VectorXd get_joint_positions() = 0; + virtual Eigen::VectorXd get_joint_positions() = 0; /** * @brief Set target joint positions * @param set_target_joint_positions_rad Target joint positions (radians) */ - virtual void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) = 0; + virtual void set_target_joint_positions(const Eigen::VectorXd& set_target_joint_positions_rad) = 0; /** * @brief Get current joint velocities * @return Vector of joint velocities * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) */ - virtual VectorXd get_joint_velocities(); + virtual Eigen::VectorXd get_joint_velocities(); /** * @brief Set target joint velocities * @param set_target_joint_velocities Target joint velocities * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) */ - virtual void set_target_joint_velocities(const VectorXd& set_target_joint_velocities); + virtual void set_target_joint_velocities(const Eigen::VectorXd& set_target_joint_velocities); /** * @brief Get current joint torques * @return Vector of joint torques * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) */ - virtual VectorXd get_joint_torques(); + virtual Eigen::VectorXd get_joint_torques(); /** * @brief Set target joint torques * @param set_target_joint_torques Target joint torques * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) */ - virtual void set_target_joint_torques(const VectorXd& set_target_joint_torques); + virtual void set_target_joint_torques(const Eigen::VectorXd& set_target_joint_torques); /** * @brief Get joint limits (min, max) * @return Tuple of (min_limits, max_limits) */ - virtual std::tuple get_joint_limits(); + virtual std::tuple get_joint_limits(); /** * @brief Set joint limits (min, max) * @param joint_limits Tuple of (min_limits, max_limits) */ - virtual void set_joint_limits(const std::tuple& joint_limits); + virtual void set_joint_limits(const std::tuple& joint_limits); /** * @brief Start the watchdog thread with the given period diff --git a/src/eigen3_std_conversions.cpp b/src/eigen3_std_conversions.cpp index a3e9306..ea4f762 100644 --- a/src/eigen3_std_conversions.cpp +++ b/src/eigen3_std_conversions.cpp @@ -31,13 +31,13 @@ namespace marinholab::sas::core { -std::vector vectorxd_to_std_vector_double(const VectorXd& vectorxd) +std::vector vectorxd_to_std_vector_double(const Eigen::VectorXd& vectorxd) { std::vector vec(vectorxd.data(), vectorxd.data() + vectorxd.rows() * vectorxd.cols()); return vec; } -VectorXd std_vector_double_to_vectorxd(std::vector std_vector_double) +Eigen::VectorXd std_vector_double_to_vectorxd(std::vector std_vector_double) { double* ptr = &std_vector_double[0]; Eigen::Map vec(ptr,std_vector_double.size()); //We need access to the pointer here so we cannot use const ref @@ -49,13 +49,13 @@ DQ std_vector_double_to_dq(const std::vector &std_vector_double) return DQ(std_vector_double_to_vectorxd(std_vector_double)); } -std::vector vectorxi_to_std_vector_int(const VectorXi &vectorxi) +std::vector vectorxi_to_std_vector_int(const Eigen::VectorXi &vectorxi) { std::vector vec(vectorxi.data(), vectorxi.data() + vectorxi.rows() * vectorxi.cols()); return vec; } -VectorXi std_vector_int_to_vectorxi(std::vector std_vector_int) +Eigen::VectorXi std_vector_int_to_vectorxi(std::vector std_vector_int) { int* ptr = &std_vector_int[0]; Eigen::Map vec(ptr,std_vector_int.size()); //We need access to the pointer here so we cannot use const ref diff --git a/src/sas_core.cpp b/src/sas_core.cpp index 4a045d4..9bd0e17 100644 --- a/src/sas_core.cpp +++ b/src/sas_core.cpp @@ -33,24 +33,24 @@ namespace marinholab::sas::core { /** - * @brief concatenate two VectorXd. - * @param a a VectorXd. - * @param b a VectorXd. + * @brief concatenate two Eigen::VectorXd. + * @param a a Eigen::VectorXd. + * @param b a Eigen::VectorXd. * @return the result of the concatenated vectors. */ -VectorXd concatenate(const VectorXd& a, const VectorXd& b) +Eigen::VectorXd concatenate(const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - return (VectorXd (a.size() + b.size()) << a, b).finished(); + return (Eigen::VectorXd (a.size() + b.size()) << a, b).finished(); } /** - * @brief concatenate a std::vector of VectorXd. - * @param a an std::vector of VectorXd. + * @brief concatenate a std::vector of Eigen::VectorXd. + * @param a an std::vector of Eigen::VectorXd. * @return the result of the concatenated vectors. */ -VectorXd concatenate(const std::vector& as) +Eigen::VectorXd concatenate(const std::vector& as) { - VectorXd b; + Eigen::VectorXd b; for(const auto& c : as) { b = concatenate(b, c); @@ -59,20 +59,20 @@ VectorXd concatenate(const std::vector& as) } /** - * @brief vstack vertically (row-wise) stack two MatrixXd. - * @param A the first MatrixXd. - * @param B the second MatrixXd. - * @return the vstacked MatrixXd. + * @brief vstack vertically (row-wise) stack two Eigen::MatrixXd. + * @param A the first Eigen::MatrixXd. + * @param B the second Eigen::MatrixXd. + * @return the vstacked Eigen::MatrixXd. * @exception a std::range_error if @a A and @a B don't have * the same number of columns. * @note returns an empty matrix if both arguments are empty * or return the other argument of only one of the arguments * is empty. */ -MatrixXd vstack(const MatrixXd& A, const MatrixXd& B) +Eigen::MatrixXd vstack(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) { if((A.size() == 0) && (B.size()==0)) - return MatrixXd(); + return Eigen::MatrixXd(); if(A.size() == 0) return B; if(B.size() == 0) @@ -80,23 +80,23 @@ MatrixXd vstack(const MatrixXd& A, const MatrixXd& B) if(A.cols()!=B.cols()) throw std::range_error("vstack needs inputs a and b with the same number of columns."); - return(MatrixXd(A.rows()+B.rows(),A.cols()) << A, B).finished(); + return(Eigen::MatrixXd(A.rows()+B.rows(),A.cols()) << A, B).finished(); } /** * @brief block_diag creates a block diagonal matrix - * using an input of std::vector. + * using an input of std::vector. * e.g. if As= [A, B, C], * then * block_diag(As) = * |A 0 0| * |0 B 0| * |0 0 C| - * @param As the std::vector contaning + * @param As the std::vector contaning * the matrix to form the block diagonal matrix. * @return the block diagonal matrix. */ -MatrixXd block_diag(const std::vector& As) +Eigen::MatrixXd block_diag(const std::vector& As) { int rows = 0; int cols = 0; @@ -106,7 +106,7 @@ MatrixXd block_diag(const std::vector& As) cols+=A.cols(); } - MatrixXd B = MatrixXd::Zero(rows,cols); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(rows,cols); int start_row = 0; int start_col = 0; for(const auto& A : As) @@ -124,15 +124,15 @@ MatrixXd block_diag(const std::vector& As) } /** - * @brief split splits the input VectorXd @a as into a set of subvectors + * @brief split splits the input Eigen::VectorXd @a as into a set of subvectors * defined by ns. - * @param as the VectorXd to be split. + * @param as the Eigen::VectorXd to be split. * @param ns the sizes of the subvectors. - * @return an std::vector of the splitted vectors. + * @return an std::vector of the splitted vectors. */ -std::vector split(const VectorXd& a, const std::vector& ns) +std::vector split(const Eigen::VectorXd& a, const std::vector& ns) { - std::vector as; + std::vector as; int n_acc = 0; for(const auto& n : ns) { diff --git a/src/sas_robot_driver.cpp b/src/sas_robot_driver.cpp index 1804041..49bd471 100644 --- a/src/sas_robot_driver.cpp +++ b/src/sas_robot_driver.cpp @@ -60,32 +60,32 @@ RobotDriver::~RobotDriver() } } -VectorXd RobotDriver::get_joint_velocities() +Eigen::VectorXd RobotDriver::get_joint_velocities() { throw std::runtime_error("Not implemented yet."); } -void RobotDriver::set_target_joint_velocities(const VectorXd&) +void RobotDriver::set_target_joint_velocities(const Eigen::VectorXd&) { throw std::runtime_error("Not implemented yet."); } -VectorXd RobotDriver::get_joint_torques() +Eigen::VectorXd RobotDriver::get_joint_torques() { throw std::runtime_error("Not implemented yet."); } -void RobotDriver::set_target_joint_torques(const VectorXd &) +void RobotDriver::set_target_joint_torques(const Eigen::VectorXd &) { throw std::runtime_error("Not implemented yet."); } -std::tuple RobotDriver::get_joint_limits() +std::tuple RobotDriver::get_joint_limits() { return joint_limits_; } -void RobotDriver::set_joint_limits(const std::tuple &joint_limits) +void RobotDriver::set_joint_limits(const std::tuple &joint_limits) { joint_limits_ = joint_limits; } diff --git a/src/sas_robot_driver_example.cpp b/src/sas_robot_driver_example.cpp index 5b96885..892dc88 100644 --- a/src/sas_robot_driver_example.cpp +++ b/src/sas_robot_driver_example.cpp @@ -43,12 +43,12 @@ marinholab::sas::core::RobotDriverExample::RobotDriverExample(const RobotDriverE set_joint_limits(configuration.joint_limits); } -VectorXd marinholab::sas::core::RobotDriverExample::get_joint_positions() +Eigen::VectorXd marinholab::sas::core::RobotDriverExample::get_joint_positions() { return joint_positions_; } -void marinholab::sas::core::RobotDriverExample::set_target_joint_positions(const VectorXd &set_target_joint_positions_rad) +void marinholab::sas::core::RobotDriverExample::set_target_joint_positions(const Eigen::VectorXd &set_target_joint_positions_rad) { if(joint_positions_.size() != set_target_joint_positions_rad.size()) throw std::runtime_error("marinholab::sas::core::RobotDriverExample::set_target_joint_positions invalid size for set_target_joint_positions_rad"); diff --git a/src/serial_manipulator_simulator_friendly.cpp b/src/serial_manipulator_simulator_friendly.cpp index c66412f..a1cd667 100644 --- a/src/serial_manipulator_simulator_friendly.cpp +++ b/src/serial_manipulator_simulator_friendly.cpp @@ -39,7 +39,7 @@ namespace marinholab::sas::core::modeling { // Free functions/constants from dqrobotics used by the implementation. (The -// types DQ/VectorXd/MatrixXd are already in scope via the header / the +// types DQ/Eigen::VectorXd/Eigen::MatrixXd are already in scope via the header / the // dqrobotics headers' global-namespace injection.) using DQ_robotics::Ad; using DQ_robotics::C8; @@ -71,8 +71,8 @@ SerialManipulatorSimulatorFriendly::SerialManipulatorSimulatorFriendly(const std // ``kDefaultJointLimit`` is a large (practically unbounded) value in // radians, the unit used for joint positions throughout dqrobotics. static constexpr double kDefaultJointLimit = 10.0; - lower_q_limit_ = VectorXd::Constant(actuation_types_.size(), -kDefaultJointLimit); - upper_q_limit_ = VectorXd::Constant(actuation_types_.size(), kDefaultJointLimit); + lower_q_limit_ = Eigen::VectorXd::Constant(actuation_types_.size(), -kDefaultJointLimit); + upper_q_limit_ = Eigen::VectorXd::Constant(actuation_types_.size(), kDefaultJointLimit); } DQ SerialManipulatorSimulatorFriendly::_joint_transformation(const double &q, const int &ith) const @@ -149,7 +149,7 @@ DQ SerialManipulatorSimulatorFriendly::_get_w(const int &ith) const throw std::runtime_error("Invalid actuation"); } -DQ SerialManipulatorSimulatorFriendly::raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const +DQ SerialManipulatorSimulatorFriendly::raw_fkm(const Eigen::VectorXd& q_vec, const int& to_ith_link) const { _check_q_vec(q_vec); _check_to_ith_link(to_ith_link); @@ -163,12 +163,12 @@ DQ SerialManipulatorSimulatorFriendly::raw_fkm(const VectorXd& q_vec, const int } -MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian(const VectorXd &q_vec, const int &to_ith_link) const +Eigen::MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian(const Eigen::VectorXd &q_vec, const int &to_ith_link) const { _check_q_vec(q_vec); _check_to_ith_link(to_ith_link); - MatrixXd J = MatrixXd::Zero(8,to_ith_link+1); + Eigen::MatrixXd J = Eigen::MatrixXd::Zero(8,to_ith_link+1); DQ x_effector = raw_fkm(q_vec,to_ith_link); DQ x(1); @@ -184,7 +184,7 @@ MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian(const VectorXd &q return J; } -MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const +Eigen::MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian_derivative(const Eigen::VectorXd &q, const Eigen::VectorXd &q_dot, const int &to_ith_link) const { _check_q_vec(q); _check_q_vec(q_dot); @@ -192,10 +192,10 @@ MatrixXd SerialManipulatorSimulatorFriendly::raw_pose_jacobian_derivative(const int n = to_ith_link+1; DQ x_effector = raw_fkm(q,to_ith_link); - MatrixXd J = raw_pose_jacobian(q,to_ith_link); - VectorXd vec_x_effector_dot = J*q_dot.head(n); + Eigen::MatrixXd J = raw_pose_jacobian(q,to_ith_link); + Eigen::VectorXd vec_x_effector_dot = J*q_dot.head(n); DQ x = DQ(1); - MatrixXd J_dot = MatrixXd::Zero(8,n); + Eigen::MatrixXd J_dot = Eigen::MatrixXd::Zero(8,n); int jth=0; for(int i=0;i