From ea43cfffca6035cf1f01fa882cb691523249b89f Mon Sep 17 00:00:00 2001 From: Matthew Tesch Date: Fri, 31 Jul 2026 15:22:07 -0400 Subject: [PATCH] Use proposed goal functions in updated examples --- kits/arms/ar_control_sm.cpp | 9 ++--- kits/arms/double_arm_teach_repeat_state.hpp | 43 +++++---------------- kits/arms/ex_AR_kit.cpp | 6 +-- kits/arms/ex_home_arm.cpp | 2 +- kits/arms/ex_mobile_io_control.cpp | 2 +- kits/arms/ex_teach_repeat.cpp | 20 ++-------- kits/arms/ex_teach_repeat_with_gripper.cpp | 27 +++---------- 7 files changed, 28 insertions(+), 81 deletions(-) diff --git a/kits/arms/ar_control_sm.cpp b/kits/arms/ar_control_sm.cpp index 2a6e4b92..39056cd1 100644 --- a/kits/arms/ar_control_sm.cpp +++ b/kits/arms/ar_control_sm.cpp @@ -48,7 +48,7 @@ void ArmMobileIOControl::send() const { } void ArmMobileIOControl::home(double duration) const { - arm_->setGoal(arm::Goal::createFromPosition(duration, arm_home_)); + arm_->setGoal({duration, arm_home_}); } void ArmMobileIOControl::transition_to(const double t_now, const ArmControlState& new_state) { @@ -99,9 +99,7 @@ arm::Goal ArmMobileIOControl::compute_arm_goal(const ArmMobileIOInputs& arm_inpu if (arm_input.ar_scaling == 0.0) phone_xyz_home_ = arm_input.phone_pos; auto joint_target = arm_->solveIK(last_locked_seed_, arm_xyz_target, arm_rot_target); - arm::Goal goal = arm::Goal::createFromPosition(traj_duration_, joint_target); - - return goal; + return arm::Goal(traj_duration_, joint_target); } void ArmMobileIOControl::update(double t_now, const ArmMobileIOInputs* arm_input) { @@ -165,8 +163,7 @@ void ArmMobileIOControl::update(double t_now, const ArmMobileIOInputs* arm_input locked_ = arm_input->locked; if (!locked_) { - const arm::Goal arm_goal = compute_arm_goal(*arm_input); - arm_->setGoal(arm_goal); + arm_->setGoal(compute_arm_goal(*arm_input)); } else { phone_xyz_home_ = arm_input->phone_pos; diff --git a/kits/arms/double_arm_teach_repeat_state.hpp b/kits/arms/double_arm_teach_repeat_state.hpp index 45138329..d7147377 100644 --- a/kits/arms/double_arm_teach_repeat_state.hpp +++ b/kits/arms/double_arm_teach_repeat_state.hpp @@ -145,35 +145,21 @@ struct DemoState { Eigen::VectorXd times(wp_count + extra_wps); auto makeGoal = [×, wp_count, extra_wps](ArmState& state){ - // Set up required variables - Eigen::MatrixXd target_pos(state.num_modules_, wp_count + extra_wps); - Eigen::MatrixXd target_vels(state.num_modules_, wp_count + extra_wps); - Eigen::MatrixXd target_accels(state.num_modules_, wp_count + extra_wps); - Eigen::MatrixXd aux(1, wp_count + extra_wps); - - // Fill up matrices appropriately - auto t = 0; + auto goal = arm::Goal(state.num_modules_); for (int i = 0; i < wp_count; i++) { const auto& wp = state.waypoints_[i]; - t += wp.time_from_prev; - times[i] = t; - target_pos.col(i) << wp.positions; - target_vels.col(i) << wp.vels; - target_accels.col(i) << wp.accels; - aux(0, i) = gripperEffort(wp.gripper_state); + auto gripper = Eigen::VectorXd::Constant(1, gripperEffort(wp.gripper_state)); + goal.addWaypoint(wp.time_from_prev, wp.positions, wp.vels, wp.accels, gripper); } if (extra_wps != 0) { - times[wp_count] = t + 0.5; const auto& wp = *state.waypoints_.rbegin(); - target_pos.col(wp_count) << wp.positions; - target_vels.col(wp_count) << wp.vels; - target_accels.col(wp_count) << wp.accels; - aux(0, wp_count) = gripperEffort(state.waypoints_[0].gripper_state); + auto gripper = Eigen::VectorXd::Constant(1, gripperEffort(state.waypoints_[0].gripper_state)); + goal.addWaypoint(0.5, wp.positions, wp.vels, wp.accels, gripper); } - return arm::Goal::createFromWaypointsWithAux(times, target_pos, target_vels, target_accels, aux); + return goal; }; l_arm_.setGoal(makeGoal(left_)); @@ -184,20 +170,11 @@ struct DemoState { static void stopArm(arm::Arm& arm, arm::Gripper::State gripper_state) { auto current_pos = arm.lastFeedback().getPosition(); - auto current_vel = arm.lastFeedback().getVelocity(); - - // Set up required variables - Eigen::VectorXd times(1); - times[0] = 1.5; // time to stop - Eigen::MatrixXd target_pos(arm.robotModel().getDoFCount(), 1); - target_pos.col(0) = current_pos; - Eigen::MatrixXd target_vels(arm.robotModel().getDoFCount(), 1); - target_vels.col(0).setConstant(0); - Eigen::MatrixXd target_accels(arm.robotModel().getDoFCount(), 1); - target_accels.col(0).setConstant(0); + + double time_to_stop = 1.5; Eigen::MatrixXd aux(1, 1); - aux(0, 0) = gripperEffort(gripper_state); - arm.setGoal(arm::Goal::createFromWaypointsWithAux(times, target_pos, target_vels, target_accels, aux)); + aux << gripperEffort(gripper_state); + arm.setGoal(arm::Goal(time_to_stop, current_pos, {}, {}, aux)); } void reset() diff --git a/kits/arms/ex_AR_kit.cpp b/kits/arms/ex_AR_kit.cpp index c7ab3225..cfcb5bda 100644 --- a/kits/arms/ex_AR_kit.cpp +++ b/kits/arms/ex_AR_kit.cpp @@ -105,7 +105,7 @@ int main(int argc, char* argv[]) // Command the softstart to home position arm -> update(); - arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); // move to goal over "soft_start_time" seconds + arm -> setGoal({soft_start_time, home_position}); // move to goal over "soft_start_time" seconds arm -> send(); // Get the cartesian position and rotation matrix @ home position @@ -146,7 +146,7 @@ int main(int argc, char* argv[]) // Button B1 - Return to home position if (mobile_io->getButtonDiff(1) == util::MobileIO::ButtonState::ToOn) { ar_mode = false; - arm -> setGoal(arm::Goal::createFromPosition(soft_start_time, home_position)); + arm -> setGoal({soft_start_time, home_position}); } // Button B3 - Start AR Control @@ -191,7 +191,7 @@ int main(int argc, char* argv[]) rot_target); // Create and send new goal to the arm - arm -> setGoal(arm::Goal::createFromPosition(delay_time, target_joints)); + arm -> setGoal({delay_time, target_joints}); } // Send latest commands to the arm diff --git a/kits/arms/ex_home_arm.cpp b/kits/arms/ex_home_arm.cpp index 144c28b0..5f07e0ea 100644 --- a/kits/arms/ex_home_arm.cpp +++ b/kits/arms/ex_home_arm.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) } double home_duration = 5.0; // seconds - arm::Goal goal = arm::Goal::createFromPosition(home_duration, home_position); + arm::Goal goal(home_duration, home_position); arm->update(); arm->setGoal(goal); diff --git a/kits/arms/ex_mobile_io_control.cpp b/kits/arms/ex_mobile_io_control.cpp index 841a447a..b325121d 100644 --- a/kits/arms/ex_mobile_io_control.cpp +++ b/kits/arms/ex_mobile_io_control.cpp @@ -112,7 +112,7 @@ int main(int argc, char* argv[]) for (int button = 1; button <= 3; button++) { if (mobile_io->getButtonDiff(button) == util::MobileIO::ButtonState::ToOn) { - arm->setGoal(arm::Goal::createFromPosition(travel_time, waypoints.at(button-1))); + arm->setGoal({travel_time, waypoints.at(button-1)}); } } diff --git a/kits/arms/ex_teach_repeat.cpp b/kits/arms/ex_teach_repeat.cpp index 3efbcbbe..7221beec 100644 --- a/kits/arms/ex_teach_repeat.cpp +++ b/kits/arms/ex_teach_repeat.cpp @@ -46,22 +46,10 @@ void addWaypoint (State& state, const GroupFeedback& feedback, bool stop) { } arm::Goal playWaypoints (State& state, double wp_time) { - - // Set up required variables - Eigen::VectorXd times(state.waypoints.size()); - Eigen::MatrixXd target_pos(state.num_modules, state.waypoints.size()); - Eigen::MatrixXd target_vels(state.num_modules, state.waypoints.size()); - Eigen::MatrixXd target_accels(state.num_modules, state.waypoints.size()); - - // Fill up matrices appropriately - for (int i = 0; i < state.waypoints.size(); i++) - { - times[i] = (i+1) * wp_time; - target_pos.col(i) << state.waypoints[i].positions; - target_vels.col(i) << state.waypoints[i].vels; - target_accels.col(i) << state.waypoints[i].accels; - } - return arm::Goal::createFromWaypoints(times, target_pos, target_vels, target_accels); + arm::Goal goal(state.num_modules); + for (const auto& wp : state.waypoints) + goal.addWaypoint(wp_time, wp.positions, wp.vels, wp.accels); + return goal; } int main(int argc, char* argv[]) diff --git a/kits/arms/ex_teach_repeat_with_gripper.cpp b/kits/arms/ex_teach_repeat_with_gripper.cpp index c91be87c..0c387cbd 100644 --- a/kits/arms/ex_teach_repeat_with_gripper.cpp +++ b/kits/arms/ex_teach_repeat_with_gripper.cpp @@ -77,35 +77,20 @@ arm::Goal playWaypoints (State& state) { extra_wps = 1; } - // Set up required variables - Eigen::VectorXd times(wp_count + extra_wps); - Eigen::MatrixXd target_pos(state.num_modules_, wp_count + extra_wps); - Eigen::MatrixXd target_vels(state.num_modules_, wp_count + extra_wps); - Eigen::MatrixXd target_accels(state.num_modules_, wp_count + extra_wps); - Eigen::MatrixXd aux(1, wp_count + extra_wps); - - // Fill up matrices appropriately - auto t = 0; + arm::Goal goal(state.num_modules_); for (int i = 0; i < wp_count; i++) { const auto& wp = state.waypoints_[i]; - t += wp.time_from_prev; - times[i] = t; - target_pos.col(i) << wp.positions; - target_vels.col(i) << wp.vels; - target_accels.col(i) << wp.accels; - aux(0, i) = arm::Gripper::StateToDouble(wp.gripper_state); + auto aux = Eigen::VectorXd::Constant(1, arm::Gripper::StateToDouble(wp.gripper_state)); + goal.addWaypoint(wp.time_from_prev, wp.positions, wp.vels, wp.accels, aux); } if (extra_wps != 0) { - times[wp_count] = t + 0.5; const auto& wp = *state.waypoints_.rbegin(); - target_pos.col(wp_count) << wp.positions; - target_vels.col(wp_count) << wp.vels; - target_accels.col(wp_count) << wp.accels; - aux(0, wp_count) = arm::Gripper::StateToDouble(state.waypoints_[0].gripper_state); + auto aux = Eigen::VectorXd::Constant(1, arm::Gripper::StateToDouble(state.waypoints_[0].gripper_state)); + goal.addWaypoint(0.5, wp.positions, wp.vels, wp.accels, aux); } - return arm::Goal::createFromWaypointsWithAux(times, target_pos, target_vels, target_accels, aux); + return goal; } enum class DemoMode