diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro index d5b8d0061..9f6f44d54 100644 --- a/franka_description/robots/panda_arm.urdf.xacro +++ b/franka_description/robots/panda_arm.urdf.xacro @@ -35,6 +35,16 @@ + + + + + + @@ -66,6 +76,24 @@ true + + + + + panda + panda_link7 + panda_leftfinger + panda_rightfinger + + 90 + 4 + 4 + 8 + 0.00001 + false + _default_topic_ + + diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro index c2b6baf1c..b0bbd0483 100644 --- a/franka_description/robots/panda_gazebo.xacro +++ b/franka_description/robots/panda_gazebo.xacro @@ -314,12 +314,40 @@ + + + + 2.0 + 2.0 + + + + + + + + 2.0 + 2.0 + + + + @@ -336,12 +364,40 @@ + + + + 2.0 + 2.0 + + + + + + + + 2.0 + 2.0 + + + + diff --git a/franka_description/robots/utils.xacro b/franka_description/robots/utils.xacro index 25b3fd4e2..d6b53a1aa 100644 --- a/franka_description/robots/utils.xacro +++ b/franka_description/robots/utils.xacro @@ -36,7 +36,7 @@ 0 - 0.003 + 0.001 diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h index 2e5d63460..4002935f5 100644 --- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -63,7 +64,11 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn // Equilibrium pose subscriber ros::Subscriber sub_equilibrium_pose_; + ros::Subscriber sub_desired_transl_stiffness_; + ros::Subscriber sub_desired_rot_stiffness_; void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); + void desiredTranslStiffnessCallback(const std_msgs::Float64Ptr& msg); + void desiredRotStiffnessCallback(const std_msgs::Float64Ptr& msg); }; } // namespace franka_example_controllers diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp index 7783f71de..a34da6c31 100644 --- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp @@ -23,6 +23,14 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo "equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this, ros::TransportHints().reliable().tcpNoDelay()); + sub_desired_transl_stiffness_ = node_handle.subscribe( + "desired_transl_stiffness", 20, &CartesianImpedanceExampleController::desiredTranslStiffnessCallback, this, + ros::TransportHints().reliable().tcpNoDelay()); + + sub_desired_rot_stiffness_ = node_handle.subscribe( + "desired_rot_stiffness", 20, &CartesianImpedanceExampleController::desiredRotStiffnessCallback, this, + ros::TransportHints().reliable().tcpNoDelay()); + std::string arm_id; if (!node_handle.getParam("arm_id", arm_id)) { ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id"); @@ -226,6 +234,20 @@ void CartesianImpedanceExampleController::complianceParamCallback( nullspace_stiffness_target_ = config.nullspace_stiffness; } +void CartesianImpedanceExampleController::desiredTranslStiffnessCallback(const std_msgs::Float64Ptr& msg) { + cartesian_stiffness_target_.topLeftCorner(3, 3) + << msg->data * Eigen::Matrix3d::Identity(); + cartesian_damping_target_.topLeftCorner(3, 3) + << 2.0 * sqrt(msg->data) * Eigen::Matrix3d::Identity(); +} + +void CartesianImpedanceExampleController::desiredRotStiffnessCallback(const std_msgs::Float64Ptr& msg) { + cartesian_stiffness_target_.bottomRightCorner(3, 3) + << msg->data * Eigen::Matrix3d::Identity(); + cartesian_damping_target_.bottomRightCorner(3, 3) + << 2.0 * sqrt(msg->data) * Eigen::Matrix3d::Identity(); +} + void CartesianImpedanceExampleController::equilibriumPoseCallback( const geometry_msgs::PoseStampedConstPtr& msg) { position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 96124bfef..6f2f904e2 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -29,6 +29,8 @@ -J $(arg arm_id)_joint7 0.785398163397" /> + + @@ -43,8 +45,7 @@ hand:=$(arg use_gripper) arm_id:=$(arg arm_id) xyz:='$(arg x) $(arg y) $(arg z)' - rpy:='$(arg roll) $(arg pitch) $(arg yaw)'"> - + rpy:='$(arg roll) $(arg pitch) $(arg yaw)'"/> @@ -92,15 +93,18 @@ - - - + + + + + + + - diff --git a/franka_gazebo/src/franka_gripper_sim.cpp b/franka_gazebo/src/franka_gripper_sim.cpp index 117bd0327..23eb1360c 100644 --- a/franka_gazebo/src/franka_gripper_sim.cpp +++ b/franka_gazebo/src/franka_gripper_sim.cpp @@ -119,8 +119,8 @@ void FrankaGripperSim::update(const ros::Time& now, const ros::Duration& period) this->mutex_.unlock(); if (state == State::IDLE) { // Track position of other finger to simulate mimicked joints + high damping - control(this->finger1_, this->pid1_, this->finger2_.getPosition(), 0, 0, period); - control(this->finger2_, this->pid2_, this->finger1_.getPosition(), 0, 0, period); + control(this->finger1_, this->pid1_, w_d / 2.0, 0, 0, period); + control(this->finger2_, this->pid2_, w_d / 2.0, 0, 0, period); return; } @@ -135,8 +135,10 @@ void FrankaGripperSim::update(const ros::Time& now, const ros::Duration& period) if (state == State::HOLDING) { // When an object is grasped, next to the force to apply, also track the other finger // to not make both fingers drift away from middle simultaneously - w1_d = this->finger2_.getPosition(); - w2_d = this->finger1_.getPosition(); + // w1_d = this->finger2_.getPosition(); + // w2_d = this->finger1_.getPosition(); + w1_d = w_d / 2.0; + w2_d = w_d / 2.0; std::lock_guard lock(this->mutex_); f_d = -this->config_.force_desired / 2.0; }