From f3162f14dca630a6f4a40071ed3c010672b955e1 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 12 Apr 2016 16:30:47 +0200 Subject: [PATCH 1/3] Implemented moveit based distance computation --- cob_obstacle_distance/CMakeLists.txt | 18 +++++++++++++----- .../src/debug/debug_obstacle_distance_node.cpp | 1 + cob_obstacle_distance/src/distance_manager.cpp | 2 +- .../src/cob_twist_controller.cpp | 6 +++--- 4 files changed, 18 insertions(+), 9 deletions(-) diff --git a/cob_obstacle_distance/CMakeLists.txt b/cob_obstacle_distance/CMakeLists.txt index fd3e22d5..d1e9ed40 100644 --- a/cob_obstacle_distance/CMakeLists.txt +++ b/cob_obstacle_distance/CMakeLists.txt @@ -21,30 +21,38 @@ catkin_package( CATKIN_DEPENDS cob_control_msgs cob_srvs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser moveit_msgs roscpp roslib sensor_msgs shape_msgs std_msgs tf_conversions tf urdf visualization_msgs DEPENDS assimp Boost fcl INCLUDE_DIRS include - LIBRARIES parsers marker_shapes_management + LIBRARIES parsers marker_shapes_management distance_manager ) ### BUILD ### include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} ${FCL_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS} ${ASSIMP_INCLUDE_DIRS}) +add_library(helper_functions src/helpers/helper_functions.cpp) +add_dependencies(helper_functions ${catkin_EXPORTED_TARGETS}) +target_link_libraries(helper_functions ${catkin_LIBRARIES}) + add_library(parsers src/parsers/stl_parser.cpp src/parsers/mesh_parser.cpp) add_dependencies(parsers ${catkin_EXPORTED_TARGETS}) -target_link_libraries(parsers assimp ${fcl_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(parsers helper_functions assimp ${fcl_LIBRARIES} ${catkin_LIBRARIES}) add_library(marker_shapes_management src/marker_shapes/marker_shapes_impl.cpp src/marker_shapes/marker_shapes_interface.cpp src/shapes_manager.cpp src/link_to_collision.cpp) add_dependencies(marker_shapes_management ${catkin_EXPORTED_TARGETS}) target_link_libraries(marker_shapes_management parsers ${fcl_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) -add_executable(cob_obstacle_distance src/cob_obstacle_distance.cpp src/helpers/helper_functions.cpp src/distance_manager.cpp src/chainfk_solvers/advanced_chainfksolver_recursive.cpp) +add_library(distance_manager src/distance_manager.cpp src/chainfk_solvers/advanced_chainfksolver_recursive.cpp) +add_dependencies(distance_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(distance_manager marker_shapes_management ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) + +add_executable(cob_obstacle_distance src/cob_obstacle_distance.cpp) add_dependencies(cob_obstacle_distance ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(cob_obstacle_distance parsers marker_shapes_management ${fcl_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) +target_link_libraries(cob_obstacle_distance helper_functions parsers marker_shapes_management distance_manager ${fcl_LIBRARIES} ${catkin_LIBRARIES}) add_executable(debug_obstacle_distance_node src/debug/debug_obstacle_distance_node.cpp) add_dependencies(debug_obstacle_distance_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(debug_obstacle_distance_node ${catkin_LIBRARIES}) ### Install ### -install(TARGETS cob_obstacle_distance parsers marker_shapes_management debug_obstacle_distance_node +install(TARGETS cob_obstacle_distance parsers marker_shapes_management distance_manager debug_obstacle_distance_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp b/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp index bdaf44d3..2c4dcd7f 100644 --- a/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp +++ b/cob_obstacle_distance/src/debug/debug_obstacle_distance_node.cpp @@ -93,6 +93,7 @@ std::string chain_base_link_; marker_vector.id = 42; marker_vector.header.frame_id = chain_base_link_; + marker_vector.scale.x = 0.01; marker_vector.scale.y = 0.05; diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp index 8c3fbc5c..7140d700 100644 --- a/cob_obstacle_distance/src/distance_manager.cpp +++ b/cob_obstacle_distance/src/distance_manager.cpp @@ -90,7 +90,7 @@ int DistanceManager::init() return -1; } - if(!nh_.getParam("joint_names", this->joints_)) + if(!nh_.getParam("/arm_left/joint_names", this->joints_)) { ROS_ERROR("Failed to get parameter \"joint_names\"."); return -2; diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp index d4b8b5ea..75d4b10c 100644 --- a/cob_twist_controller/src/cob_twist_controller.cpp +++ b/cob_twist_controller/src/cob_twist_controller.cpp @@ -142,7 +142,7 @@ bool CobTwistController::initialize() { twist_controller_params_.frame_names.push_back(chain_.getSegment(i).getName()); } - register_link_client_ = nh_.serviceClient("obstacle_distance/registerLinkOfInterest"); + register_link_client_ = nh_.serviceClient("/register_links"); register_link_client_.waitForExistence(ros::Duration(5.0)); twist_controller_params_.constraint_ca = CA_OFF; @@ -164,7 +164,7 @@ bool CobTwistController::initialize() ros::Duration(1.0).sleep(); /// initialize ROS interfaces - obstacle_distance_sub_ = nh_.subscribe("obstacle_distance", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_); + obstacle_distance_sub_ = nh_.subscribe("/obstacle_distances", 1, &CallbackDataMediator::distancesToObstaclesCallback, &callback_data_mediator_); jointstate_sub_ = nh_.subscribe("joint_states", 1, &CobTwistController::jointstateCallback, this); twist_sub_ = nh_twist.subscribe("command_twist", 1, &CobTwistController::twistCallback, this); twist_stamped_sub_ = nh_twist.subscribe("command_twist_stamped", 1, &CobTwistController::twistStampedCallback, this); @@ -309,7 +309,7 @@ void CobTwistController::checkSolverAndConstraints(cob_twist_controller::TwistCo { if (!register_link_client_.exists()) { - ROS_ERROR("ServiceServer 'obstacle_distance/registerLinkOfInterest' does not exist. CA not possible"); + ROS_ERROR("ServiceServer '/register_links' does not exist. CA not possible"); twist_controller_params_.constraint_ca = CA_OFF; config.constraint_ca = static_cast(twist_controller_params_.constraint_ca); warning = true; From 61e56ae804de651719d6d810d8b167cc9bafc5ca Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 19 Apr 2016 17:22:11 +0200 Subject: [PATCH 2/3] Work in progress: verify if the nearest_point_frame_vector is equal to the old vector --- .../cob_twist_controller/constraints/constraint_ca_impl.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h index 16eca092..a655d794 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h @@ -287,9 +287,13 @@ void CollisionAvoidance::calcPartialValues() { if (params.frame_names.end() != str_it) { - Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector; +// Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector; + Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector; + Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector; + ROS_INFO_STREAM("Distance: " << sqrt(pow(distance_vec[0],2) + pow(distance_vec[1],2) + pow(distance_vec[1],2))); + // Create a skew-symm matrix as transformation between the segment root and the critical point. Eigen::Matrix3d skew_symm; skew_symm << 0.0, collision_pnt_vector.z(), -collision_pnt_vector.y(), From 992054cde1736d3f18e3f6832f121b4af6642299 Mon Sep 17 00:00:00 2001 From: ipa-fxm-cm Date: Tue, 10 May 2016 18:04:29 +0200 Subject: [PATCH 3/3] First implementation of CA with base_active --- .../cob_twist_controller.h | 2 + .../constraints/constraint_ca_impl.h | 45 +++++++++++++++---- .../inverse_differential_kinematics_solver.h | 2 + .../kinematic_extension_base.h | 2 +- .../kinematic_extension_builder.h | 2 +- .../kinematic_extension_dof.h | 4 +- .../kinematic_extension_lookat.h | 2 +- .../kinematic_extension_urdf.h | 2 +- .../src/cob_twist_controller.cpp | 5 +++ ...inverse_differential_kinematics_solver.cpp | 4 +- .../kinematic_extension_builder.cpp | 2 +- .../kinematic_extension_dof.cpp | 37 +++++++++++---- .../kinematic_extension_lookat.cpp | 2 +- .../kinematic_extension_urdf.cpp | 2 +- 14 files changed, 86 insertions(+), 27 deletions(-) diff --git a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h index d9974c3d..f5d5816f 100644 --- a/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h +++ b/cob_twist_controller/include/cob_twist_controller/cob_twist_controller.h @@ -78,6 +78,7 @@ class CobTwistController KDL::Chain chain_; JointStates joint_states_; KDL::Twist twist_odometry_cb_; + KDL::Twist twist_odometry_bl_; TwistControllerParams twist_controller_params_; @@ -119,6 +120,7 @@ class CobTwistController boost::recursive_mutex reconfig_mutex_; boost::shared_ptr< dynamic_reconfigure::Server > reconfigure_server_; + geometry_msgs::Pose pose_; }; #endif // COB_TWIST_CONTROLLER_COB_TWIST_CONTROLLER_H diff --git a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h index a655d794..bf187ade 100644 --- a/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h +++ b/cob_twist_controller/include/cob_twist_controller/constraints/constraint_ca_impl.h @@ -287,13 +287,10 @@ void CollisionAvoidance::calcPartialValues() { if (params.frame_names.end() != str_it) { -// Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector - it->frame_vector; Eigen::Vector3d collision_pnt_vector = it->nearest_point_frame_vector; Eigen::Vector3d distance_vec = it->nearest_point_frame_vector - it->nearest_point_obstacle_vector; - ROS_INFO_STREAM("Distance: " << sqrt(pow(distance_vec[0],2) + pow(distance_vec[1],2) + pow(distance_vec[1],2))); - // Create a skew-symm matrix as transformation between the segment root and the critical point. Eigen::Matrix3d skew_symm; skew_symm << 0.0, collision_pnt_vector.z(), -collision_pnt_vector.y(), @@ -301,6 +298,8 @@ void CollisionAvoidance::calcPartialValues() collision_pnt_vector.y(), -collision_pnt_vector.x(), 0.0; Eigen::Matrix3d ident = Eigen::Matrix3d::Identity(); + + // ToDo: dynamic matrix size for Base Active mode. Eigen::Matrix T; T.block(0, 0, 3, 3) << ident; T.block(0, 3, 3, 3) << skew_symm; @@ -312,7 +311,8 @@ void CollisionAvoidance::calcPartialValues() uint32_t frame_number = idx + 1; // segment nr not index represents frame number KDL::JntArray ja = this->joint_states_.current_q_; - KDL::Jacobian new_jac_chain(this->joint_states_.current_q_.rows()); + ja.resize((unsigned int)params.dof); + KDL::Jacobian new_jac_chain(ja.rows()); // ROS_INFO_STREAM("frame_number: " << frame_number); // ROS_INFO_STREAM("ja.rows: " << ja.rows()); @@ -393,22 +393,26 @@ void CollisionAvoidance::calcPredictionValue() if (params.frame_names.end() != str_it) { + unsigned int dof; if (this->constraint_params_.current_distances_.size() > 0) { uint32_t frame_number = (str_it - params.frame_names.begin()) + 1; // segment nr not index represents frame number KDL::FrameVel frame_vel; - + ROS_WARN_STREAM("frame_number" << frame_number); // ToDo: the fk_solver_vel_ is only initialized for the primary chain - kinematic extensions cannot be considered yet! KDL::JntArrayVel jnts_prediction_chain(params.dof); + for (unsigned int i = 0; i < params.dof; i++) { jnts_prediction_chain.q(i) = this->jnts_prediction_.q(i); jnts_prediction_chain.qdot(i) = this->jnts_prediction_.qdot(i); } - // ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows()); - // Calculate prediction for pos and vel - int error = this->fk_solver_vel_.JntToCart(this->jnts_prediction_, frame_vel, frame_number); + +// ROS_INFO_STREAM("jnts_prediction_chain.q.rows: " << jnts_prediction_chain.q.rows()); + + // Calculate prediction for the mainipulator + int error = this->fk_solver_vel_.JntToCart(jnts_prediction_chain, frame_vel, frame_number); if (error != 0) { ROS_ERROR_STREAM("Could not calculate twist for frame: " << frame_number << ". Error Code: " << error << " (" << this->fk_solver_vel_.strError(error) << ")"); @@ -417,7 +421,30 @@ void CollisionAvoidance::calcPredictionValue() } // ROS_INFO_STREAM("Calculated twist for frame: " << frame_number); - KDL::Twist twist = frame_vel.GetTwist(); // predicted frame twist + KDL::Twist predicted_twist_odometry; + if(params.kinematic_extension == BASE_ACTIVE) // jnts_prediction_ gives us the predicted joint_state for the plattform (calculated in stack_of_tasks_solver.cpp) + { + predicted_twist_odometry.vel.data[0] = this->jnts_prediction_.qdot(params.dof); + predicted_twist_odometry.vel.data[1] = this->jnts_prediction_.qdot(params.dof+1); + predicted_twist_odometry.vel.data[2] = this->jnts_prediction_.qdot(params.dof+2); + + predicted_twist_odometry.rot.data[0] = this->jnts_prediction_.qdot(params.dof+3); + predicted_twist_odometry.rot.data[1] = this->jnts_prediction_.qdot(params.dof+4); + predicted_twist_odometry.rot.data[2] = this->jnts_prediction_.qdot(params.dof+5); + } + else + { + predicted_twist_odometry.vel.data[0] = 0; + predicted_twist_odometry.vel.data[1] = 0; + predicted_twist_odometry.vel.data[2] = 0; + + predicted_twist_odometry.rot.data[0] = 0; + predicted_twist_odometry.rot.data[1] = 0; + predicted_twist_odometry.rot.data[2] = 0; + } + + + KDL::Twist twist = frame_vel.GetTwist() + predicted_twist_odometry; // predicted frame twist Eigen::Vector3d pred_twist_vel; tf::vectorKDLToEigen(twist.vel, pred_twist_vel); diff --git a/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h b/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h index 170f6e6c..af46f642 100644 --- a/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h +++ b/cob_twist_controller/include/cob_twist_controller/inverse_differential_kinematics_solver.h @@ -85,6 +85,8 @@ class InverseDifferentialKinematicsSolver /** CartToJnt for chain using SVD considering KinematicExtensions and various DampingMethods **/ virtual int CartToJnt(const JointStates& joint_states, + const geometry_msgs::Pose pose, + const KDL::Twist& twist, const KDL::Twist& v_in, KDL::JntArray& qdot_out); diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h index 77da1feb..9eeade41 100644 --- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h +++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_base.h @@ -51,7 +51,7 @@ class KinematicExtensionBase virtual bool initExtension() = 0; virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain) = 0; - virtual JointStates adjustJointStates(const JointStates& joint_states) = 0; + virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) = 0; virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params) = 0; virtual void processResultExtension(const KDL::JntArray& q_dot_ik) = 0; diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h index 20c571c8..b2428a8d 100644 --- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h +++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_builder.h @@ -66,7 +66,7 @@ class KinematicExtensionNone : public KinematicExtensionBase bool initExtension(); KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain); - JointStates adjustJointStates(const JointStates& joint_states); + JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist); LimiterParams adjustLimiterParams(const LimiterParams& limiter_params); void processResultExtension(const KDL::JntArray& q_dot_ik); }; diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h index daa495c3..267ba676 100644 --- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h +++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_dof.h @@ -52,7 +52,7 @@ class KinematicExtensionDOF : public KinematicExtensionBase virtual bool initExtension() = 0; virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain) = 0; - virtual JointStates adjustJointStates(const JointStates& joint_states) = 0; + virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) = 0; virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params) = 0; virtual void processResultExtension(const KDL::JntArray& q_dot_ik) = 0; @@ -94,7 +94,7 @@ class KinematicExtensionBaseActive : public KinematicExtensionDOF bool initExtension(); KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain); - JointStates adjustJointStates(const JointStates& joint_states); + JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist); LimiterParams adjustLimiterParams(const LimiterParams& limiter_params); void processResultExtension(const KDL::JntArray& q_dot_ik); diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h index 3b3c8d13..8db3ea26 100644 --- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h +++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_lookat.h @@ -65,7 +65,7 @@ class KinematicExtensionLookat : public KinematicExtensionBase bool initExtension(); virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain); - virtual JointStates adjustJointStates(const JointStates& joint_states); + virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist); virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params); virtual void processResultExtension(const KDL::JntArray& q_dot_ik); diff --git a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h index 03133139..87ae0aa5 100644 --- a/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h +++ b/cob_twist_controller/include/cob_twist_controller/kinematic_extensions/kinematic_extension_urdf.h @@ -55,7 +55,7 @@ class KinematicExtensionURDF : public KinematicExtensionBase bool initExtension(); virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain); - virtual JointStates adjustJointStates(const JointStates& joint_states); + virtual JointStates adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist); virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params); virtual void processResultExtension(const KDL::JntArray& q_dot_ik); diff --git a/cob_twist_controller/src/cob_twist_controller.cpp b/cob_twist_controller/src/cob_twist_controller.cpp index 75d4b10c..a9f03b9c 100644 --- a/cob_twist_controller/src/cob_twist_controller.cpp +++ b/cob_twist_controller/src/cob_twist_controller.cpp @@ -142,6 +142,7 @@ bool CobTwistController::initialize() { twist_controller_params_.frame_names.push_back(chain_.getSegment(i).getName()); } + register_link_client_ = nh_.serviceClient("/register_links"); register_link_client_.waitForExistence(ros::Duration(5.0)); twist_controller_params_.constraint_ca = CA_OFF; @@ -388,6 +389,8 @@ void CobTwistController::solveTwist(KDL::Twist twist) } int ret_ik = p_inv_diff_kin_solver_->CartToJnt(this->joint_states_, + pose_, + twist_odometry_bl_, twist, q_dot_ik); @@ -565,5 +568,7 @@ void CobTwistController::odometryCallback(const nav_msgs::Odometry::ConstPtr& ms // transform into chain_base twist_odometry_transformed_cb = cb_frame_bl * (twist_odometry_bl + tangential_twist_bl); + twist_odometry_bl_ = twist_odometry_bl; twist_odometry_cb_ = twist_odometry_transformed_cb; + pose_ = msg->pose.pose; // Needed for selfcollision avoidance in stack_of_tasks_solver.cpp } diff --git a/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp b/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp index d83ae576..fed4f137 100644 --- a/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp +++ b/cob_twist_controller/src/inverse_differential_kinematics_solver.cpp @@ -36,6 +36,8 @@ * Solve the inverse kinematics problem at the first order differential level. */ int InverseDifferentialKinematicsSolver::CartToJnt(const JointStates& joint_states, + const geometry_msgs::Pose pose, + const KDL::Twist& twist, const KDL::Twist& v_in, KDL::JntArray& qdot_out) { @@ -47,7 +49,7 @@ int InverseDifferentialKinematicsSolver::CartToJnt(const JointStates& joint_stat jnt2jac_.JntToJac(joint_states.current_q_, jac_chain); // ROS_INFO_STREAM("jac_chain.rows: " << jac_chain.rows() << ", jac_chain.columns: " << jac_chain.columns()); - JointStates joint_states_full = this->kinematic_extension_->adjustJointStates(joint_states); + JointStates joint_states_full = this->kinematic_extension_->adjustJointStates(joint_states, pose, twist); // ROS_INFO_STREAM("joint_states_full.current_q_: " << joint_states_full.current_q_.rows()); /// append columns to Jacobian in order to reflect additional DoFs of kinematical extension diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp index 715666b9..b30e92f3 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_builder.cpp @@ -86,7 +86,7 @@ KDL::Jacobian KinematicExtensionNone::adjustJacobian(const KDL::Jacobian& jac_ch /** * Method adjusting the JointStates used in inverse differential computation and limiters. No changes applied. */ -JointStates KinematicExtensionNone::adjustJointStates(const JointStates& joint_states) +JointStates KinematicExtensionNone::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) { return joint_states; } diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp index 60f60df3..e407af90 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_dof.cpp @@ -214,7 +214,7 @@ KDL::Jacobian KinematicExtensionBaseActive::adjustJacobian(const KDL::Jacobian& /** * Method adjusting the JointStates used in inverse differential computation and limiters. Fill neutrally. */ -JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& joint_states) +JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) { JointStates js; unsigned int chain_dof = joint_states.current_q_.rows(); @@ -230,13 +230,34 @@ JointStates KinematicExtensionBaseActive::adjustJointStates(const JointStates& j js.current_q_dot_(i) = joint_states.current_q_dot_(i); js.last_q_dot_(i) = joint_states.last_q_dot_(i); } - for (unsigned int i = 0; i < ext_dof_; i++) - { - js.current_q_(chain_dof + i) = 0.0; - js.last_q_(chain_dof + i) = 0.0; - js.current_q_dot_(chain_dof + i) = 0.0; - js.last_q_dot_(chain_dof + i) = 0.0; - } +// for (unsigned int i = 0; i < ext_dof_; i++) +// { +// js.current_q_(chain_dof + i) = 0.0; +// js.last_q_(chain_dof + i) = 0.0; +// js.current_q_dot_(chain_dof + i) = 0.0; +// js.last_q_dot_(chain_dof + i) = 0.0; +// } + double roll, pitch, yaw; + tf::Quaternion q; + q.setW(pose.orientation.w); + q.setX(pose.orientation.x); + q.setY(pose.orientation.y); + q.setZ(pose.orientation.z); + tf::Matrix3x3(q).getRPY(roll, pitch, yaw); + + js.current_q_(chain_dof + 0) = pose.position.x; + js.current_q_(chain_dof + 1) = pose.position.y; + js.current_q_(chain_dof + 2) = 0; + js.current_q_(chain_dof + 3) = 0; + js.current_q_(chain_dof + 4) = 0; + js.current_q_(chain_dof + 5) = yaw; + + js.current_q_dot_(chain_dof + 0) = twist.vel.data[0]; + js.current_q_dot_(chain_dof + 1) = twist.vel.data[1]; + js.current_q_dot_(chain_dof + 2) = 0; + js.current_q_dot_(chain_dof + 3) = 0; + js.current_q_dot_(chain_dof + 4) = 0; + js.current_q_dot_(chain_dof + 5) = twist.rot.data[2]; return js; } diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp index 2ea2ec67..6aaa45f8 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_lookat.cpp @@ -164,7 +164,7 @@ KDL::Jacobian KinematicExtensionLookat::adjustJacobian(const KDL::Jacobian& jac_ return jac_full; } -JointStates KinematicExtensionLookat::adjustJointStates(const JointStates& joint_states) +JointStates KinematicExtensionLookat::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) { boost::mutex::scoped_lock lock(mutex_); unsigned int chain_dof = joint_states.current_q_.rows(); diff --git a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp index 0be86074..ad9b8d0d 100644 --- a/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp +++ b/cob_twist_controller/src/kinematic_extensions/kinematic_extension_urdf.cpp @@ -191,7 +191,7 @@ KDL::Jacobian KinematicExtensionURDF::adjustJacobian(const KDL::Jacobian& jac_ch return jac_full; } -JointStates KinematicExtensionURDF::adjustJointStates(const JointStates& joint_states) +JointStates KinematicExtensionURDF::adjustJointStates(const JointStates& joint_states, const geometry_msgs::Pose& pose, const KDL::Twist& twist) { JointStates js; unsigned int chain_dof = joint_states.current_q_.rows();