inholland/ros2-homework
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Work on inverse kinematics

Browse Source
This commit is contained in:
Wessel T
2025-12-04 09:42:19 +01:00
parent df2e5808f5
commit 6a6a9c9dcf
5 changed files with 88 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
src/wt-assign1-2025-4-parol6-static-pkg/meshes/game.stl Normal file
View File

Binary file not shown.

24
src/wt-assign1-2025-4-parol6-static-pkg/urdf/game_board.urdf Normal file
View File

@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<robot name="game_board">
<joint name="game_board_joint" type="fixed">
<parent link="world"/>
<child link="game_board"/>
<origin xyz="0.25 0.0 0.0" rpy="1.5708 0 1.5708"/>
</joint>
<link name="game_board">
<visual>
<geometry>
<mesh filename="package://wt-assign1-2025-4-parol6-static-pkg/meshes/game.stl" scale="0.0001 0.0001 0.0001"/>
</geometry>
<material name="board_color">
<color rgba="0.1 0.3 0.8 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://wt-assign1-2025-4-parol6-static-pkg/meshes/game.stl" scale="0.0001 0.0001 0.0001"/>
</geometry>
</collision>
</link>
</robot>

5
src/wt-assign1-2025-4-parol6-static-pkg/urdf/robot_with_game.urdf.xacro Normal file
View File

@@ -0,0 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="parol6_with_game">
<xacro:include filename="$(find wt-assign1-2025-4-parol6-static-pkg)/urdf/PAROL6.urdf.xacro"/>
<xacro:include filename="$(find wt-assign1-2025-4-parol6-static-pkg)/urdf/game_board.urdf"/>
</robot>

26
src/wt-assign3-2025-5-joint-state-publisher-pkg/launch/inverse_kinematic.launch.xml
View File

@@ -1,23 +1,27 @@
<launch>
<let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/urdf/PAROL6.urdf.xacro"/>
<let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/urdf/robot_with_game.urdf.xacro"/>
<let name="rviz_config_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/rviz/urdf_config.rviz"/>
<!-- Declare case number argument with default value -->
<arg name="L1" default="0.18" description="Length of first link"/>
<arg name="L2" default="0.0435" description="Length of second link"/>
<arg name="target_x" default="0.0" description="Target x coordinate"/>
<arg name="target_y" default="0.0" description="Target y coordinate"/>
<arg name="case_number" default="2" description="Case number for inverse kinematics test"/>
<!-- Connect 4 cell position parameters -->
<arg name="cell_column" default="-1" description="Connect 4 column (0-6, -1 for auto-cycle)"/>
<arg name="cell_row" default="-1" description="Connect 4 row (0-5, -1 for auto-cycle)"/>
<arg name="board_x" default="0.30" description="Board center X position"/>
<arg name="board_y" default="0.0" description="Board center Y position"/>
<arg name="board_z" default="0.30" description="Height above cells"/>
<arg name="cell_spacing" default="0.04" description="Distance between cell centers"/>
<arg name="case_number" default="0" description="Case number for inverse kinematics test"/>
<node pkg="robot_state_publisher" exec="robot_state_publisher">
<param name="robot_description" value="$(command 'xacro $(var urdf_path)')" />
</node>
<node pkg="wt-assign3-2025-5-joint-state-publisher-pkg" exec="ik_node">
<param name="L1" value="$(var L1)"/>
<param name="L2" value="$(var L2)"/>
<param name="target_x" value="$(var target_x)"/>
<param name="target_y" value="$(var target_y)"/>
<param name="cell_column" value="$(var cell_column)"/>
<param name="cell_row" value="$(var cell_row)"/>
<param name="board_x" value="$(var board_x)"/>
<param name="board_y" value="$(var board_y)"/>
<param name="board_z" value="$(var board_z)"/>
<param name="cell_spacing" value="$(var cell_spacing)"/>
<param name="case_number" value="$(var case_number)"/>
</node>

55
src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp
View File

@@ -17,9 +17,14 @@ public:
a5_ = this->declare_parameter<double>("a5", 0.17635); // Elbow to wrist
a6_ = this->declare_parameter<double>("a6", 0.06280); // Wrist offset
a7_ = this->declare_parameter<double>("a7", 0.04525); // End effector length
target_x_ = this->declare_parameter<double>("target_x", 0.0);
target_y_ = this->declare_parameter<double>("target_y", 0.0);
target_z_ = this->declare_parameter<double>("target_z", 0.0);
// Connect 4 board parameters
cell_column_ = this->declare_parameter<int>("cell_column", -1); // -1 = auto-cycle
cell_row_ = this->declare_parameter<int>("cell_row", -1); // -1 = auto-cycle
board_x_ = this->declare_parameter<double>("board_x", 0.25);
board_y_ = this->declare_parameter<double>("board_y", 0.0);
board_z_ = this->declare_parameter<double>("board_z", 0.15);
cell_spacing_ = this->declare_parameter<double>("cell_spacing", 0.025);
case_number_ = this->declare_parameter<int>("case_number", 0);
calculateInverseKinematics(case_number_);
@@ -36,17 +41,37 @@ public:
// std::bind(&JointPublisherNode::parametersCallback, this, std::placeholders::_1)
// );
double angle = 0.0;
int current_col = 0;
int current_row = 0;
bool auto_cycle = (cell_column_ == -1 || cell_row_ == -1);
while(true) {
angle += 0.01;
// Vary radius from 0.1 to 0.35 to cover full range (close to far)
double radius = 0.1 + 0.25 * (0.5 + 0.5 * std::sin(angle * 0.3));
target_x_ = radius * std::cos(angle);
target_y_ = radius * std::sin(angle);
target_z_ = 0.15 + 0.1 * std::sin(angle * 2);
// Use specific cell or auto-cycle through all cells
int col = auto_cycle ? current_col : cell_column_;
int row = auto_cycle ? current_row : cell_row_;
// Clamp values to valid range
col = std::max(0, std::min(6, col));
row = std::max(0, std::min(5, row));
// Calculate position for current cell (ready to drop coin)
// Board rotated 90 degrees: columns are along Y axis, rows along Z axis
// Position above top of column for coin drop
target_x_ = board_x_; // Fixed distance from robot
target_y_ = board_y_ + (col - 3) * cell_spacing_; // Column position (left to right)
target_z_ = board_z_ + 0.1; // Above the board to drop coins
calculateInverseKinematics(case_number_);
broadcastJointState();
std::this_thread::sleep_for(std::chrono::milliseconds(10));
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// Move to next column only in auto-cycle mode (cycle through columns, not all cells)
if (auto_cycle) {
current_col++;
if (current_col >= 7) {
current_col = 0;
}
}
}
RCLCPP_INFO(this->get_logger(),
@@ -80,6 +105,14 @@ private:
double a5_; // Elbow to wrist (176.35mm)
double a6_; // Wrist offset (62.8mm)
double a7_; // End effector length (45.25mm)
// Connect 4 board parameters
int cell_column_;
int cell_row_;
double board_x_;
double board_y_;
double board_z_;
double cell_spacing_;
int case_number_;
double theta_1_;