From 671f8c7983182a62d786efb8b99bd4f8676ca32b Mon Sep 17 00:00:00 2001 From: Witty-Wizard <shashankmarch27@gmail.com> Date: Mon, 26 Aug 2024 12:21:42 +0530 Subject: [PATCH] [Added]: World Link for the arm to be fixed --- .../ur_description/urdf/inc/ur5_macro.xacro | 8 +- .../ur_description/urdf/inc/ur_macro.xacro | 195 +++++++++++------- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- .../config/initial_positions.yaml | 4 +- 11 files changed, 138 insertions(+), 101 deletions(-) diff --git a/workspace/src/ur_description/urdf/inc/ur5_macro.xacro b/workspace/src/ur_description/urdf/inc/ur5_macro.xacro index 645b240..40c363d 100644 --- a/workspace/src/ur_description/urdf/inc/ur5_macro.xacro +++ b/workspace/src/ur_description/urdf/inc/ur5_macro.xacro @@ -20,7 +20,8 @@ model will be used, which will almost certainly not correspond to any real robot. --> - <xacro:macro name="ur5_robot" params=" + <xacro:macro name="ur5_robot" + params=" prefix joint_limits_parameters_file:='$(find ur_description)/config/ur5/joint_limits.yaml' kinematics_parameters_file:='$(find ur_description)/config/ur5/default_kinematics.yaml' @@ -31,7 +32,7 @@ safety_pos_margin:=0.15 safety_k_position:=20" > - <xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro"/> + <xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro" /> <xacro:ur_robot prefix="${prefix}" joint_limits_parameters_file="${joint_limits_parameters_file}" @@ -43,5 +44,6 @@ safety_pos_margin="${safety_pos_margin}" safety_k_position="${safety_k_position}" /> + </xacro:macro> -</robot> +</robot> \ No newline at end of file diff --git a/workspace/src/ur_description/urdf/inc/ur_macro.xacro b/workspace/src/ur_description/urdf/inc/ur_macro.xacro index d5eaca8..634056d 100644 --- a/workspace/src/ur_description/urdf/inc/ur_macro.xacro +++ b/workspace/src/ur_description/urdf/inc/ur_macro.xacro @@ -54,7 +54,8 @@ <xacro:include filename="$(find ur_description)/urdf/inc/ur_transmissions.xacro" /> <xacro:include filename="$(find ur_description)/urdf/inc/ur_common.xacro" /> - <xacro:macro name="ur_robot" params=" + <xacro:macro name="ur_robot" + params=" prefix joint_limits_parameters_file kinematics_parameters_file @@ -67,154 +68,162 @@ > <!-- Load configuration data from the provided .yaml files --> <xacro:read_model_data - joint_limits_parameters_file="${joint_limits_parameters_file}" + joint_limits_parameters_file="${joint_limits_parameters_file}" kinematics_parameters_file="${kinematics_parameters_file}" physical_parameters_file="${physical_parameters_file}" - visual_parameters_file="${visual_parameters_file}"/> + visual_parameters_file="${visual_parameters_file}" /> <!-- Add URDF transmission elements (for ros_control) --> <xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" /> <!-- links: main serial chain --> - <link name="${prefix}base_link"/> + <link name="${prefix}base_link" /> <link name="${prefix}base_link_inertia"> <visual> - <origin xyz="0 0 0" rpy="0 0 ${pi}"/> + <origin xyz="0 0 0" rpy="0 0 ${pi}" /> <geometry> - <mesh filename="${base_visual_mesh}"/> + <mesh filename="${base_visual_mesh}" /> </geometry> <material name="${base_visual_material_name}"> - <color rgba="${base_visual_material_color}"/> + <color rgba="${base_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 0" rpy="0 0 ${pi}"/> + <origin xyz="0 0 0" rpy="0 0 ${pi}" /> <geometry> - <mesh filename="${base_collision_mesh}"/> + <mesh filename="${base_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${base_inertia_radius}" length="${base_inertia_length}" mass="${base_mass}"> + <xacro:cylinder_inertial radius="${base_inertia_radius}" length="${base_inertia_length}" + mass="${base_mass}"> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <link name="${prefix}shoulder_link"> <visual> - <origin xyz="0 0 0" rpy="0 0 ${pi}"/> + <origin xyz="0 0 0" rpy="0 0 ${pi}" /> <geometry> - <mesh filename="${shoulder_visual_mesh}"/> + <mesh filename="${shoulder_visual_mesh}" /> </geometry> <material name="${shoulder_visual_material_name}"> - <color rgba="${shoulder_visual_material_color}"/> + <color rgba="${shoulder_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 0" rpy="0 0 ${pi}"/> + <origin xyz="0 0 0" rpy="0 0 ${pi}" /> <geometry> - <mesh filename="${shoulder_collision_mesh}"/> + <mesh filename="${shoulder_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${shoulder_inertia_radius}" length="${shoulder_inertia_length}" mass="${shoulder_mass}"> + <xacro:cylinder_inertial radius="${shoulder_inertia_radius}" + length="${shoulder_inertia_length}" mass="${shoulder_mass}"> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <link name="${prefix}upper_arm_link"> <visual> - <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/> + <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}" /> <geometry> - <mesh filename="${upper_arm_visual_mesh}"/> + <mesh filename="${upper_arm_visual_mesh}" /> </geometry> <material name="${upper_arm_visual_material_name}"> - <color rgba="${upper_arm_visual_material_color}"/> + <color rgba="${upper_arm_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/> + <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}" /> <geometry> - <mesh filename="${upper_arm_collision_mesh}"/> + <mesh filename="${upper_arm_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${upperarm_inertia_radius}" length="${upperarm_inertia_length}" mass="${upper_arm_mass}"> - <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" /> + <xacro:cylinder_inertial radius="${upperarm_inertia_radius}" + length="${upperarm_inertia_length}" mass="${upper_arm_mass}"> + <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}" + rpy="0 ${pi/2} 0" /> </xacro:cylinder_inertial> </link> <link name="${prefix}forearm_link"> <visual> - <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/> + <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}" /> <geometry> - <mesh filename="${forearm_visual_mesh}"/> + <mesh filename="${forearm_visual_mesh}" /> </geometry> <material name="${forearm_visual_material_name}"> - <color rgba="${forearm_visual_material_color}"/> + <color rgba="${forearm_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/> + <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}" /> <geometry> - <mesh filename="${forearm_collision_mesh}"/> + <mesh filename="${forearm_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}" mass="${forearm_mass}"> + <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}" + mass="${forearm_mass}"> <origin xyz="${-0.5 * forearm_inertia_length} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" /> </xacro:cylinder_inertial> </link> <link name="${prefix}wrist_1_link"> <visual> <!-- TODO: Move this to a parameter --> - <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/> + <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0" /> <geometry> - <mesh filename="${wrist_1_visual_mesh}"/> + <mesh filename="${wrist_1_visual_mesh}" /> </geometry> <material name="${wrist_1_visual_material_name}"> - <color rgba="${wrist_1_visual_material_color}"/> + <color rgba="${wrist_1_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/> + <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0" /> <geometry> - <mesh filename="${wrist_1_collision_mesh}"/> + <mesh filename="${wrist_1_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}" mass="${wrist_1_mass}"> + <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}" + mass="${wrist_1_mass}"> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <link name="${prefix}wrist_2_link"> <visual> - <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/> + <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0" /> <geometry> - <mesh filename="${wrist_2_visual_mesh}"/> + <mesh filename="${wrist_2_visual_mesh}" /> </geometry> <material name="${wrist_2_visual_material_name}"> - <color rgba="${wrist_2_visual_material_color}"/> + <color rgba="${wrist_2_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/> + <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0" /> <geometry> - <mesh filename="${wrist_2_collision_mesh}"/> + <mesh filename="${wrist_2_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}" mass="${wrist_2_mass}"> + <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}" + mass="${wrist_2_mass}"> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <link name="${prefix}wrist_3_link"> <visual> - <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/> + <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0" /> <geometry> - <mesh filename="${wrist_3_visual_mesh}"/> + <mesh filename="${wrist_3_visual_mesh}" /> </geometry> <material name="${wrist_3_visual_material_name}"> - <color rgba="${wrist_3_visual_material_color}"/> + <color rgba="${wrist_3_visual_material_color}" /> </material> </visual> <collision> - <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/> + <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0" /> <geometry> - <mesh filename="${wrist_3_collision_mesh}"/> + <mesh filename="${wrist_3_collision_mesh}" /> </geometry> </collision> - <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}" mass="${wrist_3_mass}"> + <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}" + mass="${wrist_3_mass}"> <origin xyz="0.0 0.0 ${-0.5 * wrist_3_inertia_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> @@ -233,87 +242,105 @@ <joint name="${prefix}shoulder_pan_joint" type="revolute"> <parent link="${prefix}base_link_inertia" /> <child link="${prefix}shoulder_link" /> - <origin xyz="${shoulder_x} ${shoulder_y} ${shoulder_z}" rpy="${shoulder_roll} ${shoulder_pitch} ${shoulder_yaw}" /> + <origin xyz="${shoulder_x} ${shoulder_y} ${shoulder_z}" + rpy="${shoulder_roll} ${shoulder_pitch} ${shoulder_yaw}" /> <axis xyz="0 0 1" /> <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" - effort="${shoulder_pan_effort_limit}" velocity="${shoulder_pan_velocity_limit}"/> + effort="${shoulder_pan_effort_limit}" velocity="${shoulder_pan_velocity_limit}" /> <xacro:if value="${safety_limits}"> - <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> + <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" + soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" + k_position="${safety_k_position}" k_velocity="0.0" /> </xacro:if> - <dynamics damping="0" friction="0"/> + <dynamics damping="0" friction="0" /> </joint> <joint name="${prefix}shoulder_lift_joint" type="revolute"> <parent link="${prefix}shoulder_link" /> <child link="${prefix}upper_arm_link" /> - <origin xyz="${upper_arm_x} ${upper_arm_y} ${upper_arm_z}" rpy="${upper_arm_roll} ${upper_arm_pitch} ${upper_arm_yaw}" /> + <origin xyz="${upper_arm_x} ${upper_arm_y} ${upper_arm_z}" + rpy="${upper_arm_roll} ${upper_arm_pitch} ${upper_arm_yaw}" /> <axis xyz="0 0 1" /> <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" - effort="${shoulder_lift_effort_limit}" velocity="${shoulder_lift_velocity_limit}"/> + effort="${shoulder_lift_effort_limit}" velocity="${shoulder_lift_velocity_limit}" /> <xacro:if value="${safety_limits}"> - <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> + <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" + soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" + k_position="${safety_k_position}" k_velocity="0.0" /> </xacro:if> - <dynamics damping="0" friction="0"/> + <dynamics damping="0" friction="0" /> </joint> <joint name="${prefix}elbow_joint" type="revolute"> <parent link="${prefix}upper_arm_link" /> <child link="${prefix}forearm_link" /> - <origin xyz="${forearm_x} ${forearm_y} ${forearm_z}" rpy="${forearm_roll} ${forearm_pitch} ${forearm_yaw}" /> + <origin xyz="${forearm_x} ${forearm_y} ${forearm_z}" + rpy="${forearm_roll} ${forearm_pitch} ${forearm_yaw}" /> <axis xyz="0 0 1" /> <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" - effort="${elbow_joint_effort_limit}" velocity="${elbow_joint_velocity_limit}"/> + effort="${elbow_joint_effort_limit}" velocity="${elbow_joint_velocity_limit}" /> <xacro:if value="${safety_limits}"> - <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> + <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" + soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" + k_position="${safety_k_position}" k_velocity="0.0" /> </xacro:if> - <dynamics damping="0" friction="0"/> + <dynamics damping="0" friction="0" /> </joint> <joint name="${prefix}wrist_1_joint" type="revolute"> <parent link="${prefix}forearm_link" /> <child link="${prefix}wrist_1_link" /> - <origin xyz="${wrist_1_x} ${wrist_1_y} ${wrist_1_z}" rpy="${wrist_1_roll} ${wrist_1_pitch} ${wrist_1_yaw}" /> + <origin xyz="${wrist_1_x} ${wrist_1_y} ${wrist_1_z}" + rpy="${wrist_1_roll} ${wrist_1_pitch} ${wrist_1_yaw}" /> <axis xyz="0 0 1" /> <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" - effort="${wrist_1_effort_limit}" velocity="${wrist_1_velocity_limit}"/> + effort="${wrist_1_effort_limit}" velocity="${wrist_1_velocity_limit}" /> <xacro:if value="${safety_limits}"> - <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> + <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" + soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" + k_position="${safety_k_position}" k_velocity="0.0" /> </xacro:if> - <dynamics damping="0" friction="0"/> + <dynamics damping="0" friction="0" /> </joint> <joint name="${prefix}wrist_2_joint" type="revolute"> <parent link="${prefix}wrist_1_link" /> <child link="${prefix}wrist_2_link" /> - <origin xyz="${wrist_2_x} ${wrist_2_y} ${wrist_2_z}" rpy="${wrist_2_roll} ${wrist_2_pitch} ${wrist_2_yaw}" /> + <origin xyz="${wrist_2_x} ${wrist_2_y} ${wrist_2_z}" + rpy="${wrist_2_roll} ${wrist_2_pitch} ${wrist_2_yaw}" /> <axis xyz="0 0 1" /> <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" - effort="${wrist_2_effort_limit}" velocity="${wrist_2_velocity_limit}"/> + effort="${wrist_2_effort_limit}" velocity="${wrist_2_velocity_limit}" /> <xacro:if value="${safety_limits}"> - <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> + <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" + soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" + k_position="${safety_k_position}" k_velocity="0.0" /> </xacro:if> - <dynamics damping="0" friction="0"/> + <dynamics damping="0" friction="0" /> </joint> <joint name="${prefix}wrist_3_joint" type="revolute"> <parent link="${prefix}wrist_2_link" /> <child link="${prefix}wrist_3_link" /> - <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" /> + <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" + rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" /> <axis xyz="0 0 1" /> <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" - effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/> + effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}" /> <xacro:if value="${safety_limits}"> - <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> + <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" + soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" + k_position="${safety_k_position}" k_velocity="0.0" /> </xacro:if> - <dynamics damping="0" friction="0"/> + <dynamics damping="0" friction="0" /> </joint> <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform --> - <link name="${prefix}base"/> + <link name="${prefix}base" /> <joint name="${prefix}base_link-base_fixed_joint" type="fixed"> <!-- Note the rotation over Z of pi radians: as base_link is REP-103 aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed to correctly align 'base' with the 'Base' coordinate system of the UR controller. --> - <origin xyz="0 0 0" rpy="0 0 ${pi}"/> - <parent link="${prefix}base_link"/> - <child link="${prefix}base"/> + <origin xyz="0 0 0" rpy="0 0 ${pi}" /> + <parent link="${prefix}base_link" /> + <child link="${prefix}base" /> </joint> <!-- ROS-Industrial 'flange' frame: attachment point for EEF models --> @@ -325,12 +352,20 @@ </joint> <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame --> - <link name="${prefix}tool0"/> + <link name="${prefix}tool0" /> <joint name="${prefix}flange-tool0" type="fixed"> <!-- default toolframe: X+ left, Y+ up, Z+ front --> - <origin xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}"/> - <parent link="${prefix}flange"/> - <child link="${prefix}tool0"/> + <origin xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}" /> + <parent link="${prefix}flange" /> + <child link="${prefix}tool0" /> </joint> + + <!-- Fixed World link for the arm --> + <link name="world" /> + <joint name="world_joint" type="fixed"> + <parent link="world" /> + <child link="base_link" /> + </joint> + </xacro:macro> -</robot> +</robot> \ No newline at end of file diff --git a/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml index 8f2e90d..c157a1b 100644 --- a/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur10_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml index 890f639..97c548b 100644 --- a/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur10e_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml index 2e29dc4..3a4337e 100644 --- a/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur16e_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml index 3d6fbb8..66f7117 100644 --- a/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur20_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml index 2dbc466..986c8ff 100644 --- a/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur30_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml index 00445c4..2ef24e3 100644 --- a/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur3_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml index e960f37..d33ac12 100644 --- a/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur3e_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml index 238fcb2..effe23c 100644 --- a/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur5_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: -0.3 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 diff --git a/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml index 4db0c62..1f7af84 100644 --- a/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml +++ b/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml @@ -1,8 +1,8 @@ # Default initial positions for ur5e_robot's ros2_control fake system initial_positions: - elbow_joint: 0 - shoulder_lift_joint: 0 + elbow_joint: 1.7 + shoulder_lift_joint: -2.4 shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0