fr3py调试遇到的一些问题

fr3jointinterface 在运行时，支持两种模式：torque 和 velocity 模式；但是这2种模式是互斥的；也就是如果同时运行2个，必然有一个会崩溃。

可能会提示如下错误：

libfranka: Move command rejected: command not possible in the current mode (“User stopped”)!

fr3jointinterface 不能在【Execution】模式下执行，因为 fr3jointinterface 内部会通过fci的control去注册数据回调，在 【Execution】模式下，运行 fr3jointinterface ，FCI会报错，提示模式不允许直接退出。

fr3jointinterface 只能在franka的【Programming】模式下执行，此模式可以执行 LCM转发到FCI的任何指令，也能采集到数据。

但是 franka的【Programming】模式下 无法执行示教操作，无法用手引导Pilot 末端执行器做一些操作。此时关节被电击控制，即使按了pilot的按钮，也无法人工操作末端执行器。

所以我们不可以使用LCM模式的情况下，利用fr3py 去采集Pilot的示教数据。

官方只提供了 gripper.cpp桥接，可以让python直接调用 libfranka实现数据采集。 而没有提供关节的桥接。 这样可以避开问题2，问题2 里使用control指令受限。

我们编写了joint.cpp 自己绕过libfranka,获取关节数据。

#include <franka/gripper.h> 
#include <franka/gripper_state.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <string>

namespace py = pybind11;
using namespace pybind11::literals;

PYBIND11_MODULE(fr3_gripper, handle)
{
  py::class_<franka::GripperState>(handle, "GripperState")
    .def(py::init<>())
    .def_readwrite("width", &franka::GripperState::width)
    .def_readwrite("max_width", &franka::GripperState::max_width)
    .def_readwrite("is_grasped", &franka::GripperState::is_grasped)
    .def_readwrite("temperature", &franka::GripperState::temperature)
    .def_readwrite("time", &franka::GripperState::time)
    .def("__repr__",
        [](const franka::GripperState &state) {
            return "<GripperState width='" + std::to_string(state.width) + "' max_width='" + std::to_string(state.max_width) + "' is_grasped='" + std::to_string(state.is_grasped) + "' temperature='" + std::to_string(state.temperature) + "' time='" + std::to_string(state.time.toMSec()) + "'>";
        }
    );

  py::class_<franka::Gripper>(handle, "Gripper")
    .def(py::init<const std::string&>())
    .def("homing", &franka::Gripper::homing)
    .def("grasp", &franka::Gripper::grasp)
    .def("move", &franka::Gripper::move)
    .def("stop", &franka::Gripper::stop)
    .def("readOnce", [](franka::Gripper &self) -> py::dict {
        franka::GripperState state = self.readOnce();
        return py::dict("width"_a=state.width, "max_width"_a=state.max_width, "is_grasped"_a=state.is_grasped, "temperature"_a=state.temperature, "time (ms)"_a=state.time.toMSec());
    }, "read the gripper state")
    .def("serverVersion", &franka::Gripper::serverVersion);
}

一般会有类似的错误：

ModuleNotFoundError: No module named 'fr3_joint'

解决方式：

script_dir = os.path.dirname(os.path.abspath(__file__))
parent_dir = os.path.dirname(script_dir)
fr3_gripper_dir = os.path.join(parent_dir, 'fr3_gripper')

如果还是找不到，则要考虑是否编译使用的python版本和运行的python不是一个版本，无法进行定位