Skip to content
Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
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
7 changes: 7 additions & 0 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -67,3 +67,10 @@ repos:
- id: codespell
args:
- --ignore-words=.codespell-ignore

- repo: https://github.com/jjaime2/pre-commit-hooks-ros
rev: v1.0.0
hooks:
- id: prettier-xacro
- id: prettier-launch-xml
- id: prettier-package-xml
13 changes: 9 additions & 4 deletions ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,17 @@
Copyright (c) 2024-2025, Personal Robotics Laboratory
License: BSD 3-Clause. See LICENSE.md file in root directory.
-->

<launch>
<arg name="calibration_file_name" default="auto" description="Name of the calibration file to save" />
<arg name="log_level" default="info" description="Log level to pass to create_action_servers: debug, info, warn" />
<arg name="calibration_file_name" default="auto" description="Name of the calibration file to save"/>
<arg name="log_level" default="info" description="Log level to pass to create_action_servers: debug, info, warn"/>

<node pkg="ada_calibrate_camera" exec="publish_camera_extrinsics" name="publish_camera_extrinsics" respawn="true" args="--ros-args --log-level $(var log_level) --log-level rcl:=INFO --log-level rmw_cyclonedds_cpp:=INFO">
<node
pkg="ada_calibrate_camera"
exec="publish_camera_extrinsics"
name="publish_camera_extrinsics"
respawn="true"
args="--ros-args --log-level $(var log_level) --log-level rcl:=INFO --log-level rmw_cyclonedds_cpp:=INFO"
>
<param from="$(find-pkg-share ada_calibrate_camera)/config/calibs/$(var calibration_file_name).yaml"/>
</node>
</launch>
4 changes: 1 addition & 3 deletions ada_description/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,7 @@
<package format="3">
<name>ada_description</name>
<version>0.0.2</version>
<description>
URDF description for Assistive Dexterous Arm
</description>
<description>URDF description for Assistive Dexterous Arm</description>

<author>Ethan K. Gordon</author>

Expand Down
118 changes: 86 additions & 32 deletions ada_description/urdf/ada.xacro
Original file line number Diff line number Diff line change
@@ -1,51 +1,82 @@
<?xml version="1.0"?>

<robot xmlns:xi="http://www.w3.org/2001/XInclude"
<robot
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
xmlns:xacro="http://www.ros.org/wiki/xacro" name="ada">

xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
xmlns:xacro="http://www.ros.org/wiki/xacro"
name="ada"
>
<xacro:include filename="$(find ada_description)/urdf/forque/forque.xacro"/>
<xacro:include filename="$(find ada_description)/urdf/camera/camera.xacro"/>
<xacro:include filename="$(find ada_description)/urdf/j2n6s200.xacro"/>

<xacro:arg name="use_forque" default="true"/>
<xacro:arg
name="use_forque"
default="true"
/>

<link name="root"/>

<link name="root_tilt"/>
<joint name="robot_tilt" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0" />
<child link="root_tilt" />
<parent link="root" />
<joint
name="robot_tilt"
type="revolute"
>
<origin
xyz="0 0 0"
rpy="0 0 0"
/>
<child link="root_tilt"/>
<parent link="root"/>
<axis xyz="-1 0 0"/>
<!-- effort copied from j2n6s200 joint 2, velocity is max velocity the wheelchair can tilt,
lower and upper limits generously encompass the range of the wheelchair's tilt-->
<limit effort="80" velocity="0.15" lower="${-J_PI}" upper="${J_PI}"/>
<dynamics damping="0.0" friction="0.0"/>
<limit
effort="80"
velocity="0.15"
lower="${-J_PI}"
upper="${J_PI}"
/>
<dynamics
damping="0.0"
friction="0.0"
/>
</joint>

<xacro:property name="robot_root" value="root_tilt" />
<xacro:property
name="robot_root"
value="root_tilt"
/>

<xacro:j2n6s200 base_parent="${robot_root}"/>

<xacro:camera_assembly base_parent="j2n6s200_link_6" base_xyz="0.0165 0 0.0011" base_rpy="${3*J_PI/2} 0 ${3*J_PI/2}"/>
<xacro:camera_assembly
base_parent="j2n6s200_link_6"
base_xyz="0.0165 0 0.0011"
base_rpy="${3*J_PI/2} 0 ${3*J_PI/2}"
/>

<!-- Hard-coded Extrinsics -->
<link name="camera_link" />
<joint name="extrinsics" type="fixed">
<origin xyz="0.0194 -0.01606 0.029014" rpy="0.17234 -1.56127 2.96967" />
<parent link="cameraMount" />
<child link="camera_link" />
<link name="camera_link"/>
<joint
name="extrinsics"
type="fixed"
>
<origin
xyz="0.0194 -0.01606 0.029014"
rpy="0.17234 -1.56127 2.96967"
/>
<parent link="cameraMount"/>
<child link="camera_link"/>
</joint>
<!-- Copy Realsense in case of simulation -->
<!--
Expand All @@ -57,20 +88,43 @@
</joint>
-->

<xacro:forque_assembly_link link_name="FTArmMount" link_mesh="2024_01_18_FTArmMount" mass="0.03783" cog="0.010957 -0.019223 -0.03777">
<inertia ixx="0.000018817" iyy="0.00001119989" iyz="-0.00000120296" izz="0.000026883" ixy="-0.0000003327" ixz="-0.000000092476"/>
<xacro:forque_assembly_link
link_name="FTArmMount"
link_mesh="2024_01_18_FTArmMount"
mass="0.03783"
cog="0.010957 -0.019223 -0.03777"
>
<inertia
ixx="0.000018817"
iyy="0.00001119989"
iyz="-0.00000120296"
izz="0.000026883"
ixy="-0.0000003327"
ixz="-0.000000092476"
/>
</xacro:forque_assembly_link>
<xacro:forque_assembly_joint joint_name="FTArmMount_to_arm" parent="j2n6s200_link_6" child="FTArmMount" joint_origin_xyz="0.0065 -0.011 -0.0075" joint_origin_rpy="${J_PI/2} 0 ${J_PI/2}"/>
<xacro:forque_assembly_joint
joint_name="FTArmMount_to_arm"
parent="j2n6s200_link_6"
child="FTArmMount"
joint_origin_xyz="0.0065 -0.011 -0.0075"
joint_origin_rpy="${J_PI/2} 0 ${J_PI/2}"
/>

<xacro:if value="$(arg use_forque)">
<xacro:forque_assembly base_parent="FTArmMount"/>

<link name="FTSensor" />
<joint name="EE_to_FTSensor" type="fixed">
<child link="FTSensor"/>
<parent link="j2n6s200_end_effector"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.092"/>
<link name="FTSensor"/>
<joint
name="EE_to_FTSensor"
type="fixed"
>
<child link="FTSensor"/>
<parent link="j2n6s200_end_effector"/>
<origin
rpy="0 0 0"
xyz="0.0 0.0 0.092"
/>
</joint>
</xacro:if>

</robot>
Loading