Skip to content

Commit

Permalink
Implemented non-zero velocity joint waypoints
Browse files Browse the repository at this point in the history
  • Loading branch information
TimSchneider42 committed May 27, 2024
1 parent caa1500 commit 101fc6d
Show file tree
Hide file tree
Showing 10 changed files with 88 additions and 25 deletions.
1 change: 1 addition & 0 deletions franky/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
RobotPose,
RobotVelocity,
CartesianState,
JointState,
CartesianWaypoint,
Affine,
NullSpaceHandling,
Expand Down
1 change: 1 addition & 0 deletions include/franky.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "franky/cartesian_state.hpp"
#include "franky/control_signal_type.hpp"
#include "franky/gripper.hpp"
#include "franky/joint_state.hpp"
#include "franky/kinematics.hpp"
#include "franky/robot.hpp"
#include "franky/robot_pose.hpp"
Expand Down
40 changes: 40 additions & 0 deletions include/franky/joint_state.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
#pragma once

#include <optional>
#include <Eigen/Core>
#include <utility>

#include "franky/robot_pose.hpp"
#include "franky/robot_velocity.hpp"

namespace franky {

class JointState {
public:
// Suppress implicit conversion warning
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wimplicit-conversion"
JointState(Vector7d position) : position_(std::move(position)), velocity_(Vector7d::Zero()) {}
#pragma clang diagnostic pop

JointState(Vector7d pose, Vector7d velocity)
: position_(std::move(pose)), velocity_(std::move(velocity)) {}

JointState(const JointState &) = default;

JointState() = default;

[[nodiscard]] inline Vector7d position() const {
return position_;
}

[[nodiscard]] inline Vector7d velocity() const {
return velocity_;
}

private:
Vector7d position_;
Vector7d velocity_;
};

} // namespace franky
4 changes: 2 additions & 2 deletions include/franky/motion/joint_motion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <optional>
#include <ruckig/ruckig.hpp>

#include "franky/robot_pose.hpp"
#include "franky/joint_state.hpp"
#include "franky/motion/joint_waypoint_motion.hpp"
#include "franky/motion/reference_type.hpp"

Expand All @@ -13,7 +13,7 @@ namespace franky {
class JointMotion : public JointWaypointMotion {
public:
explicit JointMotion(
const Vector7d &target,
const JointState &target,
ReferenceType reference_type,
RelativeDynamicsFactor relative_dynamics_factor,
bool return_when_finished);
Expand Down
13 changes: 5 additions & 8 deletions include/franky/motion/joint_waypoint_motion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,19 +6,16 @@

#include <franka/robot_state.h>

#include "franky/robot_pose.hpp"
#include "franky/robot.hpp"
#include "franky/motion/reference_type.hpp"
#include "franky/util.hpp"
#include "franky/joint_state.hpp"
#include "franky/motion/waypoint_motion.hpp"

namespace franky {

class JointWaypointMotion : public WaypointMotion<franka::JointPositions, Vector7d> {
class JointWaypointMotion : public WaypointMotion<franka::JointPositions, JointState> {
public:
explicit JointWaypointMotion(const std::vector<Waypoint<Vector7d>> &waypoints);
explicit JointWaypointMotion(const std::vector<Waypoint<JointState>> &waypoints);

explicit JointWaypointMotion(const std::vector<Waypoint<Vector7d>> &waypoints, Params params);
explicit JointWaypointMotion(const std::vector<Waypoint<JointState>> &waypoints, Params params);

protected:

Expand All @@ -30,7 +27,7 @@ class JointWaypointMotion : public WaypointMotion<franka::JointPositions, Vector
void setNewWaypoint(
const franka::RobotState &robot_state,
const std::optional<franka::JointPositions> &previous_command,
const Waypoint<Vector7d> &new_waypoint,
const Waypoint<JointState> &new_waypoint,
ruckig::InputParameter<7> &input_parameter) override;

[[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override;
Expand Down
4 changes: 2 additions & 2 deletions include/franky/motion/stop_motion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@ template<>
class StopMotion<franka::JointPositions> : public JointWaypointMotion {
public:
explicit StopMotion() : JointWaypointMotion(
{{
.target=Vector7d::Zero(),
{Waypoint<JointState>{
.target=JointState(Vector7d::Zero()),
.reference_type= ReferenceType::Relative,
.relative_dynamics_factor = RelativeDynamicsFactor::MAX_DYNAMICS()
}}) {}
Expand Down
1 change: 1 addition & 0 deletions include/franky/motion/waypoint_motion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include "franky/util.hpp"
#include "franky/motion/motion.hpp"
#include "franky/relative_dynamics_factor.hpp"
#include "franky/motion/reference_type.hpp"

namespace franky {

Expand Down
29 changes: 27 additions & 2 deletions python/python.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -584,6 +584,31 @@ PYBIND11_MODULE(_franky, m) {
));
py::implicitly_convertible<RobotPose, CartesianState>();

py::class_<JointState>(m, "JointState")
.def(py::init<const Vector7d &>(), "position"_a)
.def(py::init<const Vector7d &, const Vector7d &>(), "position"_a,"velocity"_a)
.def(py::init<const JointState &>()) // Copy constructor
.def_property_readonly("position", &JointState::position)
.def_property_readonly("velocity", &JointState::velocity)
.def("__repr__", [](const JointState &joint_state) {
std::stringstream ss;
ss << "(pose=" << vecToStr(joint_state.position())
<< ", velocity=" << vecToStr(joint_state.velocity()) << ")";
return ss.str();
})
.def(py::pickle(
[](const JointState &joint_state) { // __getstate__
return py::make_tuple(joint_state.position(), joint_state.velocity());
},
[](const py::tuple &t) { // __setstate__
if (t.size() != 2)
throw std::runtime_error("Invalid state!");
return JointState(t[0].cast<Vector7d>(), t[1].cast<Vector7d>());
}
));
py::implicitly_convertible<Vector7d, JointState>();


py::class_<Kinematics::NullSpaceHandling>(m, "NullSpaceHandling")
.def(py::init<size_t, double>(), "joint_index"_a, "value"_a)
.def_readwrite("joint_index", &Kinematics::NullSpaceHandling::joint_index)
Expand Down Expand Up @@ -745,7 +770,7 @@ PYBIND11_MODULE(_franky, m) {
py::class_<JointWaypointMotion, Motion<franka::JointPositions>, std::shared_ptr<JointWaypointMotion>>(
m, "JointWaypointMotion")
.def(py::init<>([](
const std::vector<Waypoint<Vector7d>> &waypoints,
const std::vector<Waypoint<JointState>> &waypoints,
RelativeDynamicsFactor relative_dynamics_factor,
bool return_when_finished) {
return std::make_shared<JointWaypointMotion>(
Expand All @@ -757,7 +782,7 @@ PYBIND11_MODULE(_franky, m) {
"return_when_finished"_a = true);

py::class_<JointMotion, JointWaypointMotion, std::shared_ptr<JointMotion>>(m, "JointMotion")
.def(py::init<const Vector7d &, ReferenceType, double, bool>(),
.def(py::init<const JointState &, ReferenceType, double, bool>(),
"target"_a,
py::arg_v("reference_type", ReferenceType::Absolute, "_franky.ReferenceType.Absolute"),
"relative_dynamics_factor"_a = 1.0,
Expand Down
2 changes: 1 addition & 1 deletion src/motion/joint_motion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
namespace franky {

JointMotion::JointMotion(
const Vector7d &target,
const JointState &target,
ReferenceType reference_type,
RelativeDynamicsFactor relative_dynamics_factor,
bool return_when_finished)
Expand Down
18 changes: 8 additions & 10 deletions src/motion/joint_waypoint_motion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,17 +2,15 @@

#include <ruckig/ruckig.hpp>

#include "franky/robot_pose.hpp"
#include "franky/robot.hpp"
#include "franky/util.hpp"
#include "franky/motion/reference_type.hpp"

namespace franky {

JointWaypointMotion::JointWaypointMotion(const std::vector<Waypoint<Vector7d>> &waypoints)
JointWaypointMotion::JointWaypointMotion(const std::vector<Waypoint<JointState>> &waypoints)
: JointWaypointMotion(waypoints, Params()) {}

JointWaypointMotion::JointWaypointMotion(const std::vector<Waypoint<Vector7d>> &waypoints, Params params)
: WaypointMotion<franka::JointPositions, Vector7d>(waypoints, params) {}
JointWaypointMotion::JointWaypointMotion(const std::vector<Waypoint<JointState>> &waypoints, Params params)
: WaypointMotion<franka::JointPositions, JointState>(waypoints, params) {}

void JointWaypointMotion::initWaypointMotion(
const franka::RobotState &robot_state,
Expand All @@ -34,13 +32,13 @@ franka::JointPositions JointWaypointMotion::getControlSignal(
void JointWaypointMotion::setNewWaypoint(
const franka::RobotState &robot_state,
const std::optional<franka::JointPositions> &previous_command,
const Waypoint<Vector7d> &new_waypoint,
const Waypoint<JointState> &new_waypoint,
ruckig::InputParameter<7> &input_parameter) {
auto new_target = new_waypoint.target;
if (new_waypoint.reference_type == ReferenceType::Relative)
new_target += toEigen(input_parameter.current_position);
input_parameter.target_position = toStd<7>(new_target);
input_parameter.target_velocity = toStd<7>(Vector7d::Zero());
new_target.position() += toEigen(input_parameter.current_position);
input_parameter.target_position = toStd<7>(new_target.position());
input_parameter.target_velocity = toStd<7>(new_target.velocity());
input_parameter.target_acceleration = toStd<7>(Vector7d::Zero());
}

Expand Down

0 comments on commit 101fc6d

Please sign in to comment.