Skip to content

Use proposed goal functions in updated examples #141

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
iamtesch wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Use proposed goal functions in updated examples #141

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 3 additions & 6 deletions kits/arms/ar_control_sm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
Expand Down
43 changes: 10 additions & 33 deletions kits/arms/double_arm_teach_repeat_state.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,35 +145,21 @@ struct DemoState {

Eigen::VectorXd times(wp_count + extra_wps);
auto makeGoal = [&times, 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_));
Expand All @@ -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()
Expand Down
6 changes: 3 additions & 3 deletions kits/arms/ex_AR_kit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion kits/arms/ex_home_arm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
2 changes: 1 addition & 1 deletion kits/arms/ex_mobile_io_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)});
}
}

Expand Down
20 changes: 4 additions & 16 deletions kits/arms/ex_teach_repeat.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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[])
Expand Down
27 changes: 6 additions & 21 deletions kits/arms/ex_teach_repeat_with_gripper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down