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

Make the integration with MoveIt and Gazebo working #2

Closed
wants to merge 11 commits into from
Closed

Make the integration with MoveIt and Gazebo working #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
4ead405
Define finger_joint2 as mimic joint of finger_joint1
rhaschke Oct 27, 2021
ce03ab7
Add detailed collision models for fingers
rhaschke Oct 29, 2021
6bc0977
Distinguish links hand_coarse and hand
rhaschke Oct 29, 2021
83670b7
Define hand_tcp frame
rhaschke Nov 11, 2021
cc5bb7d
Merge PRs #194 (tool-frame), #190 (mimic-joint), #191 (drop-joint-sta…
rhaschke Nov 11, 2021
04f705c
Moves finger collision geometries to the finger joints
rickstaa Aug 18, 2021
68f2927
fix: adds gazebo gravity vector to the gravity calculation
rickstaa Oct 8, 2021
29519c8
Fixes gazebo gripper action bug
rickstaa Oct 18, 2021
5ea7e4f
Adds position and velocity motion generators to FrankaHWSim
rickstaa Aug 23, 2021
e22c79c
Define hand_tcp frame
rhaschke Nov 11, 2021
212e35e
Add hand_tcp frame to gazebo xacro.
tahsinkose Nov 11, 2021
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
54 changes: 39 additions & 15 deletions franka_description/robots/hand.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,16 @@
<link name="${ns}_hand">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/hand.dae"/>
<mesh filename="package://franka_description/meshes/visual/hand.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/hand.stl" />
</geometry>
</collision>
</link>
<link name="${ns}_hand_coarse">
<collision>
<origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
<geometry>
Expand All @@ -33,39 +40,56 @@
<sphere radius="${0.04+safety_distance}" />
</geometry>
</collision>
<collision>
<origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}"/>
</link>
<link name="${ns}_leftfinger">
<visual>
<geometry>
<cylinder radius="${0.02+safety_distance}" length="0.1" />
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
</geometry>
</collision>
</visual>
<collision>
<origin xyz="0 -0.05 0.1" rpy="0 0 0"/>
<origin xyz="0 0.01 0.0415" rpy="0 0 0"/>
<geometry>
<sphere radius="${0.02+safety_distance}" />
</geometry>
</collision>
<collision>
<origin xyz="0 0.05 0.1" rpy="0 0 0"/>
<origin xyz="0 -0.015 0.0416" rpy="0 ${pi/2} ${pi/2}"/>
<geometry>
<sphere radius="${0.02+safety_distance}" />
<cylinder radius="${0.02+safety_distance}" length="0.05" />
</geometry>
</collision>
</link>
<link name="${ns}_leftfinger">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
</geometry>
</visual>
</link>
<joint name="${ns}_hand_coarse_joint" type="fixed">
<parent link="${ns}_hand" />
<child link="${ns}_hand_coarse" />
</joint>
<!-- Define the hand_tcp frame -->
<link name="${ns}_hand_tcp" />
<joint name="${ns}_hand_tcp_joint" type="fixed">
<origin xyz="0 0 0.1034" rpy="0 0 ${-pi/4}" />
<parent link="${ns}_hand" />
<child link="${ns}_hand_tcp" />
</joint>
<link name="${ns}_rightfinger">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<geometry>
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0 -0.01 0.0415" rpy="0 0 0"/>
<geometry>
<sphere radius="${0.02+safety_distance}" />
</geometry>
</collision>
<collision>
<origin xyz="0 0.015 0.0416" rpy="0 ${pi/2} ${pi/2}"/>
<geometry>
<cylinder radius="${0.02+safety_distance}" length="0.05" />
</geometry>
</collision>
</link>
<joint name="${ns}_finger_joint1" type="prismatic">
<parent link="${ns}_hand"/>
Expand Down
26 changes: 13 additions & 13 deletions franka_description/robots/panda_arm.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">

<!-- Name of this panda -->
<xacro:arg name="arm_id" default="panda" />
<!-- Should a franka_gripper be mounted at the flange?" -->
<xacro:arg name="hand" default="false" />
<!-- Is the robot being simulated in gazebo?" -->
<xacro:arg name="gazebo" default="false" />
<xacro:arg name="arm_id" default="panda" /> <!-- Name of this panda -->
<xacro:arg name="hand" default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
<xacro:arg name="gazebo" default="false" /> <!-- Is the robot being simulated in gazebo?" -->
<xacro:arg name="transmission" default="hardware_interface/EffortJointInterface" /> <!-- The transmission that is used in the gazebo joints -->

<xacro:property name="arm_id" value="$(arg arm_id)" />

<xacro:unless value="$(arg gazebo)">
<!-- Create a URDF for a real hardware -->
Expand Down Expand Up @@ -45,13 +45,13 @@
</joint>


<xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint7" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="${arm_id}_joint1" transmission="$(arg transmission)" />
<xacro:gazebo-joint joint="${arm_id}_joint2" transmission="$(arg transmission)" />
<xacro:gazebo-joint joint="${arm_id}_joint3" transmission="$(arg transmission)" />
<xacro:gazebo-joint joint="${arm_id}_joint4" transmission="$(arg transmission)" />
<xacro:gazebo-joint joint="${arm_id}_joint5" transmission="$(arg transmission)" />
<xacro:gazebo-joint joint="${arm_id}_joint6" transmission="$(arg transmission)" />
<xacro:gazebo-joint joint="${arm_id}_joint7" transmission="$(arg transmission)" />

<xacro:transmission-franka-state arm_id="$(arg arm_id)" />
<xacro:transmission-franka-model arm_id="$(arg arm_id)"
Expand Down
45 changes: 41 additions & 4 deletions franka_description/robots/panda_gazebo.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,6 +300,11 @@
izz="0.0017" />
</inertial>
</link>
<link name="${ns}_hand_coarse" />
<joint name="${ns}_hand_coarse_joint" type="fixed">
<parent link="${ns}_hand" />
<child link="${ns}_hand_coarse" />
</joint>

<link name="${ns}_leftfinger">
<xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
Expand All @@ -309,12 +314,31 @@
</geometry>
</visual>
<collision>
<origin xyz="0 0.0184 0.0101" rpy="0 0 0"/>
<geometry>
<box size="0.022 0.014 0.0177" />
</geometry>
</collision>
<collision>
<origin xyz="0 0.01655 0.02755" rpy="${3.257/(2*pi)} 0 0"/>
<geometry>
<mesh filename="package://franka_description/meshes/collision/finger.stl" />
<box size="0.022 0.00686 0.0237" />
</geometry>
</collision>
<collision>
<origin xyz="0 0.0073 0.045" rpy="0 0 0"/>
<geometry>
<box size="0.022 0.0147 0.0178" />
</geometry>
</collision>
</link>

<!-- Define the hand_tcp frame -->
<link name="${ns}_hand_tcp" />
<joint name="${ns}_hand_tcp_joint" type="fixed">
<origin xyz="0 0 0.1034" rpy="0 0 ${-pi/4}" />
<parent link="${ns}_hand" />
<child link="${ns}_hand_tcp" />
</joint>
<link name="${ns}_rightfinger">
<xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
<visual>
Expand All @@ -324,9 +348,21 @@
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 ${pi}"/>
<origin xyz="0 -0.0184 0.0101" rpy="0 0 0"/>
<geometry>
<box size="0.022 0.014 0.0177" />
</geometry>
</collision>
<collision>
<origin xyz="0 -0.01655 0.02755" rpy="${-3.257/(2*pi)} 0 0"/>
<geometry>
<box size="0.022 0.00686 0.0237" />
</geometry>
</collision>
<collision>
<origin xyz="0 -0.0073 0.045" rpy="0 0 0"/>
<geometry>
<mesh filename="package://franka_description/meshes/collision/finger.stl" />
<box size="0.022 0.0147 0.0178" />
</geometry>
</collision>
</link>
Expand All @@ -351,6 +387,7 @@
<origin xyz="0 0 0.0584" rpy="0 0 0"/>
<axis xyz="0 -1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="${ns}_finger_joint1" />
<dynamics damping="0.3"/>
</joint>
</xacro:macro>
Expand Down
23 changes: 23 additions & 0 deletions franka_gazebo/config/franka_hw_sim.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,3 +10,26 @@ franka_gripper:

finger2:
gains: { p: 100, i: 25, d: 20 }

# Motion generators PID gains
# NOTE: Needed since the gazebo_ros_control currently only directly support effort hardware
# interfaces (see #161).
motion_generators:
position:
gains:
panda_joint1: { p: 600, d: 30, i: 0 }
panda_joint2: { p: 600, d: 30, i: 0 }
panda_joint3: { p: 600, d: 30, i: 0 }
panda_joint4: { p: 600, d: 30, i: 0 }
panda_joint5: { p: 250, d: 10, i: 0 }
panda_joint6: { p: 150, d: 10, i: 0 }
panda_joint7: { p: 50, d: 5, i: 0 }
velocity:
gains:
panda_joint1: { p: 600, d: 30, i: 0 }
panda_joint2: { p: 600, d: 30, i: 0 }
panda_joint3: { p: 600, d: 30, i: 0 }
panda_joint4: { p: 600, d: 30, i: 0 }
panda_joint5: { p: 250, d: 10, i: 0 }
panda_joint6: { p: 150, d: 10, i: 0 }
panda_joint7: { p: 50, d: 5, i: 0 }
50 changes: 34 additions & 16 deletions franka_gazebo/config/sim_controllers.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,33 @@ cartesian_impedance_example_controller:
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7

position_joint_trajectory_controller:
type: position_controllers/JointTrajectoryController
joints:
- $(arg arm_id)_joint1
- $(arg arm_id)_joint2
- $(arg arm_id)_joint3
- $(arg arm_id)_joint4
- $(arg arm_id)_joint5
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
constraints:
goal_time: 0.5
panda_joint1:
goal: 0.05
panda_joint2:
goal: 0.05
panda_joint3:
goal: 0.05
panda_joint4:
goal: 0.05
panda_joint5:
goal: 0.05
panda_joint6:
goal: 0.05
panda_joint7:
goal: 0.05

effort_joint_trajectory_controller:
type: effort_controllers/JointTrajectoryController
joints:
Expand All @@ -50,19 +77,10 @@ effort_joint_trajectory_controller:
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
gains:
$(arg arm_id)_joint1: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint2: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint3: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint4: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
$(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
$(arg arm_id)_joint7: { p: 50, d: 5, i: 0 }
constraints:
goal_time: 0.5
$(arg arm_id)_joint1: { goal: 0.05 }
$(arg arm_id)_joint2: { goal: 0.05 }
$(arg arm_id)_joint3: { goal: 0.05 }
$(arg arm_id)_joint4: { goal: 0.05 }
$(arg arm_id)_joint5: { goal: 0.05 }
$(arg arm_id)_joint6: { goal: 0.05 }
$(arg arm_id)_joint7: { goal: 0.05 }
panda_joint1: { p: 600, d: 30, i: 0.0 }
panda_joint2: { p: 600, d: 30, i: 0.0 }
panda_joint3: { p: 600, d: 30, i: 0.0 }
panda_joint4: { p: 600, d: 30, i: 0.0 }
panda_joint5: { p: 250, d: 10, i: 0 }
panda_joint6: { p: 150, d: 10, i: 0 }
panda_joint7: { p: 50, d: 5, i: 0 }
13 changes: 13 additions & 0 deletions franka_gazebo/include/franka_gazebo/franka_hw_sim.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#pragma once

#include <control_toolbox/pid.h>
#include <franka/robot_state.h>
#include <franka_gazebo/joint.h>
#include <franka_hw/franka_model_interface.h>
Expand Down Expand Up @@ -29,6 +30,8 @@ namespace franka_gazebo {
* ### transmission_interface/SimpleTransmission
* - hardware_interface/JointStateInterface
* - hardware_interface/EffortJointInterface
* - hardware_interface/PositionJointInterface
* - hardware_interface/VelocityJointInterface
*
* ### franka_hw/FrankaStateInterface
* ### franka_hw/FrankaModelInterface
Expand Down Expand Up @@ -90,12 +93,20 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
void eStopActive(const bool active) override;

private:
std::array<double, 3> gravity_earth_;

std::string arm_id_;
gazebo::physics::ModelPtr robot_;
std::map<std::string, std::shared_ptr<franka_gazebo::Joint>> joints_;

enum ControlMethod { EFFORT, POSITION, VELOCITY };
std::map<std::string, ControlMethod> joint_control_methods_;
std::map<std::string, control_toolbox::Pid> pid_controllers_;

hardware_interface::JointStateInterface jsi_;
hardware_interface::EffortJointInterface eji_;
hardware_interface::PositionJointInterface pji_;
hardware_interface::VelocityJointInterface vji_;
franka_hw::FrankaStateInterface fsi_;
franka_hw::FrankaModelInterface fmi_;

Expand All @@ -112,6 +123,8 @@ class FrankaHWSim : public gazebo_ros_control::RobotHWSim {

void initJointStateHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
void initEffortCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
void initPositionCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
void initVelocityCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
void initFrankaStateHandle(const std::string& robot,
const urdf::Model& urdf,
const transmission_interface::TransmissionInfo& transmission);
Expand Down
5 changes: 5 additions & 0 deletions franka_gazebo/include/franka_gazebo/joint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#pragma once

#include <angles/angles.h>
#include <joint_limits_interface/joint_limits.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <gazebo/physics/Joint.hh>
Expand Down Expand Up @@ -35,6 +36,10 @@ struct Joint {
/// http://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1Joint.html
int type;

/// Joint limits @see
/// https://docs.ros.org/en/diamondback/api/urdf/html/classurdf_1_1JointLimits.html
joint_limits_interface::JointLimits limits;

/// The axis of rotation/translation of this joint in local coordinates
Eigen::Vector3d axis;

Expand Down
8 changes: 4 additions & 4 deletions franka_gazebo/launch/panda.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
<arg name="arm_id" default="panda" doc="Name of the panda robot to spawn" />
<arg name="use_gripper" default="true" doc="Should a franka hand be mounted on the flange?" />
<arg name="controller" default=" " doc="Which example controller should be started? (One of {cartesian_impedance,model,force}_example_controller)" />
<arg name="transmission" default="hardware_interface/EffortJointInterface" doc="Which transmission should be used for the panda joints? (One of hardware_interface/{EffortJointInterface,PositionJointInterface,VelocityJointInterface})" />
<arg name="x" default="0" doc="How far forward to place the base of the robot in [m]?" />
<arg name="y" default="0" doc="How far leftwards to place the base of the robot in [m]?" />
<arg name="z" default="0" doc="How far upwards to place the base of the robot in [m]?" />
Expand All @@ -36,12 +37,12 @@
<arg name="gui" value="$(eval not arg('headless'))"/>
<arg name="use_sim_time" value="true"/>
</include>

<param name="robot_description"
command="xacro $(find franka_description)/robots/panda_arm.urdf.xacro
command="xacro $(find franka_description)/robots/panda_arm.urdf.xacro
gazebo:=true
hand:=$(arg use_gripper)
arm_id:=$(arg arm_id)
transmission:=$(arg transmission)
xyz:='$(arg x) $(arg y) $(arg z)'
rpy:='$(arg roll) $(arg pitch) $(arg yaw)'">
</param>
Expand Down Expand Up @@ -89,7 +90,6 @@
<param name="link_name" value="$(arg arm_id)_link0" />
<remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
</node>

<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz" if="$(arg rviz)"/>
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz" if="$(arg rviz)"/>

</launch>
Loading