Skip to content

Commit

Permalink
update ruckig dep
Browse files Browse the repository at this point in the history
  • Loading branch information
Lars Berscheid committed Mar 9, 2021
1 parent 7ce81f7 commit cc81e52
Show file tree
Hide file tree
Showing 8 changed files with 151 additions and 100 deletions.
15 changes: 7 additions & 8 deletions include/frankx/motion_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
#include <franka/duration.h>
#include <franka/robot_state.h>

#include <ruckig/parameter.hpp>
#include <ruckig/input_parameter.hpp>
#include <movex/robot/motion_data.hpp>
#include <movex/robot/robot_state.hpp>
#include <movex/waypoint.hpp>
Expand Down Expand Up @@ -70,8 +70,7 @@ struct MotionGenerator {

template<class RobotType>
static std::tuple<std::array<double, 7>, std::array<double, 7>, std::array<double, 7>> getInputLimits(RobotType* robot, const Waypoint& waypoint, const MotionData& data) {
constexpr double translation_factor {0.5};
constexpr double elbow_factor {0.36};
constexpr double translation_factor {0.4};
constexpr double derivative_factor {0.4};

if (waypoint.max_dynamics || data.max_dynamics) {
Expand All @@ -96,17 +95,17 @@ struct MotionGenerator {
auto max_velocity = MotionGenerator::VectorCartRotElbow(
translation_factor * waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_translation_velocity,
waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_rotation_velocity,
elbow_factor * waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_elbow_velocity
waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_elbow_velocity
);
auto max_acceleration = MotionGenerator::VectorCartRotElbow(
translation_factor * derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_translation_acceleration,
derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_rotation_acceleration,
elbow_factor * derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_elbow_acceleration
derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_elbow_acceleration
);
auto max_jerk = MotionGenerator::VectorCartRotElbow(
translation_factor * std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_translation_jerk,
std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_rotation_jerk,
elbow_factor * std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_elbow_jerk
std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_elbow_jerk
);
return {max_velocity, max_acceleration, max_jerk};
}
Expand All @@ -125,9 +124,9 @@ struct MotionGenerator {
}

if (waypoint.max_dynamics) {
input_parameters.synchronization = ruckig::InputParameter<7>::Synchronization::TimeIfNecessary;
input_parameters.synchronization = ruckig::Synchronization::TimeIfNecessary;
} else {
input_parameters.synchronization = ruckig::InputParameter<7>::Synchronization::TimeAlways;
input_parameters.synchronization = ruckig::Synchronization::Time;
}
}
};
Expand Down
2 changes: 1 addition & 1 deletion include/frankx/motion_waypoint_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ struct WaypointMotionGenerator: public MotionGenerator {
WaypointMotion& motion;
MotionData& data;

const size_t cooldown_iterations {4};
const size_t cooldown_iterations {5};
size_t current_cooldown_iteration {0};

explicit WaypointMotionGenerator(RobotType* robot, const Affine& frame, WaypointMotion& motion, MotionData& data): robot(robot), frame(frame), motion(motion), current_motion(motion), data(data) { }
Expand Down
182 changes: 100 additions & 82 deletions notebooks/kinematics.nb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 158, 7]
NotebookDataLength[ 22428, 572]
NotebookOptionsPosition[ 21182, 544]
NotebookOutlinePosition[ 21518, 559]
CellTagsIndexPosition[ 21475, 556]
NotebookDataLength[ 21966, 590]
NotebookOptionsPosition[ 20530, 560]
NotebookOutlinePosition[ 20867, 575]
CellTagsIndexPosition[ 20824, 572]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Expand Down Expand Up @@ -67,7 +67,7 @@ Cell[BoxData[
RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}],
"\[IndentingNewLine]", "}"}]}]], "Input",
CellChangeTimes->{{3.823582093563463*^9, 3.823582186507723*^9}},
CellLabel->"In[2]:=",ExpressionUUID->"1293ae6c-31e5-4370-8907-caad95c252d0"],
CellLabel->"In[1]:=",ExpressionUUID->"1293ae6c-31e5-4370-8907-caad95c252d0"],

Cell[BoxData[
RowBox[{
Expand Down Expand Up @@ -112,7 +112,7 @@ Cell[BoxData[
CellChangeTimes->{{3.823582198644116*^9, 3.823582337516674*^9}, {
3.823582386285802*^9, 3.823582419461158*^9}, {3.823582453054434*^9,
3.82358249544624*^9}},
CellLabel->"In[3]:=",ExpressionUUID->"d8b24c1a-4cdd-43ed-9ee4-98459c265c06"],
CellLabel->"In[2]:=",ExpressionUUID->"d8b24c1a-4cdd-43ed-9ee4-98459c265c06"],

Cell[BoxData[
RowBox[{
Expand All @@ -129,7 +129,7 @@ Cell[BoxData[
RowBox[{"1.", "\[Rule]", "1"}]}], "}"}]}], "]"}]}], ";"}]], "Input",
CellChangeTimes->{{3.823582623495618*^9, 3.823582655774441*^9}, {
3.823582693975222*^9, 3.823582697862906*^9}},
CellLabel->"In[4]:=",ExpressionUUID->"5eced034-3e4c-4a28-956f-542ef9fac943"],
CellLabel->"In[3]:=",ExpressionUUID->"5eced034-3e4c-4a28-956f-542ef9fac943"],

Cell[BoxData[
RowBox[{
Expand Down Expand Up @@ -205,7 +205,7 @@ Cell[BoxData[
RowBox[{"3", ",", "3"}], "]"}], "]"}]}], "]"}]}],
"\[IndentingNewLine]", "}"}]}]], "Input",
CellChangeTimes->{{3.823594209828718*^9, 3.823594305244712*^9}},
CellLabel->"In[5]:=",ExpressionUUID->"25bca131-2606-4f40-956f-9b4b520b9c99"],
CellLabel->"In[4]:=",ExpressionUUID->"25bca131-2606-4f40-956f-9b4b520b9c99"],

Cell[BoxData[
RowBox[{
Expand All @@ -226,25 +226,27 @@ Cell[BoxData[
Cell[CellGroupData[{

Cell[BoxData[
RowBox[{"{",
RowBox[{
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]1"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]2"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]3"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]4"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]5"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]6"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]7"}], "]"}]}], "}"}]], "Input",
RowBox[{
RowBox[{"{",
RowBox[{
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]1"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]2"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]3"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]4"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]5"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]6"}], "]"}], ",",
RowBox[{"D", "[",
RowBox[{"eM", ",", "\[Theta]7"}], "]"}]}], "}"}], ";"}]], "Input",
CellChangeTimes->{{3.8235956031590548`*^9, 3.8235957116785507`*^9}, {
3.823602846632757*^9, 3.8236028864799213`*^9}},
CellLabel->"In[16]:=",ExpressionUUID->"9db25a01-77cb-4265-b9c7-eac93a89fd5b"],
3.823602846632757*^9, 3.8236028864799213`*^9},
3.823751768880197*^9},ExpressionUUID->"9db25a01-77cb-4265-b9c7-\
eac93a89fd5b"],

Cell[BoxData[
InterpretationBox[
Expand Down Expand Up @@ -311,7 +313,8 @@ Cell[BoxData[
StyleBox[
DynamicBox[ToBoxes[
FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"],
StandardForm]],
StandardForm],
ImageSizeCache->{54., {0., 8.}}],
StripOnInput->False,
DynamicUpdating->True,
LineSpacing->{1, 2},
Expand Down Expand Up @@ -385,7 +388,8 @@ Cell[BoxData[
StyleBox[
DynamicBox[ToBoxes[
FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"],
StandardForm]],
StandardForm],
ImageSizeCache->{46., {0., 8.}}],
StripOnInput->False,
DynamicUpdating->True,
LineSpacing->{1, 2},
Expand Down Expand Up @@ -489,61 +493,73 @@ Cell[BoxData[{
RowBox[{"eM", "[",
RowBox[{"[", "6", "]"}], "]"}], ",", "\[Theta]7"}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{"ToString", "[",
RowBox[{"tmp", ",", "CForm"}], "]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"ToString", "[",
RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]",
RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input",
CellChangeTimes->{{3.823594395534802*^9, 3.823594412717284*^9}, {
3.823594895697111*^9, 3.823594908600604*^9}, {3.823594963617971*^9,
3.823595138170384*^9}, {3.823595177571471*^9, 3.8235953003878927`*^9}},
CellLabel->
"In[234]:=",ExpressionUUID->"f934a522-8b1f-4ba2-941b-f8a1252d3712"],
3.823594895697111*^9, 3.823594908600604*^9}, {3.823594963617971*^9,
3.823595138170384*^9}, {3.823595177571471*^9, 3.8235953003878927`*^9},
3.8237517626082907`*^9},
CellLabel->"In[23]:=",ExpressionUUID->"f934a522-8b1f-4ba2-941b-f8a1252d3712"],

Cell[BoxData["\<\"(Cos(\[Theta]6)*Cos(\[Theta]7)*Sin(\[Theta]2)*Sin(\[Theta]3)\
*Sin(\[Theta]5) + Cos(\[Theta]2)*Cos(\[Theta]7)*Sin(\[Theta]4)*Sin(\[Theta]5) \
+ Cos(\[Theta]2)*Cos(\[Theta]4)*Cos(\[Theta]7)*Sin(\[Theta]6) - \
Cos(\[Theta]6)*Sin(\[Theta]2)*Sin(\[Theta]3)*Sin(\[Theta]5)*Sin(\[Theta]7) + \
Cos(\[Theta]2)*Sin(\[Theta]4)*Sin(\[Theta]5)*Sin(\[Theta]7) - \
Cos(\[Theta]2)*Cos(\[Theta]4)*Sin(\[Theta]6)*Sin(\[Theta]7) + \
Cos(\[Theta]5)*(-(Cos(\[Theta]7)*Sin(\[Theta]2)*Sin(\[Theta]3)) + \
Cos(\[Theta]2)*Cos(\[Theta]6)*Cos(\[Theta]7)*Sin(\[Theta]4) - \
Sin(\[Theta]2)*Sin(\[Theta]3)*Sin(\[Theta]7) - \
Cos(\[Theta]2)*Cos(\[Theta]6)*Sin(\[Theta]4)*Sin(\[Theta]7)) + \
Cos(\[Theta]3)*Sin(\[Theta]2)*(Sin(\[Theta]4)*Sin(\[Theta]6)*(Cos(\[Theta]7) \
- Sin(\[Theta]7)) - Cos(\[Theta]4)*(Cos(\[Theta]5)*Cos(\[Theta]6)*(Cos(\
\[Theta]7) - Sin(\[Theta]7)) + Sin(\[Theta]5)*(Cos(\[Theta]7) + \
Sin(\[Theta]7)))))/(Sqrt(2)*(Cos(\[Theta]2)*(Cos(\[Theta]4)*Cos(\[Theta]6) - \
Cos(\[Theta]5)*Sin(\[Theta]4)*Sin(\[Theta]6)) + \
Sin(\[Theta]2)*(-(Sin(\[Theta]3)*Sin(\[Theta]5)*Sin(\[Theta]6)) + \
Cos(\[Theta]3)*(Cos(\[Theta]6)*Sin(\[Theta]4) + \
Cos(\[Theta]4)*Cos(\[Theta]5)*Sin(\[Theta]6))))*(1 + \
Power(Cos(\[Theta]6)*Cos(\[Theta]7)*Sin(\[Theta]2)*Sin(\[Theta]3)*Sin(\[Theta]\
5) - Cos(\[Theta]2)*Cos(\[Theta]7)*Sin(\[Theta]4)*Sin(\[Theta]5) + Cos(\
\[Theta]2)*Cos(\[Theta]4)*Cos(\[Theta]7)*Sin(\[Theta]6) + Cos(\[Theta]6)*Sin(\
\[Theta]2)*Sin(\[Theta]3)*Sin(\[Theta]5)*Sin(\[Theta]7) + Cos(\[Theta]2)*Sin(\
\[Theta]4)*Sin(\[Theta]5)*Sin(\[Theta]7) + Cos(\[Theta]2)*Cos(\[Theta]4)*Sin(\
\[Theta]6)*Sin(\[Theta]7) + \
Cos(\[Theta]5)*(Cos(\[Theta]7)*Sin(\[Theta]2)*Sin(\[Theta]3) + \
Cos(\[Theta]2)*Cos(\[Theta]6)*Cos(\[Theta]7)*Sin(\[Theta]4) - \
Sin(\[Theta]2)*Sin(\[Theta]3)*Sin(\[Theta]7) + \
Cos(\[Theta]2)*Cos(\[Theta]6)*Sin(\[Theta]4)*Sin(\[Theta]7)) + \
Cos(\[Theta]3)*Sin(\[Theta]2)*(Sin(\[Theta]4)*Sin(\[Theta]6)*(Cos(\[Theta]7) \
+ Sin(\[Theta]7)) - Cos(\[Theta]4)*(Sin(\[Theta]5)*(-Cos(\[Theta]7) + Sin(\
\[Theta]7)) + Cos(\[Theta]5)*Cos(\[Theta]6)*(Cos(\[Theta]7) + \
Sin(\[Theta]7)))),2)/(2.*Power(Cos(\[Theta]2)*(Cos(\[Theta]4)*Cos(\[Theta]6) \
- Cos(\[Theta]5)*Sin(\[Theta]4)*Sin(\[Theta]6)) + Sin(\[Theta]2)*(-(Sin(\
\[Theta]3)*Sin(\[Theta]5)*Sin(\[Theta]6)) + \
Cos(\[Theta]3)*(Cos(\[Theta]6)*Sin(\[Theta]4) + \
Cos(\[Theta]4)*Cos(\[Theta]5)*Sin(\[Theta]6))),2))))\"\>"], "Output",
CellChangeTimes->{{3.823594397520789*^9, 3.8235944132450027`*^9}, {
3.8235948923741407`*^9, 3.823594908963525*^9}, {3.823594964006518*^9,
3.8235951384849863`*^9}, {3.823595172596004*^9, 3.823595300711917*^9}},
Cell[BoxData[
TemplateBox[{
"Part","partd",
"\"Part specification \\!\\(\\*RowBox[{\\\"eM\\\", \
\\\"\[LeftDoubleBracket]\\\", \\\"6\\\", \\\"\[RightDoubleBracket]\\\"}]\\) \
is longer than depth of object.\"",2,23,1,29561611951775162962,"Local"},
"MessageTemplate"]], "Message", "MSG",
CellChangeTimes->{3.8237517631418056`*^9},
CellLabel->
"Out[235]=",ExpressionUUID->"489bd59f-b2aa-4c5d-8e5e-186abf5c5e30"]
}, Open ]]
"During evaluation of \
In[23]:=",ExpressionUUID->"00ec474b-f02f-45f7-861f-2e1395d96af0"]
}, Open ]],

Cell[BoxData[
RowBox[{
RowBox[{"pelbow", "[",
RowBox[{"\[Theta]1_", ",", "\[Theta]2_", ",", "\[Theta]3_"}], "]"}], ":=",

RowBox[{
RowBox[{"T", "[",
RowBox[{"0", ",", "0", ",", "\[Theta]1", ",", "0.333"}], "]"}], ".",
RowBox[{"T", "[",
RowBox[{
RowBox[{"-",
FractionBox["\[Pi]", "2"]}], ",", "0", ",", "\[Theta]2", ",", "0"}],
"]"}], ".",
RowBox[{"T", "[",
RowBox[{
FractionBox["\[Pi]", "2"], ",", "0", ",", "\[Theta]3", ",", "0.316"}],
"]"}], ".",
RowBox[{"T", "[",
RowBox[{
FractionBox["\[Pi]", "2"], ",", "0.0825", ",", "0", ",", "0"}],
"]"}]}]}]], "Input",
CellChangeTimes->{{3.823692303381714*^9, 3.823692313093701*^9}},
CellLabel->"In[9]:=",ExpressionUUID->"945a2b31-754d-4fdb-8276-9e69341b497a"],

Cell[BoxData[{
RowBox[{
RowBox[{"tmp", "=",
RowBox[{"pelbow", "[",
RowBox[{"\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3"}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"ToString", "[",
RowBox[{
RowBox[{"Simplify", "[", "tmp", "]"}], ",", "CForm"}], "]"}],
";"}], "\[IndentingNewLine]",
RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input",
CellChangeTimes->{{3.82369231492756*^9, 3.823692322197592*^9}, {
3.8236923671989527`*^9, 3.823692382966064*^9}, {3.823751749664494*^9,
3.823751764784334*^9}},
CellLabel->"In[26]:=",ExpressionUUID->"b631774c-2d04-493b-8a99-35a0466935c8"]
}, Open ]]
},
WindowSize->{808, 911},
WindowMargins->{{Automatic, 497}, {250, Automatic}},
WindowSize->{1519, 1198},
WindowMargins->{{Automatic, 264}, {66, Automatic}},
FrontEndVersion->"12.0 for Linux x86 (64-bit) (April 8, 2019)",
StyleDefinitions->"Default.nb"
]
Expand All @@ -566,13 +582,15 @@ Cell[3874, 116, 625, 15, 31, "Input",ExpressionUUID->"5eced034-3e4c-4a28-956f-54
Cell[4502, 133, 2978, 74, 193, "Input",ExpressionUUID->"25bca131-2606-4f40-956f-9b4b520b9c99"],
Cell[7483, 209, 580, 14, 31, "Input",ExpressionUUID->"31153068-cde7-4919-97b6-1af27bb17782"],
Cell[CellGroupData[{
Cell[8088, 227, 763, 19, 31, "Input",ExpressionUUID->"9db25a01-77cb-4265-b9c7-eac93a89fd5b"],
Cell[8854, 248, 8912, 230, 95, "Output",ExpressionUUID->"38574d08-ae8e-4fc2-be09-08955360fdff"]
Cell[8088, 227, 801, 21, 31, "Input",ExpressionUUID->"9db25a01-77cb-4265-b9c7-eac93a89fd5b"],
Cell[8892, 250, 9008, 232, 95, "Output",ExpressionUUID->"38574d08-ae8e-4fc2-be09-08955360fdff"]
}, Open ]],
Cell[CellGroupData[{
Cell[17803, 483, 650, 15, 78, "Input",ExpressionUUID->"f934a522-8b1f-4ba2-941b-f8a1252d3712"],
Cell[18456, 500, 2698, 40, 289, "Output",ExpressionUUID->"489bd59f-b2aa-4c5d-8e5e-186abf5c5e30"]
}, Open ]]
Cell[17937, 487, 695, 16, 78, "Input",ExpressionUUID->"f934a522-8b1f-4ba2-941b-f8a1252d3712"],
Cell[18635, 505, 443, 10, 23, "Message",ExpressionUUID->"00ec474b-f02f-45f7-861f-2e1395d96af0"]
}, Open ]],
Cell[19093, 518, 788, 22, 46, "Input",ExpressionUUID->"945a2b31-754d-4fdb-8276-9e69341b497a"],
Cell[19884, 542, 630, 15, 78, "Input",ExpressionUUID->"b631774c-2d04-493b-8a99-35a0466935c8"]
}, Open ]]
}
]
Expand Down
2 changes: 1 addition & 1 deletion ruckig
Submodule ruckig updated 33 files
+23 −11 .github/workflows/ci.yml
+69 −16 CMakeLists.txt
+32 −21 README.md
+5 −0 cmake/ruckig-config.cmake.in
+3 −3 doc/Doxyfile
+ − doc/benchmark.png
+8 −0 examples/CMakeLists.txt
+2 −3 examples/position.cpp
+51 −0 examples/position.py
+4 −2 include/ruckig/alternative/quintic.hpp
+13 −10 include/ruckig/alternative/reflexxes.hpp
+5 −3 include/ruckig/alternative/smoothie.hpp
+158 −0 include/ruckig/block.hpp
+20 −0 include/ruckig/brake.hpp
+115 −0 include/ruckig/input_parameter.hpp
+27 −0 include/ruckig/output_parameter.hpp
+0 −125 include/ruckig/parameter.hpp
+9 −64 include/ruckig/position.hpp
+282 −0 include/ruckig/profile.hpp
+34 −9 include/ruckig/roots.hpp
+28 −186 include/ruckig/ruckig.hpp
+130 −194 include/ruckig/trajectory.hpp
+58 −0 include/ruckig/velocity.hpp
+17 −1 src/brake.cpp
+29 −86 src/position-step1.cpp
+110 −49 src/position-step2.cpp
+50 −37 src/python.cpp
+97 −0 src/velocity-step1.cpp
+103 −0 src/velocity-step2.cpp
+54 −0 test/benchmark_plot.py
+97 −37 test/otg-benchmark.cpp
+35 −13 test/otg-test.cpp
+16 −32 test/otg_plot.py
6 changes: 5 additions & 1 deletion src/frankx/kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -247,6 +247,10 @@ Eigen::Matrix<double, 7, 1> Kinematics::inverse(const Eigen::Matrix<double, 6, 1
auto x_new = forwardEuler(q_current + alpha * dq);
double new_dis = (x_target - x_new).squaredNorm();

if (null_space) {
new_dis += std::pow(null_space->value - q_current(null_space->joint_index), 2);
}

if (new_dis < dis_min) {
dis_min = new_dis;
alpha_min = alpha;
Expand All @@ -258,7 +262,7 @@ Eigen::Matrix<double, 7, 1> Kinematics::inverse(const Eigen::Matrix<double, 6, 1
break;
}

// std::cout << i << " dis_min: " << dis_min << " " << (q0 - q_current).squaredNorm() << std::endl;
// std::cout << i << " dis_min: " << dis_min << " " << (null_space->value - q_current).squaredNorm() << std::endl;
}
return q_current;
}
Expand Down
22 changes: 16 additions & 6 deletions src/frankx/robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,20 +165,30 @@ bool Robot::move(const Affine& frame, WaypointMotion& motion, MotionData& data)
std::cout << "[frankx robot] continue motion after exception: " << exception.what() << std::endl;
automaticErrorRecovery();

data.velocity_rel *= 0.4;
data.acceleration_rel *= 0.4;
data.jerk_rel *= 0.4;
data.velocity_rel *= 0.5;
data.acceleration_rel *= 0.5;
data.jerk_rel *= 0.5;
mg.reset();

bool success {false};

try {
control(mg, controller_mode);
success = true;

} catch (franka::Exception exception) {
std::cout << exception.what() << std::endl;
return false;
}
return true;
data.velocity_rel *= 2;
data.acceleration_rel *= 2;
data.jerk_rel *= 2;

return success;

} else {
std::cout << exception.what() << std::endl;
}
std::cout << exception.what() << std::endl;

return false;
}
return true;
Expand Down
Loading

0 comments on commit cc81e52

Please sign in to comment.