While I was running my approach_retreat.cpp script in Section 3 of the course I realised that the gripper doesn’t close. Later when I tried to manually set the value with the lines below. The gripper closes. It also must be said that gripper_open does work.
joint_group_positions_gripper[2] = 0.55;
move_group_gripper.setJointValueTarget(joint_group_positions_gripper);
I then wrote the below code to help debug the problem, to check where the error is caused.
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
static const std::string PLANNING_GROUP_GRIPPER = "gripper";
moveit::planning_interface::MoveGroupInterface move_group_gripper(move_group_node, PLANNING_GROUP_GRIPPER);
move_group_gripper.setPlanningTime(10.0); // Increase planning time to 10 seconds
// Get current state
const moveit::core::JointModelGroup *joint_model_group_gripper = move_group_gripper.getCurrentState()->getJointModelGroup(PLANNING_GROUP_GRIPPER);
moveit::core::RobotStatePtr current_state_gripper = move_group_gripper.getCurrentState(10);
std::vector<double> joint_group_positions_gripper;
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper, joint_group_positions_gripper);
// Open Gripper
RCLCPP_INFO(LOGGER, "Open Gripper using named target!");
move_group_gripper.setNamedTarget("gripper_open");
moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
bool success_gripper = (move_group_gripper.plan(my_plan_gripper) == moveit::core::MoveItErrorCode::SUCCESS);
if (success_gripper) {
move_group_gripper.execute(my_plan_gripper);
} else {
RCLCPP_ERROR(LOGGER, "Planning failed for Opening Gripper using named target");
}
// Close Gripper
RCLCPP_INFO(LOGGER, "Close Gripper using named target!");
move_group_gripper.setNamedTarget("gripper_close");
success_gripper = (move_group_gripper.plan(my_plan_gripper) == moveit::core::MoveItErrorCode::SUCCESS);
if (success_gripper) {
move_group_gripper.execute(my_plan_gripper);
} else {
RCLCPP_ERROR(LOGGER, "Planning failed for Closing Gripper using named target");
// Print debug information
RCLCPP_INFO(LOGGER, "Joint values for gripper_open:");
for (size_t i = 0; i < joint_group_positions_gripper.size(); ++i) {
RCLCPP_INFO(LOGGER, "Joint %zu: %f", i, joint_group_positions_gripper[i]);
}
move_group_gripper.setNamedTarget("gripper_close");
current_state_gripper = move_group_gripper.getCurrentState(10);
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper, joint_group_positions_gripper);
RCLCPP_INFO(LOGGER, "Joint values for gripper_close:");
for (size_t i = 0; i < joint_group_positions_gripper.size(); ++i) {
RCLCPP_INFO(LOGGER, "Joint %zu: %f", i, joint_group_positions_gripper[i]);
}
}
rclcpp::shutdown();
return 0;
}
After running this I found that I would get the Error “MoveGroupInterface::plan() failed or timeout reached” when gripper_close is run. I have added additional files below to help check if there was any mistake during moveit configuration.
name.srdf
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="name">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="ur_manipulator">
<chain base_link="base_link" tip_link="tool0"/>
</group>
<group name="gripper">
<joint name="robotiq_85_base_joint"/>
<joint name="robotiq_85_left_inner_knuckle_joint"/>
<joint name="robotiq_85_left_finger_tip_joint"/>
<joint name="robotiq_85_left_knuckle_joint"/>
<joint name="robotiq_85_left_finger_joint"/>
<joint name="robotiq_85_right_inner_knuckle_joint"/>
<joint name="robotiq_85_right_finger_tip_joint"/>
<joint name="robotiq_85_right_knuckle_joint"/>
<joint name="robotiq_85_right_finger_joint"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="home" group="ur_manipulator">
<joint name="elbow_joint" value="1.5"/>
<joint name="shoulder_lift_joint" value="-2.5"/>
<joint name="shoulder_pan_joint" value="0"/>
<joint name="wrist_1_joint" value="-1.5"/>
<joint name="wrist_2_joint" value="-1.55"/>
<joint name="wrist_3_joint" value="0"/>
</group_state>
<group_state name="gripper_open" group="gripper">
<joint name="robotiq_85_left_knuckle_joint" value="0"/>
</group_state>
<group_state name="gripper_close" group="gripper">
<joint name="robotiq_85_left_knuckle_joint" value="0.55"/>
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="hand_ee" parent_link="wrist_3_link" group="gripper" parent_group="ur_manipulator"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="fixed_base" type="fixed" parent_frame="world" child_link="base_link"/>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="base_link_inertia" link2="ground_plane_box" reason="Adjacent"/>
<disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
<disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Never"/>
<disable_collisions link1="base_link_inertia" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="base_link_inertia" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="base_link_inertia" link2="wrist_rgbd_camera_link" reason="Adjacent"/>
<disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
<disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
<disable_collisions link1="forearm_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="ground_plane_box" link2="shoulder_link" reason="Never"/>
<disable_collisions link1="ground_plane_box" link2="wrist_rgbd_camera_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_finger_link" reason="Never"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_inner_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_left_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_finger_link" reason="Never"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_inner_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_base_link" link2="robotiq_85_right_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_base_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_base_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_base_link" link2="wrist_3_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_base_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_finger_tip_link" reason="Default"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_inner_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_finger_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_left_inner_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_left_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_finger_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_finger_tip_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_left_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_finger_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_inner_knuckle_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_finger_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_left_knuckle_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Default"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_tip_link" link2="robotiq_85_right_inner_knuckle_link" reason="Adjacent"/>
<disable_collisions link1="robotiq_85_right_finger_tip_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_finger_tip_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_inner_knuckle_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="robotiq_85_right_knuckle_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
<disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never"/>
<disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never"/>
<disable_collisions link1="shoulder_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="upper_arm_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/>
<disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/>
<disable_collisions link1="wrist_1_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
<disable_collisions link1="wrist_2_link" link2="wrist_rgbd_camera_link" reason="Never"/>
<disable_collisions link1="wrist_3_link" link2="wrist_rgbd_camera_link" reason="Never"/>
</robot>
robotic_85_gripper_macro.urdf.xacro
<?xml version="1.0"?>
<robot name="robotiq_85_gripper" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.transmission.xacro" />
<xacro:macro name="mimic_joint_plugin_gazebo" params="name_prefix parent_joint mimic_joint has_pid:=false multiplier:=1.0 offset:=0 sensitiveness:=0.0 max_effort:=1.0 robot_namespace:=''">
<gazebo>
<plugin name="${name_prefix}mimic_joint_plugin" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
<joint>${parent_joint}</joint>
<mimicJoint>${mimic_joint}</mimicJoint>
<xacro:if value="${has_pid}"> <!-- if set to true, PID parameters from "/gazebo_ros_control/pid_gains/${mimic_joint}" are loaded -->
<hasPID />
</xacro:if>
<multiplier>${multiplier}</multiplier>
<offset>${offset}</offset>
<sensitiveness>${sensitiveness}</sensitiveness> <!-- if absolute difference between setpoint and process value is below this threshold, do nothing; 0.0 = disable [rad] -->
<maxEffort>${max_effort}</maxEffort> <!-- only taken into account if has_pid:=true [Nm] -->
<xacro:unless value="${robot_namespace == ''}">
<robotNamespace>($robot_namespace)</robotNamespace>
</xacro:unless>
</plugin>
</gazebo>
</xacro:macro>
<xacro:macro name="robotiq_85_gripper" params="prefix parent *origin sim:=true">
<!-- The current hardware driver does not depend on ros2_control but we do utilize
it for simulating the gripper in Gazebo. Enabling this on hardware causes the
JointStateBroadcaster to publish values for all joints including ones that are
mimic'd which is causing issues on systems consuming the joint_state data. -->
<xacro:if value="${sim}">
<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.ros2_control.xacro"/>
<!-- ros2 control instance -->
<xacro:robotiq_85_gripper_ros2_control name="RobotiqGripperSystem"/>
</xacro:if>
<joint name="${prefix}robotiq_85_base_joint" type="fixed">
<parent link="${parent}"/>
<child link="${prefix}robotiq_85_base_link"/>
<xacro:insert_block name="origin"/>
</joint>
<link name="${prefix}robotiq_85_base_link">
<!-- Visual aid for grasping -->
<visual>
<origin xyz="0.16 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="0.007"/>
</geometry>
<material name="">
<color rgba="0 1.0 0 1.0"/>
</material>
</visual>
<!-- Gripper body -->
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_base_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_base_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.636951" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000380" ixy = "0.000000" ixz = "0.000000"
iyx = "0.000000" iyy = "0.001110" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.001171" />
</inertial>
</link>
<joint name="${prefix}robotiq_85_left_knuckle_joint" type="revolute">
<parent link="${prefix}robotiq_85_base_link"/>
<child link="${prefix}robotiq_85_left_knuckle_link"/>
<axis xyz="0 0 1"/>
<origin rpy="${pi} 0.0 0.0" xyz="0.05490451627 0.03060114443 0.0"/>
<limit lower="0.0" upper="0.804" velocity="0.5" effort="50"/>
<dynamics damping="100.0" />
</joint>
<gazebo reference="${prefix}robotiq_85_left_knuckle_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
<material>Gazebo/Grey</material>
</gazebo>
<joint name="${prefix}robotiq_85_right_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link"/>
<child link="${prefix}robotiq_85_right_knuckle_link"/>
<axis xyz="0 0 1"/>
<origin rpy="0.0 0.0 0.0" xyz="0.05490451627 -0.03060114443 0.0"/>
<limit lower="-3.14" upper="3.14" velocity="100.0" effort="50"/>
<mimic joint="${prefix}robotiq_85_left_knuckle_joint"/>
<dynamics damping="100.0" />
</joint>
<gazebo reference="${prefix}robotiq_85_right_knuckle_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
<material>Gazebo/Grey</material>
</gazebo>
<xacro:mimic_joint_plugin_gazebo name_prefix="{prefix}robotiq_85_right_knuckle_joint"
parent_joint="${prefix}robotiq_85_left_knuckle_joint" mimic_joint="${prefix}robotiq_85_right_knuckle_joint"
multiplier="1.0" max_effort="10.0" />
<link name="${prefix}robotiq_85_left_knuckle_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_knuckle_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_knuckle_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.018491" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000009" ixy = "-0.000001" ixz = "0.000000"
iyx = "-0.000001" iyy = "0.000001" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000010" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_knuckle_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_knuckle_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_knuckle_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.018491" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000009" ixy = "-0.000001" ixz = "0.000000"
iyx = "-0.000001" iyy = "0.000001" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000010" />
</inertial>
</link>
<joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
<parent link="${prefix}robotiq_85_left_knuckle_link"/>
<child link="${prefix}robotiq_85_left_finger_link"/>
<origin xyz="-0.00408552455 -0.03148604435 0.0" rpy="0 0 0" />
</joint>
<joint name="${prefix}robotiq_85_right_finger_joint" type="fixed">
<parent link="${prefix}robotiq_85_right_knuckle_link"/>
<child link="${prefix}robotiq_85_right_finger_link"/>
<origin xyz="-0.00408552455 -0.03148604435 0.0" rpy="0 0 0" />
</joint>
<link name="${prefix}robotiq_85_left_finger_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_finger_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_finger_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.027309" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000003" ixy = "-0.000002" ixz = "0.000000"
iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000020" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_finger_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_finger_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_finger_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.027309" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000003" ixy = "-0.000002" ixz = "0.000000"
iyx = "-0.000002" iyy = "0.000021" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000020" />
</inertial>
</link>
<joint name="${prefix}robotiq_85_left_inner_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link"/>
<child link="${prefix}robotiq_85_left_inner_knuckle_link"/>
<axis xyz="0 0 1"/>
<origin xyz="0.06142 0.0127 0" rpy="${pi} 0.0 0.0" />
<limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" offset="0"/>
</joint>
<xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}robotiq_85_left_inner_knuckle_joint"
parent_joint="${prefix}robotiq_85_left_knuckle_joint" mimic_joint="${prefix}robotiq_85_left_inner_knuckle_joint"
multiplier="1.0" max_effort="10.0" />
<joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link"/>
<child link="${prefix}robotiq_85_right_inner_knuckle_link"/>
<axis xyz="0 0 1"/>
<origin xyz="0.06142 -0.0127 0" rpy="0 0 0"/>
<limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" offset="0"/>
</joint>
<xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}robotiq_85_right_inner_knuckle_joint"
parent_joint="${prefix}robotiq_85_left_knuckle_joint" mimic_joint="${prefix}robotiq_85_right_inner_knuckle_joint"
multiplier="1.0" max_effort="10.0" />
<link name="${prefix}robotiq_85_left_inner_knuckle_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_inner_knuckle_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_inner_knuckle_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.029951" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000039" ixy = "0.000000" ixz = "0.000000"
iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000035" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_inner_knuckle_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_inner_knuckle_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_inner_knuckle_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.029951" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000039" ixy = "0.000000" ixz = "0.000000"
iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000035" />
</inertial>
</link>
<joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
<parent link="${prefix}robotiq_85_left_inner_knuckle_link"/>
<child link="${prefix}robotiq_85_left_finger_tip_link"/>
<axis xyz="0 0 1"/>
<origin xyz="0.04303959807 -0.03759940821 0.0" rpy="0.0 0.0 0.0"/>
<limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1"/>
</joint>
<xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}robotiq_85_left_finger_tip_joint"
parent_joint="${prefix}robotiq_85_left_knuckle_joint" mimic_joint="${prefix}robotiq_85_left_finger_tip_joint"
multiplier="1.0" max_effort="10.0" />
<joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
<parent link="${prefix}robotiq_85_right_inner_knuckle_link"/>
<child link="${prefix}robotiq_85_right_finger_tip_link"/>
<axis xyz="0 0 1"/>
<origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821 0.0"/>
<limit lower="-3.14" upper="3.14" velocity="100.0" effort="0.1"/>
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1"/>
</joint>
<xacro:mimic_joint_plugin_gazebo name_prefix="${prefix}robotiq_85_right_finger_tip_joint"
parent_joint="${prefix}robotiq_85_left_knuckle_joint" mimic_joint="${prefix}robotiq_85_right_finger_tip_joint"
multiplier="1.0" max_effort="10.0" />
<link name="${prefix}robotiq_85_left_finger_tip_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_finger_tip_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_finger_tip_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.019555" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000002" ixy = "0.000000" ixz = "0.000000"
iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000006" />
</inertial>
</link>
<gazebo reference="${prefix}robotiq_85_left_finger_tip_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
<material>Gazebo/Grey</material>
</gazebo>
<link name="${prefix}robotiq_85_right_finger_tip_link">
<visual>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/visual/robotiq_85_finger_tip_link.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://robotiq_85_description/meshes/collision/robotiq_85_finger_tip_link.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.019555" />
<origin xyz="0.0 0.0 0.0" />
<inertia ixx = "0.000002" ixy = "0.000000" ixz = "0.000000"
iyx = "0.000000" iyy = "0.000005" iyz = "0.000000"
izx = "0.000000" izy = "0.000000" izz = "0.000006" />
</inertial>
</link>
<gazebo reference="${prefix}robotiq_85_right_finger_tip_link">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>30.0</mu1>
<mu2>30.0</mu2>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
<material>Gazebo/Grey</material>
</gazebo>
<xacro:robotiq_85_gripper_transmission prefix="${prefix}" />
</xacro:macro>
</robot>
