mirror of
https://github.com/SHC-ASTRA/rover-ros2.git
synced 2026-02-11 09:20:40 +00:00
feat: add viz code from Tristan's ik_test and add CAD to URDF
This commit is contained in:
1
.gitignore
vendored
1
.gitignore
vendored
@@ -14,3 +14,4 @@ __pycache__/
|
|||||||
|
|
||||||
#Direnv
|
#Direnv
|
||||||
.direnv/
|
.direnv/
|
||||||
|
.venv
|
||||||
|
|||||||
91
arm_launch.py
Normal file
91
arm_launch.py
Normal file
@@ -0,0 +1,91 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
import os
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
|
||||||
|
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch.conditions import IfCondition
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#Prevent making __pycache__ directories
|
||||||
|
from sys import dont_write_bytecode
|
||||||
|
dont_write_bytecode = True
|
||||||
|
|
||||||
|
def launch_setup(context, *args, **kwargs):
|
||||||
|
# Retrieve the resolved value of the launch argument 'mode'
|
||||||
|
mode = LaunchConfiguration('mode').perform(context)
|
||||||
|
nodes = []
|
||||||
|
|
||||||
|
# Arm
|
||||||
|
# package_dir = get_package_share_directory('arm_pkg')
|
||||||
|
package_dir = "/home/david/repos/rover-ros2/src/arm_pkg" # TODO: copy files to share and point there
|
||||||
|
urdf_file_name = 'arm12.urdf'
|
||||||
|
urdf = os.path.join(package_dir, "urdf", urdf_file_name)
|
||||||
|
# urdf = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/" + urdf_file_name
|
||||||
|
with open(urdf, 'r') as infp:
|
||||||
|
robot_desc = infp.read()
|
||||||
|
|
||||||
|
rviz_config_path = os.path.join(package_dir, 'viz.rviz')
|
||||||
|
|
||||||
|
if mode == 'anchor':
|
||||||
|
nodes.append(
|
||||||
|
Node(
|
||||||
|
package='robot_state_publisher',
|
||||||
|
executable='robot_state_publisher',
|
||||||
|
name='robot_state_publisher',
|
||||||
|
output='both',
|
||||||
|
parameters=[{'robot_description': robot_desc}],
|
||||||
|
arguments=[urdf],
|
||||||
|
on_exit=Shutdown()
|
||||||
|
)
|
||||||
|
)
|
||||||
|
nodes.append(
|
||||||
|
Node(
|
||||||
|
package='arm_pkg',
|
||||||
|
executable='arm', # change as needed
|
||||||
|
name='arm',
|
||||||
|
output='both',
|
||||||
|
parameters=[{'launch_mode': 'anchor'}],
|
||||||
|
on_exit=Shutdown()
|
||||||
|
)
|
||||||
|
)
|
||||||
|
nodes.append(
|
||||||
|
Node(
|
||||||
|
package='rviz2',
|
||||||
|
executable='rviz2',
|
||||||
|
name='rviz2',
|
||||||
|
output='both',
|
||||||
|
arguments=['-d', rviz_config_path],
|
||||||
|
on_exit=Shutdown()
|
||||||
|
)
|
||||||
|
)
|
||||||
|
# nodes.append(
|
||||||
|
# Node(
|
||||||
|
# package='anchor_pkg',
|
||||||
|
# executable='anchor', # change as needed
|
||||||
|
# name='anchor',
|
||||||
|
# output='both',
|
||||||
|
# parameters=[{'launch_mode': 'anchor'}],
|
||||||
|
# on_exit=Shutdown()
|
||||||
|
# )
|
||||||
|
# )
|
||||||
|
else:
|
||||||
|
# If an invalid mode is provided, print an error.
|
||||||
|
print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
|
||||||
|
|
||||||
|
return nodes
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
declare_arg = DeclareLaunchArgument(
|
||||||
|
'mode',
|
||||||
|
default_value='anchor',
|
||||||
|
description='Launch mode: arm, core, bio, anchor, or ptz'
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
declare_arg,
|
||||||
|
OpaqueFunction(function=launch_setup)
|
||||||
|
])
|
||||||
@@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
|
||||||
from launch.substitutions import LaunchConfiguration
|
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
@@ -18,13 +18,18 @@ def launch_setup(context, *args, **kwargs):
|
|||||||
|
|
||||||
if mode == 'anchor':
|
if mode == 'anchor':
|
||||||
# Launch every node and pass "anchor" as the parameter
|
# Launch every node and pass "anchor" as the parameter
|
||||||
|
|
||||||
|
# Arm
|
||||||
|
urdf_file = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/arm12.urdf"
|
||||||
|
robot_description = {'robot_description': open(urdf_file).read()}
|
||||||
nodes.append(
|
nodes.append(
|
||||||
Node(
|
Node(
|
||||||
package='arm_pkg',
|
package='arm_pkg',
|
||||||
executable='arm', # change as needed
|
executable='arm', # change as needed
|
||||||
name='arm',
|
name='arm',
|
||||||
output='both',
|
output='both',
|
||||||
parameters=[{'launch_mode': mode}],
|
parameters=[{'launch_mode': mode},
|
||||||
|
{'robot_description': robot_description}],
|
||||||
on_exit=Shutdown()
|
on_exit=Shutdown()
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -13,6 +13,9 @@ from ros2_interfaces_pkg.msg import ArmManual
|
|||||||
from ros2_interfaces_pkg.msg import ArmIK
|
from ros2_interfaces_pkg.msg import ArmIK
|
||||||
from ros2_interfaces_pkg.msg import SocketFeedback
|
from ros2_interfaces_pkg.msg import SocketFeedback
|
||||||
from ros2_interfaces_pkg.msg import DigitFeedback
|
from ros2_interfaces_pkg.msg import DigitFeedback
|
||||||
|
from sensor_msgs.msg import JointState
|
||||||
|
from tf2_ros import TransformBroadcaster, TransformStamped
|
||||||
|
import math
|
||||||
|
|
||||||
|
|
||||||
# IK-Related imports
|
# IK-Related imports
|
||||||
@@ -20,10 +23,6 @@ import numpy as np
|
|||||||
import time, math, os
|
import time, math, os
|
||||||
from math import sin, cos, pi
|
from math import sin, cos, pi
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
# from ikpy.chain import Chain
|
|
||||||
# from ikpy.link import OriginLink, URDFLink
|
|
||||||
# #import pygame as pyg
|
|
||||||
# from scipy.spatial.transform import Rotation as R
|
|
||||||
|
|
||||||
from . import astra_arm
|
from . import astra_arm
|
||||||
|
|
||||||
@@ -64,6 +63,19 @@ class SerialRelay(Node):
|
|||||||
|
|
||||||
self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
|
self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
|
||||||
|
|
||||||
|
# New messages
|
||||||
|
self.joint_state_pub = self.create_publisher(JointState, 'joint_states', 10)
|
||||||
|
self.tf_broadcaster = TransformBroadcaster(self)
|
||||||
|
self.joint_state = JointState()
|
||||||
|
self.joint_state.name = ['Axis_0', 'Axis_1_Joint', 'Axis_2_Joint', 'Axis_3_Joint', 'Continuous_Joint', 'Wrist_Joint']
|
||||||
|
self.joint_state.position = [0.0] * 6 # Initialize with zeros
|
||||||
|
self.odom_trans = TransformStamped()
|
||||||
|
self.odom_trans.header.frame_id = 'odom'
|
||||||
|
self.odom_trans.child_frame_id = 'base_footprint'
|
||||||
|
self.odom_trans.transform.translation.x = 0.0
|
||||||
|
self.odom_trans.transform.translation.y = 0.0
|
||||||
|
self.odom_trans.transform.translation.z = 0.0
|
||||||
|
|
||||||
# Topics used in anchor mode
|
# Topics used in anchor mode
|
||||||
if self.launch_mode == 'anchor':
|
if self.launch_mode == 'anchor':
|
||||||
self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
|
self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
|
||||||
@@ -302,7 +314,6 @@ class SerialRelay(Node):
|
|||||||
angles_in = parts[3:7]
|
angles_in = parts[3:7]
|
||||||
# Convert the angles to floats divide by 10.0
|
# Convert the angles to floats divide by 10.0
|
||||||
angles = [float(angle) / 10.0 for angle in angles_in]
|
angles = [float(angle) / 10.0 for angle in angles_in]
|
||||||
# angles[0] = 0.0 #override axis0 to zero
|
|
||||||
#
|
#
|
||||||
#
|
#
|
||||||
#THIS NEEDS TO BE REMOVED LATER
|
#THIS NEEDS TO BE REMOVED LATER
|
||||||
@@ -319,11 +330,19 @@ class SerialRelay(Node):
|
|||||||
self.arm_feedback.axis1_angle = angles[1]
|
self.arm_feedback.axis1_angle = angles[1]
|
||||||
self.arm_feedback.axis2_angle = angles[2]
|
self.arm_feedback.axis2_angle = angles[2]
|
||||||
self.arm_feedback.axis3_angle = angles[3]
|
self.arm_feedback.axis3_angle = angles[3]
|
||||||
# self.get_logger().info(f"Angles: {angles}")
|
|
||||||
# #debug publish angles
|
# Joint state publisher for URDF visualization
|
||||||
# tempMsg = String()
|
self.joint_state.position[0] = math.radians(angles[0]) # Axis 0
|
||||||
# tempMsg.data = "Angles: " + str(angles)
|
self.joint_state.position[1] = math.radians(angles[1]) # Axis 1
|
||||||
# #self.debug_pub.publish(tempMsg)
|
self.joint_state.position[2] = math.radians(angles[2]) # Axis 2
|
||||||
|
self.joint_state.position[3] = math.radians(angles[3]) # Axis 3
|
||||||
|
self.joint_state.position[4] = math.radians(angles[4]) # Wrist roll
|
||||||
|
self.joint_state.position[5] = math.radians(angles[5]) # Wrist yaw
|
||||||
|
self.joint_state.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
self.joint_state_pub.publish(self.joint_state)
|
||||||
|
|
||||||
|
self.odom_trans.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
self.tf_broadcaster.sendTransform(self.odom_trans)
|
||||||
else:
|
else:
|
||||||
self.get_logger().info("Invalid angle feedback input format")
|
self.get_logger().info("Invalid angle feedback input format")
|
||||||
|
|
||||||
|
|||||||
@@ -2,3 +2,5 @@
|
|||||||
script_dir=$base/lib/arm_pkg
|
script_dir=$base/lib/arm_pkg
|
||||||
[install]
|
[install]
|
||||||
install_scripts=$base/lib/arm_pkg
|
install_scripts=$base/lib/arm_pkg
|
||||||
|
[build_scripts]
|
||||||
|
executable= /usr/bin/env python3
|
||||||
|
|||||||
BIN
src/arm_pkg/urdf/Arm Base Plate.STL
Executable file
BIN
src/arm_pkg/urdf/Arm Base Plate.STL
Executable file
Binary file not shown.
BIN
src/arm_pkg/urdf/Axis 0 Plate.STL
Executable file
BIN
src/arm_pkg/urdf/Axis 0 Plate.STL
Executable file
Binary file not shown.
BIN
src/arm_pkg/urdf/Axis 1.STL
Executable file
BIN
src/arm_pkg/urdf/Axis 1.STL
Executable file
Binary file not shown.
BIN
src/arm_pkg/urdf/Segment 1.STL
Executable file
BIN
src/arm_pkg/urdf/Segment 1.STL
Executable file
Binary file not shown.
@@ -1,22 +1,26 @@
|
|||||||
<robot name="robot">
|
<robot name="robot">
|
||||||
|
|
||||||
<link name="base_footprint"></link>
|
<link name="base_footprint"></link>
|
||||||
|
|
||||||
<joint name="base_joint" type="fixed">
|
<joint name="base_joint" type="fixed">
|
||||||
<parent link="base_footprint" />
|
<parent link="base_footprint" />
|
||||||
<child link="base_link" />
|
<child link="base_link" />
|
||||||
<origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
|
<origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="base_link">
|
<link name="base_link">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="-0.175 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<box size="0.5 0.5 0.01" />
|
<!-- <box size="0.5 0.5 0.01" /> -->
|
||||||
|
<mesh filename="package://arm_pkg/urdf/Arm Base Plate.STL" scale="1 1 1"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="base_link-material">
|
<material name="base_link-material">
|
||||||
<color rgba="0 0.6038273388475408 1 1" />
|
<color rgba="0 0.6038273388475408 1 1" />
|
||||||
</material>
|
</material>
|
||||||
</visual>
|
</visual>
|
||||||
<collision>
|
<collision>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="-0.175 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<box size="0.5 0.5 0.01" />
|
<box size="0.5 0.5 0.01" />
|
||||||
</geometry>
|
</geometry>
|
||||||
@@ -27,24 +31,28 @@
|
|||||||
<inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
|
<inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
<joint name="Axis_0_Joint" type="revolute">
|
|
||||||
|
<joint name="Axis_0" type="revolute">
|
||||||
<parent link="base_link" />
|
<parent link="base_link" />
|
||||||
<child link="Axis_0" />
|
<child link="Axis_0_Plate" />
|
||||||
<origin xyz="0 0 0.035" rpy="0 0 0" />
|
<origin xyz="0 0 0.035" rpy="0 0 0" />
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
<limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/> </joint>
|
<limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
|
||||||
<link name="Axis_0">
|
</joint>
|
||||||
|
|
||||||
|
<link name="Axis_0_Plate">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="-0.175 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<cylinder radius="0.15" length="0.059" />
|
<!-- <cylinder radius="0.15" length="0.059" /> -->
|
||||||
|
<mesh filename="package://arm_pkg/urdf/Axis 0 Plate.STL" scale="1 1 1"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="Axis_0-material">
|
<material name="Axis_0-material">
|
||||||
<color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
|
<color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
|
||||||
</material>
|
</material>
|
||||||
</visual>
|
</visual>
|
||||||
<collision>
|
<collision>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="-0.175 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<cylinder radius="0.15" length="0.059" />
|
<cylinder radius="0.15" length="0.059" />
|
||||||
</geometry>
|
</geometry>
|
||||||
@@ -55,17 +63,21 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_1_Joint" type="revolute">
|
<joint name="Axis_1_Joint" type="revolute">
|
||||||
<parent link="Axis_0" />
|
<parent link="Axis_0_Plate" />
|
||||||
<child link="Axis_1" />
|
<child link="Axis_1" />
|
||||||
<origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
|
<origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
<limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/> </joint>
|
<limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
<link name="Axis_1">
|
<link name="Axis_1">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 -0.11 0.005" rpy="-1.6 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<cylinder radius="0.082" length="0.1" />
|
<!-- <cylinder radius="0.082" length="0.1" /> -->
|
||||||
|
<mesh filename="package://arm_pkg/urdf/Axis 1.STL" scale="1 1 1"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="Axis_1-material">
|
<material name="Axis_1-material">
|
||||||
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
|
<color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
|
||||||
@@ -83,19 +95,22 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_1_to_Segment_1" type="fixed">
|
<joint name="Axis_1_to_Segment_1" type="fixed">
|
||||||
<parent link="Axis_1" />
|
<parent link="Axis_1" />
|
||||||
<child link="Segment_1" />
|
<child link="Segment_1" />
|
||||||
<origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
|
<origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="Segment_1">
|
<link name="Segment_1">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="-0.005 0 -0.35" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<cylinder radius="0.025" length="0.473" />
|
<!-- <cylinder radius="0.025" length="0.473" /> -->
|
||||||
|
<mesh filename="package://arm_pkg/urdf/Segment 1.STL" scale="1 1 1"/>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="Segment_1-material">
|
<material name="Segment_1-material">
|
||||||
<color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
|
<color rgba="0.2 0.2 0.2 1" />
|
||||||
</material>
|
</material>
|
||||||
</visual>
|
</visual>
|
||||||
<collision>
|
<collision>
|
||||||
@@ -110,12 +125,15 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_2_Joint" type="revolute">
|
<joint name="Axis_2_Joint" type="revolute">
|
||||||
<parent link="Segment_1" />
|
<parent link="Segment_1" />
|
||||||
<child link="Axis_2" />
|
<child link="Axis_2" />
|
||||||
<origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
|
<origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
<limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/> </joint>
|
<limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
<link name="Axis_2">
|
<link name="Axis_2">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -138,11 +156,13 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_2_to_Segment_2" type="fixed">
|
<joint name="Axis_2_to_Segment_2" type="fixed">
|
||||||
<parent link="Axis_2" />
|
<parent link="Axis_2" />
|
||||||
<child link="Segment_2" />
|
<child link="Segment_2" />
|
||||||
<origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
|
<origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="Segment_2">
|
<link name="Segment_2">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -165,12 +185,15 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_3_Joint" type="revolute">
|
<joint name="Axis_3_Joint" type="revolute">
|
||||||
<parent link="Segment_2" />
|
<parent link="Segment_2" />
|
||||||
<child link="Axis_3" />
|
<child link="Axis_3" />
|
||||||
<origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
|
<origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
|
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
<link name="Axis_3">
|
<link name="Axis_3">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -193,11 +216,13 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_3_to_Segment_3" type="fixed">
|
<joint name="Axis_3_to_Segment_3" type="fixed">
|
||||||
<parent link="Axis_3" />
|
<parent link="Axis_3" />
|
||||||
<child link="Segment_3" />
|
<child link="Segment_3" />
|
||||||
<origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
|
<origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="Segment_3">
|
<link name="Segment_3">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -220,12 +245,15 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Wrist_Joint" type="revolute">
|
<joint name="Wrist_Joint" type="revolute">
|
||||||
<parent link="Segment_3" />
|
<parent link="Segment_3" />
|
||||||
<child link="Axis_4" />
|
<child link="Axis_4" />
|
||||||
<origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
|
<origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
|
||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/> </joint>
|
<limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
<link name="Axis_4">
|
<link name="Axis_4">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -248,6 +276,7 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Continuous_Joint" type="revolute">
|
<joint name="Continuous_Joint" type="revolute">
|
||||||
<parent link="Axis_4" />
|
<parent link="Axis_4" />
|
||||||
<child link="Axis_4_C" />
|
<child link="Axis_4_C" />
|
||||||
@@ -255,6 +284,7 @@
|
|||||||
<axis xyz="0 0 1"/>
|
<axis xyz="0 0 1"/>
|
||||||
<limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
|
<limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="Axis_4_C">
|
<link name="Axis_4_C">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -277,11 +307,13 @@
|
|||||||
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
<inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="Axis_4_C_to_Effector" type="fixed">
|
<joint name="Axis_4_C_to_Effector" type="fixed">
|
||||||
<parent link="Axis_4_C" />
|
<parent link="Axis_4_C" />
|
||||||
<child link="Effector" />
|
<child link="Effector" />
|
||||||
<origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
|
<origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="Effector">
|
<link name="Effector">
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
@@ -304,4 +336,5 @@
|
|||||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||||
</inertial>
|
</inertial>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
</robot>
|
</robot>
|
||||||
234
src/arm_pkg/viz.rviz
Normal file
234
src/arm_pkg/viz.rviz
Normal file
@@ -0,0 +1,234 @@
|
|||||||
|
Panels:
|
||||||
|
- Class: rviz_common/Displays
|
||||||
|
Help Height: 78
|
||||||
|
Name: Displays
|
||||||
|
Property Tree Widget:
|
||||||
|
Expanded:
|
||||||
|
- /Global Options1
|
||||||
|
- /Status1
|
||||||
|
- /TF1
|
||||||
|
- /RobotModel1
|
||||||
|
- /RobotModel1/Description Topic1
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Tree Height: 717
|
||||||
|
- Class: rviz_common/Selection
|
||||||
|
Name: Selection
|
||||||
|
- Class: rviz_common/Tool Properties
|
||||||
|
Expanded:
|
||||||
|
- /2D Goal Pose1
|
||||||
|
- /Publish Point1
|
||||||
|
Name: Tool Properties
|
||||||
|
Splitter Ratio: 0.5886790156364441
|
||||||
|
- Class: rviz_common/Views
|
||||||
|
Expanded:
|
||||||
|
- /Current View1
|
||||||
|
Name: Views
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Visualization Manager:
|
||||||
|
Class: ""
|
||||||
|
Displays:
|
||||||
|
- Alpha: 0.5
|
||||||
|
Cell Size: 1
|
||||||
|
Class: rviz_default_plugins/Grid
|
||||||
|
Color: 160; 160; 164
|
||||||
|
Enabled: true
|
||||||
|
Line Style:
|
||||||
|
Line Width: 0.029999999329447746
|
||||||
|
Value: Lines
|
||||||
|
Name: Grid
|
||||||
|
Normal Cell Count: 0
|
||||||
|
Offset:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Plane: XY
|
||||||
|
Plane Cell Count: 10
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Value: true
|
||||||
|
- Class: rviz_default_plugins/TF
|
||||||
|
Enabled: true
|
||||||
|
Frame Timeout: 15
|
||||||
|
Frames:
|
||||||
|
All Enabled: true
|
||||||
|
arm_link_0:
|
||||||
|
Value: true
|
||||||
|
arm_link_1:
|
||||||
|
Value: true
|
||||||
|
arm_link_2:
|
||||||
|
Value: true
|
||||||
|
arm_link_3:
|
||||||
|
Value: true
|
||||||
|
arm_link_4:
|
||||||
|
Value: true
|
||||||
|
arm_link_5:
|
||||||
|
Value: true
|
||||||
|
arm_link_6:
|
||||||
|
Value: true
|
||||||
|
base_link:
|
||||||
|
Value: true
|
||||||
|
odom:
|
||||||
|
Value: true
|
||||||
|
Marker Scale: 0.44999998807907104
|
||||||
|
Name: TF
|
||||||
|
Show Arrows: true
|
||||||
|
Show Axes: true
|
||||||
|
Show Names: false
|
||||||
|
Tree:
|
||||||
|
odom:
|
||||||
|
base_link:
|
||||||
|
arm_link_0:
|
||||||
|
arm_link_1:
|
||||||
|
arm_link_2:
|
||||||
|
arm_link_3:
|
||||||
|
arm_link_4:
|
||||||
|
arm_link_5:
|
||||||
|
arm_link_6:
|
||||||
|
{}
|
||||||
|
Update Interval: 0
|
||||||
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Class: rviz_default_plugins/RobotModel
|
||||||
|
Collision Enabled: false
|
||||||
|
Description File: ""
|
||||||
|
Description Source: Topic
|
||||||
|
Description Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /robot_description
|
||||||
|
Enabled: true
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
arm_link_0:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
arm_link_1:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
arm_link_2:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
arm_link_3:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
arm_link_4:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
arm_link_5:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
arm_link_6:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Mass Properties:
|
||||||
|
Inertia: false
|
||||||
|
Mass: false
|
||||||
|
Name: RobotModel
|
||||||
|
TF Prefix: ""
|
||||||
|
Update Interval: 0
|
||||||
|
Value: true
|
||||||
|
Visual Enabled: true
|
||||||
|
Enabled: true
|
||||||
|
Global Options:
|
||||||
|
Background Color: 48; 48; 48
|
||||||
|
Fixed Frame: odom
|
||||||
|
Frame Rate: 30
|
||||||
|
Name: root
|
||||||
|
Tools:
|
||||||
|
- Class: rviz_default_plugins/Interact
|
||||||
|
Hide Inactive Objects: true
|
||||||
|
- Class: rviz_default_plugins/MoveCamera
|
||||||
|
- Class: rviz_default_plugins/Select
|
||||||
|
- Class: rviz_default_plugins/FocusCamera
|
||||||
|
- Class: rviz_default_plugins/Measure
|
||||||
|
Line color: 128; 128; 0
|
||||||
|
- Class: rviz_default_plugins/SetInitialPose
|
||||||
|
Covariance x: 0.25
|
||||||
|
Covariance y: 0.25
|
||||||
|
Covariance yaw: 0.06853891909122467
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /initialpose
|
||||||
|
- Class: rviz_default_plugins/SetGoal
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /goal_pose
|
||||||
|
- Class: rviz_default_plugins/PublishPoint
|
||||||
|
Single click: true
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /clicked_point
|
||||||
|
Transformation:
|
||||||
|
Current:
|
||||||
|
Class: rviz_default_plugins/TF
|
||||||
|
Value: true
|
||||||
|
Views:
|
||||||
|
Current:
|
||||||
|
Class: rviz_default_plugins/Orbit
|
||||||
|
Distance: 3.1002085208892822
|
||||||
|
Enable Stereo Rendering:
|
||||||
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
|
Stereo Focal Distance: 1
|
||||||
|
Swap Stereo Eyes: false
|
||||||
|
Value: false
|
||||||
|
Focal Point:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Focal Shape Fixed Size: true
|
||||||
|
Focal Shape Size: 0.05000000074505806
|
||||||
|
Invert Z Axis: false
|
||||||
|
Name: Current View
|
||||||
|
Near Clip Distance: 0.009999999776482582
|
||||||
|
Pitch: 0.6997972726821899
|
||||||
|
Target Frame: <Fixed Frame>
|
||||||
|
Value: Orbit (rviz)
|
||||||
|
Yaw: 5.6954216957092285
|
||||||
|
Saved: ~
|
||||||
|
Window Geometry:
|
||||||
|
Displays:
|
||||||
|
collapsed: true
|
||||||
|
Height: 945
|
||||||
|
Hide Left Dock: true
|
||||||
|
Hide Right Dock: true
|
||||||
|
QMainWindow State: 000000ff00000000fd0000000400000000000001630000036afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002c0000036a000000ab00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002effc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c000002ef000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e80000036a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
|
Selection:
|
||||||
|
collapsed: false
|
||||||
|
Tool Properties:
|
||||||
|
collapsed: false
|
||||||
|
Views:
|
||||||
|
collapsed: true
|
||||||
|
Width: 1512
|
||||||
|
X: 0
|
||||||
|
Y: 37
|
||||||
Reference in New Issue
Block a user