Skip to content
Merged
Show file tree
Hide file tree
Changes from all 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
24 changes: 23 additions & 1 deletion src/control_mode/abstract_control_mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -119,11 +119,28 @@ bool AbstractControlMode::moveToJointPosition(const std::array<double, NUM_DOFS>
// {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
// {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
// {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
// a motion that finishes in its first cycle is rejected by libfranka ("still moving")
const franka::RobotState state = robot_->readOnce();
state_buffer_->write(state);
double max_error = 0.0;
for (size_t i = 0; i < NUM_DOFS; i++)
{
max_error = std::max(max_error, std::abs(target_q[i] - state.q[i]));
}
if (max_error < tolerance)
{
zlc::info("[{}] Already at target joint position.", getModeName());
startControl();
return true;
}
bool reached = false;
for (size_t i = 0; i < 5; i++)
{
try {
JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_, tolerance);
robot_->control(motion_generator);
reached = true;
break;
}catch (const franka::Exception& e) {
zlc::error("Error when move joint position {}", e.what());
}
Expand All @@ -134,8 +151,13 @@ bool AbstractControlMode::moveToJointPosition(const std::array<double, NUM_DOFS>
break;
}
}
zlc::info("[{}] Reached target joint position.", getModeName());
startControl();
if (!reached)
{
zlc::error("[{}] Move to joint position failed, target not reached.", getModeName());
return false;
}
zlc::info("[{}] Reached target joint position.", getModeName());
return true;
}

Expand Down
5 changes: 3 additions & 2 deletions src/motion_generator/joint_position_motion_generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,13 @@ franka::JointPositions JointPositionMotionGenerator::operator()(const franka::Ro
time_ += period.toSec();

if (time_ == 0.0) {
q_start_ = JointPositionMotionGenerator::Vector7d(robot_state.q.data());
// q_d, not q: starting from q makes the first command jump by q - q_d (discontinuity)
q_start_ = JointPositionMotionGenerator::Vector7d(robot_state.q_d.data());
delta_q_ = q_goal_ - q_start_;

// Check if already within tolerance - skip motion if so
if (delta_q_.cwiseAbs().maxCoeff() < tolerance_) {
franka::JointPositions output(robot_state.q);
franka::JointPositions output(robot_state.q_d);
output.motion_finished = true;
return output;
}
Expand Down
Loading