Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

added subscriber for online impedance regulation #277

Open
wants to merge 3 commits into
base: develop
Choose a base branch
from
Open

added subscriber for online impedance regulation #277

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9b4016d
added subscriber for online impedance regulation
Aug 22, 2022
98fda39
change HOLDING state franka gripper plugin | added friction fingers f…
Aug 23, 2022
5a94750
modifications to urdf panda
Sep 15, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 28 additions & 0 deletions franka_description/robots/panda_arm.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,16 @@
<xacro:gazebo-joint joint="${arm_id}_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
</xacro:if>

<xacro:include filename="$(find open_vico_cameras)/urdf/realsense_d435.xacro"/>

<xacro:realsense_d435 parent="${arm_id}_link8"
camera_name_prefix="realsense_d435_camera1"
enable_pointCloud="true"
align_depth="false"
enable_gazebo_plugin="true">
<origin xyz="-0.045 0.025 -0.04" rpy="0.0 -1.3 -0.75"/>
</xacro:realsense_d435>

<!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
<link name="world" />
<joint name="world_joint" type="fixed">
Expand Down Expand Up @@ -66,6 +76,24 @@
</plugin>
<self_collide>true</self_collide>
</gazebo>

<gazebo>
<plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
<arm>
<arm_name>panda</arm_name>
<palm_link> panda_link7 </palm_link>
<gripper_link> panda_leftfinger </gripper_link>
<gripper_link> panda_rightfinger </gripper_link>
</arm>
<forces_angle_tolerance>90</forces_angle_tolerance>
<update_rate>4</update_rate>
<grip_count_threshold>4</grip_count_threshold>
<max_grip_count>8</max_grip_count> <!-- this should be at least double than grip_count_threshold -->
<release_tolerance>0.00001</release_tolerance>
<disable_collisions_on_attach>false</disable_collisions_on_attach>
<contact_topic>_default_topic_</contact_topic>
</plugin>
</gazebo>
</xacro:if>

</robot>
56 changes: 56 additions & 0 deletions franka_description/robots/panda_gazebo.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -314,12 +314,40 @@
<geometry>
<box size="0.03 0.005 0.047" />
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>2.0</mu2>
</ode>
</friction>
<!-- <contact>
<ode>
<kp>1e15</kp>
<kd>1e13</kd>
</ode>
</contact> -->
</surface>
</collision>
<collision>
<origin xyz="0 0.007 0.045" rpy="0 0 0"/>
<geometry>
<box size="0.03 0.015 0.017" />
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>2.0</mu2>
</ode>
</friction>
<!-- <contact>
<ode>
<kp>1e15</kp>
<kd>1e13</kd>
</ode>
</contact> -->
</surface>
</collision>
</link>

Expand All @@ -336,12 +364,40 @@
<geometry>
<box size="0.03 0.005 0.047" />
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>2.0</mu2>
</ode>
</friction>
<!-- <contact>
<ode>
<kp>1e15</kp>
<kd>1e13</kd>
</ode>
</contact> -->
</surface>
</collision>
<collision>
<origin xyz="0 -0.007 0.045" rpy="0 0 0"/>
<geometry>
<box size="0.03 0.015 0.017" />
</geometry>
<surface>
<friction>
<ode>
<mu>2.0</mu>
<mu2>2.0</mu2>
</ode>
</friction>
<!-- <contact>
<ode>
<kp>1e15</kp>
<kd>1e13</kd>
</ode>
</contact> -->
</surface>
</collision>
</link>

Expand Down
2 changes: 1 addition & 1 deletion franka_description/robots/utils.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@
<!-- Usually you want no "snap out" velocity and a generous -->
<!-- penetration depth to keep the grasping stable -->
<max_vel>0</max_vel>
<min_depth>0.003</min_depth>
<min_depth>0.001</min_depth>
</ode>
</contact>
<friction>
Expand Down
5 changes: 5 additions & 0 deletions ...e_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <controller_interface/multi_interface_controller.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float64.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
Expand Down Expand Up @@ -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
22 changes: 22 additions & 0 deletions franka_example_controllers/src/cartesian_impedance_example_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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;
Expand Down
22 changes: 13 additions & 9 deletions franka_gazebo/launch/panda.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@
-J $(arg arm_id)_joint7 0.785398163397"
/>

<arg name="launch_interactive_marker" default="false"/>

<include file="$(find gazebo_ros)/launch/empty_world.launch" if="$(arg gazebo)">
<arg name="world_name" value="$(arg world)"/>
<arg name="paused" value="true"/>
Expand All @@ -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)'">
</param>
rpy:='$(arg roll) $(arg pitch) $(arg yaw)'"/>

<rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" />
<rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" />
Expand Down Expand Up @@ -92,15 +93,18 @@
</node>

<!-- Start only if cartesian_impedance_example_controller -->
<node name="interactive_marker"
pkg="franka_example_controllers"
type="interactive_marker.py"
if="$(eval arg('controller') == 'cartesian_impedance_example_controller')">
<param name="link_name" value="$(arg arm_id)_link0" />
</node>
<group if="$(arg launch_interactive_marker)">
<node name="interactive_marker"
pkg="franka_example_controllers"
type="interactive_marker.py"
if="$(eval arg('controller') == 'cartesian_impedance_example_controller')">
<param name="link_name" value="$(arg arm_id)_link0" />
</node>
</group>

<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find teleimpedance-m5core2)/rviz/new_config.rviz" if="$(arg rviz)"/>

</group>

<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_gazebo)/config/franka_sim_description_with_marker.rviz" if="$(arg rviz)"/>

</launch>
10 changes: 6 additions & 4 deletions franka_gazebo/src/franka_gripper_sim.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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<std::mutex> lock(this->mutex_);
f_d = -this->config_.force_desired / 2.0;
}
Expand Down