From 767701db843de4a148746fa102210dcac5f6ae43 Mon Sep 17 00:00:00 2001 From: Ryan Shim Date: Thu, 17 Oct 2019 17:10:17 +0900 Subject: [PATCH] om friends missing libraries added --- .../OpenManipulator/src/delta_libs.h | 1 + .../src/delta_libs/include/delta_libs/delta.h | 74 ++ .../delta_libs/delta_custom_trajectory.h | 158 +++ .../include/delta_libs/delta_dynamixel.h | 189 ++++ .../include/delta_libs/delta_kinematics.h | 54 + .../src/delta_libs/src/delta.cpp | 243 +++++ .../src/delta_custom_trajectory.cpp | 346 +++++++ .../src/delta_libs/src/delta_dynamixel.cpp | 973 ++++++++++++++++++ .../src/delta_libs/src/delta_kinematics.cpp | 140 +++ .../OpenManipulator/src/linear_libs.h | 1 + .../linear_libs/include/linear_libs/linear.h | 79 ++ .../linear_libs/linear_custom_trajectory.h | 158 +++ .../include/linear_libs/linear_dynamixel.h | 189 ++++ .../include/linear_libs/linear_kinematics.h | 55 + .../src/linear_libs/src/linear.cpp | 214 ++++ .../src/linear_custom_trajectory.cpp | 346 +++++++ .../src/linear_libs/src/linear_dynamixel.cpp | 973 ++++++++++++++++++ .../src/linear_libs/src/linear_kinematics.cpp | 158 +++ .../OpenManipulator/src/planar_libs.h | 1 + .../planar_libs/include/planar_libs/planar.h | 77 ++ .../planar_libs/planar_custom_trajectory.h | 159 +++ .../include/planar_libs/planar_dynamixel.h | 189 ++++ .../include/planar_libs/planar_kinematics.h | 54 + .../src/planar_libs/src/planar.cpp | 215 ++++ .../src/planar_custom_trajectory.cpp | 346 +++++++ .../src/planar_libs/src/planar_dynamixel.cpp | 973 ++++++++++++++++++ .../src/planar_libs/src/planar_kinematics.cpp | 125 +++ .../OpenManipulator/src/scara_libs.h | 1 + .../src/scara_libs/include/scara_libs/scara.h | 79 ++ .../scara_libs/scara_custom_trajectory.h | 158 +++ .../include/scara_libs/scara_dynamixel.h | 189 ++++ .../include/scara_libs/scara_kinematics.h | 55 + .../src/scara_libs/src/scara.cpp | 206 ++++ .../src/scara_custom_trajectory.cpp | 346 +++++++ .../src/scara_libs/src/scara_dynamixel.cpp | 973 ++++++++++++++++++ .../src/scara_libs/src/scara_kinematics.cpp | 169 +++ .../OpenManipulator/src/stewart_libs.h | 1 + .../include/stewart_libs/stewart.h | 78 ++ .../stewart_libs/stewart_custom_trajectory.h | 158 +++ .../include/stewart_libs/stewart_dynamixel.h | 189 ++++ .../include/stewart_libs/stewart_kinematics.h | 54 + .../src/stewart_libs/src/stewart.cpp | 326 ++++++ .../src/stewart_custom_trajectory.cpp | 346 +++++++ .../stewart_libs/src/stewart_dynamixel.cpp | 973 ++++++++++++++++++ .../stewart_libs/src/stewart_kinematics.cpp | 227 ++++ 45 files changed, 11018 insertions(+) create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_custom_trajectory.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_dynamixel.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_kinematics.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_custom_trajectory.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_dynamixel.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_kinematics.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_custom_trajectory.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_dynamixel.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_kinematics.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_custom_trajectory.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_dynamixel.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_kinematics.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_custom_trajectory.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_dynamixel.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_kinematics.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_custom_trajectory.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_dynamixel.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_kinematics.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_custom_trajectory.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_dynamixel.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_kinematics.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_custom_trajectory.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_dynamixel.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_kinematics.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_custom_trajectory.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_dynamixel.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_kinematics.h create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_custom_trajectory.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_dynamixel.cpp create mode 100644 arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_kinematics.cpp diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs.h new file mode 100644 index 000000000..efffe2b0e --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs.h @@ -0,0 +1 @@ +#include "delta_libs/include/delta_libs/delta.h" diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta.h new file mode 100644 index 000000000..ac0e486b5 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta.h @@ -0,0 +1,74 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef DELTA_H_ +#define DELTA_H_ + +#include "delta_custom_trajectory.h" +#include "delta_dynamixel.h" +#include "delta_kinematics.h" + +#define CUSTOM_TRAJECTORY_SIZE 4 +#define CUSTOM_TRAJECTORY_LINE "custom_trajectory_line" +#define CUSTOM_TRAJECTORY_CIRCLE "custom_trajectory_circle" +#define CUSTOM_TRAJECTORY_RHOMBUS "custom_trajectory_rhombus" +#define CUSTOM_TRAJECTORY_HEART "custom_trajectory_heart" + +#define DXL_SIZE 3 +#define JOINT_DYNAMIXEL "joint_dxl" + +#define RECEIVE_RATE 0.100 // unit: s +#define CONTROL_RATE 0.010 // unit: s + +#define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) +#define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) +#define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) + +class Delta : public robotis_manipulator::RobotisManipulator +{ +private: + robotis_manipulator::Kinematics *kinematics_; + robotis_manipulator::JointActuator *joint_; + robotis_manipulator::ToolActuator *tool_; + robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE]; + + bool using_actual_robot_state_; + bool receive_data_flag_ = false; + double prev_receive_time_ = 0.0; + double prev_control_time_ = 0.0; + +public: + Delta(); + virtual ~Delta(); + + void initDebug(); + void initOpenManipulator(bool using_actual_robot_state, + STRING usb_port = "/dev/ttyUSB0", + STRING baud_rate = "1000000", + float control_rate = CONTROL_RATE); + void processOpenManipulator(double present_time); + + bool getUsingActualRobotState(); + bool getReceiveDataFlag(); + double getPrevReceiveTime(); + + void setReceiveDataFlag(bool receive_data_flag); + void setPrevReceiveTime(double prev_receive_time); +}; + +#endif // Delta_H_ diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_custom_trajectory.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_custom_trajectory.h new file mode 100644 index 000000000..7e405ff7e --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_custom_trajectory.h @@ -0,0 +1,158 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef SCARA_CUSTOM_TRAJECTORY_H_ +#define SCARA_CUSTOM_TRAJECTORY_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace robotis_manipulator; +using namespace Eigen; + +namespace delta_custom_trajectory +{ + +enum AXIS{ + X_AXIS, + Y_AXIS, + Z_AXIS, +}; + +/***************************************************************************** +** Line +*****************************************************************************/ +class Line : public robotis_manipulator::CustomTaskTrajectory +{ +private: + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double acc_dec_time_; + double move_time_; + std::vector vel_max_; + +public: + Line() {} + virtual ~Line() {} + + void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta); + TaskWaypoint drawLine(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Circle +*****************************************************************************/ +class Circle : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Circle() {} + virtual ~Circle() {} + + void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawCircle(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +class Rhombus : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Rhombus() {} + virtual ~Rhombus() {} + + void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawRhombus(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Heart +*****************************************************************************/ +class Heart : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Heart() {} + virtual ~Heart() {} + + void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawHeart(double tick); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +} // namespace delta_custom_trajectory +#endif // DELTA_CUSTOM_TRAJECTORY_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_dynamixel.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_dynamixel.h new file mode 100644 index 000000000..0802e8c0a --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_dynamixel.h @@ -0,0 +1,189 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef DELTA_DYNAMIXEL_H_ +#define DELTA_DYNAMIXEL_H_ + +#if defined(__OPENCR__) + #include + #include +#else + #include + #include +#endif + +namespace delta_dynamixel +{ + +#define SYNC_WRITE_HANDLER 0 +#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 + +//#define CONTROL_LOOP_TIME 10; //ms + +// Protocol 2.0 +#define ADDR_PRESENT_CURRENT_2 126 +#define ADDR_PRESENT_VELOCITY_2 128 +#define ADDR_PRESENT_POSITION_2 132 +#define ADDR_VELOCITY_TRAJECTORY_2 136 +#define ADDR_POSITION_TRAJECTORY_2 140 +#define ADDR_PROFILE_ACCELERATION_2 108 +#define ADDR_PROFILE_VELOCITY_2 112 +#define ADDR_GOAL_POSITION_2 116 + + +#define LENGTH_PRESENT_CURRENT_2 2 +#define LENGTH_PRESENT_VELOCITY_2 4 +#define LENGTH_PRESENT_POSITION_2 4 +#define LENGTH_VELOCITY_TRAJECTORY_2 4 +#define LENGTH_POSITION_TRAJECTORY_2 4 +#define LENGTH_PROFILE_ACCELERATION_2 4 +#define LENGTH_PROFILE_VELOCITY_2 4 +#define LENGTH_GOAL_POSITION_2 4 + + +// Protocol 1.0 +#define ADDR_PRESENT_CURRENT_1 = 40; +#define ADDR_PRESENT_VELOCITY_1 = 38; +#define ADDR_PRESENT_POSITION_1 = 36; + +#define LENGTH_PRESENT_CURRENT_1 = 2; +#define LENGTH_PRESENT_VELOCITY_1 = 2; +#define LENGTH_PRESENT_POSITION_1 = 2; + +typedef struct +{ + std::vector id; + uint8_t num; +} Joint; + +class JointDynamixel : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + +public: + JointDynamixel(){} + virtual ~JointDynamixel(){} + + + /***************************************************************************** + ** Joint Dynamixel Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalPosition(std::vector actuator_id, std::vector radian_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class JointDynamixelProfileControl : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + float control_loop_time_; // unit: ms + std::map previous_goal_value_; + +public: + JointDynamixelProfileControl(float control_loop_time = 0.010); + virtual ~JointDynamixelProfileControl(){} + + + /***************************************************************************** + ** Joint Dynamixel Profile Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class ToolDynamixel : public robotis_manipulator::ToolActuator +{ + private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + + public: + ToolDynamixel() {} + virtual ~ToolDynamixel() {} + + + /***************************************************************************** + ** Tool Dynamixel Control Functions + *****************************************************************************/ + virtual void init(uint8_t actuator_id, const void *arg); + virtual void setMode(const void *arg); + virtual uint8_t getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value); + virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue(); + + + /***************************************************************************** + ** Functions called in Tool Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(STRING dynamixel_mode = "position_mode"); + bool writeProfileValue(STRING profile_mode, uint32_t value); + bool setSDKHandler(); + bool writeGoalPosition(double radian); + double receiveDynamixelValue(); +}; + +} // namespace delta_dynamixel +#endif // DELTA_DYNAMIXEL_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_kinematics.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_kinematics.h new file mode 100644 index 000000000..8478bd98f --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/include/delta_libs/delta_kinematics.h @@ -0,0 +1,54 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef DELTA_KINEMATICS_H_ +#define DELTA_KINEMATICS_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace Eigen; +using namespace robotis_manipulator; + +namespace delta_kinematics +{ + +/***************************************************************************** +** Kinematics Solver Using Geometry +*****************************************************************************/ +class SolverUsingGeometry : public robotis_manipulator::Kinematics +{ +private: + bool inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); + +public: + SolverUsingGeometry() {} + virtual ~SolverUsingGeometry() {} + + virtual void setOption(const void *arg); + virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); + virtual void solveForwardKinematics(Manipulator *manipulator); + virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); +}; + +} // namespace delta_kinematics + +#endif // DELTA_KINEMATICS_H_ diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta.cpp new file mode 100644 index 000000000..910a6330e --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta.cpp @@ -0,0 +1,243 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/delta_libs/delta.h" + +Delta::Delta() {} +Delta::~Delta() +{ + delete kinematics_; + delete joint_; + delete tool_; + for(uint8_t index = 0; index < CUSTOM_TRAJECTORY_SIZE; index++) + delete custom_trajectory_[index]; +} + +void Delta::initDebug() +{ + DEBUG.begin(57600); // Using Serial4(=SerialBT2) + log::print("OpenManipulator Debugging Port"); +} + +void Delta::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_rate) +{ + /***************************************************************************** + ** Set if using actual robot + *****************************************************************************/ + using_actual_robot_state_ = using_actual_robot_state; + + /***************************************************************************** + ** Initialize Manipulator Parameters + *****************************************************************************/ + addWorld("world", // world name + "joint1"); // child name + + addJoint("joint1", // my name + "world", // parent name + "joint4", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 1); // actuator id + + addJoint("joint2", // my name + "world", // parent name + "joint5", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation + Z_AXIS, // axis of rotation + 2); // actuator id + + addJoint("joint3", // my name + "world", // parent name + "joint6", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 3); // actuator id + + addJoint("joint4", // my name + "joint1", // parent name + "joint7", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint4_2", // my name + "joint1", // parent name + "joint7", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint5", // my name + "joint2", // parent name + "joint8", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint5_2", // my name + "joint2", // parent name + "joint8", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint6", // my name + "joint3", // parent name + "joint9", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint6_2", // my name + "joint3", // parent name + "joint9", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint7", // my name + "joint4", // parent name + "tool", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint7_2", // my name + "joint4", // parent name + "tool", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addTool("tool", // my name + "joint7", // Not used + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + -1); // actuator id + + /***************************************************************************** + ** Initialize Kinematics + *****************************************************************************/ + kinematics_ = new delta_kinematics::SolverUsingGeometry(); + addKinematics(kinematics_); + + /***************************************************************************** + ** Initialize Custom Trajectory + *****************************************************************************/ + custom_trajectory_[0] = new delta_custom_trajectory::Line(); + custom_trajectory_[1] = new delta_custom_trajectory::Circle(); + custom_trajectory_[2] = new delta_custom_trajectory::Rhombus(); + custom_trajectory_[3] = new delta_custom_trajectory::Heart(); + + addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]); + addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]); + addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]); + addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]); + + if (using_actual_robot_state_) + { + /***************************************************************************** + ** Initialize ㅓoint Actuator + *****************************************************************************/ + joint_ = new delta_dynamixel::JointDynamixelProfileControl(control_rate); + + // Set communication arguments + STRING dxl_comm_arg[2] = {usb_port, baud_rate}; + void *p_dxl_comm_arg = &dxl_comm_arg; + + // Set joint actuator id + std::vector jointDxlId; + jointDxlId.push_back(1); + jointDxlId.push_back(2); + jointDxlId.push_back(3); + addJointActuator(JOINT_DYNAMIXEL, joint_, jointDxlId, p_dxl_comm_arg); + + // Set joint actuator control mode + STRING joint_dxl_mode_arg = "position_mode"; + void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; + setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); + + /***************************************************************************** + ** Enable actuators and Receive actuator values + *****************************************************************************/ + // Enable All Actuators + enableAllActuator(); + + // Receive current angles from all actuators + receiveAllJointActuatorValue(); + } +} + +/***************************************************************************** +** Process actuator values received from external controllers +*****************************************************************************/ +void Delta::processOpenManipulator(double present_time) +{ + if (present_time - prev_control_time_ >= CONTROL_RATE) + { + JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time); + if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value); + + // Set previous control time + prev_control_time_ = millis()/1000.0; + } +} + +/***************************************************************************** +** State Functions +*****************************************************************************/ +// Check if using acutal robot +bool Delta::getUsingActualRobotState() +{ + return using_actual_robot_state_; +} + +/* Check if the program read data within control rate) */ +bool Delta::getReceiveDataFlag() +{ + return receive_data_flag_; +} + +/* Get the previous time when data were received */ +double Delta::getPrevReceiveTime() +{ + return prev_receive_time_; +} + +/* Set whether data were received or not */ +void Delta::setReceiveDataFlag(bool receive_data_flag) +{ + receive_data_flag_ = receive_data_flag; +} + +/* Set the previous time when data were received */ +void Delta::setPrevReceiveTime(double prev_receive_time) +{ + prev_receive_time_ = prev_receive_time; +} \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_custom_trajectory.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_custom_trajectory.cpp new file mode 100644 index 000000000..7b0e65bbd --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_custom_trajectory.cpp @@ -0,0 +1,346 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/delta_libs/delta_custom_trajectory.h" + +using namespace delta_custom_trajectory; +using namespace Eigen; + + +/***************************************************************************** +** Line +*****************************************************************************/ +void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta) +{ + move_time_ = move_time; + acc_dec_time_ = move_time_ * 0.2; + vel_max_.resize(3); + + TaskWaypoint start_to_goal; + + start_pose_ = start; + + goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation; + goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position; + + vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_); +} + +TaskWaypoint Line::drawLine(double time_var) +{ + TaskWaypoint pose; + + if(acc_dec_time_ >= time_var) + { + pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var ) + { + pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_)) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + } + else if(time_var <= move_time_) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS); + } + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Line::getTaskWaypoint(double tick) +{ + return drawLine(tick); +} + + +void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + TaskWaypoint *c_arg = (TaskWaypoint *)arg; + initLine(move_time, start, c_arg[0]); +} +void Line::setOption(const void *arg) {} + + +/***************************************************************************** +** Circle +*****************************************************************************/ +void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Circle::drawCircle(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + + double diff_pose[2]; + + diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_); + diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Circle::getTaskWaypoint(double tick) +{ + return drawCircle(tick); +} + +void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +void Circle::setOption(const void *arg){} + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + + +TaskWaypoint Rhombus::drawRhombus(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + while(true) + { + if (get_time_var < PI*2) break; + get_time_var = get_time_var - PI*2; + } + + if (get_time_var >= 0 && get_time_var < PI/2){ + traj[0] = - get_time_var / (PI/2); + traj[1] = - get_time_var / (PI/2); + } else if (get_time_var >= PI/2 && get_time_var < PI){ + traj[0] = - get_time_var / (PI/2); + traj[1] = get_time_var / (PI/2) - 2; + } else if (get_time_var >= PI && get_time_var < PI*3/2){ + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = get_time_var / (PI/2) - 2; + } else { + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = - get_time_var / (PI/2) + 4; + } + + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + + +void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +TaskWaypoint Rhombus::getTaskWaypoint(double tick) +{ + return drawRhombus(tick); +} +void Rhombus::setOption(const void *arg){} + + +/***************************************************************************** +** Heart +*****************************************************************************/ +void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Heart::drawHeart(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + double shift_offset = - 5.0; + + traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16; + traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16; + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} +void Heart::setOption(const void *arg){} + +TaskWaypoint Heart::getTaskWaypoint(double tick) +{ + return drawHeart(tick); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_dynamixel.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_dynamixel.cpp new file mode 100644 index 000000000..a4bdeb888 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_dynamixel.cpp @@ -0,0 +1,973 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/delta_libs/delta_dynamixel.h" + +using namespace delta_dynamixel; +using namespace robotis_manipulator; + +/***************************************************************************** +** Joint Dynamixel Control Functions +*****************************************************************************/ +void JointDynamixel::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixel::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixel::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixel::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixel::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixel::getId() +{ + return dynamixel_.id; +} + +void JointDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixel::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + std::vector radian_vector; + for(uint32_t index = 0; index < value_vector.size(); index++) + { + radian_vector.push_back(value_vector.at(index).position); + } + result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixel::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixel::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Control Functions +*****************************************************************************/ +bool JointDynamixel::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixel::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixel::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixel::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool JointDynamixel::writeGoalPosition(std::vector actuator_id, std::vector radian_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_position[actuator_id.size()]; + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + id_array[index] = actuator_id.at(index); + goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index)); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +std::vector JointDynamixel::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Joint Dynamixel Profile Control Functions +*****************************************************************************/ +JointDynamixelProfileControl::JointDynamixelProfileControl(float control_loop_time) +{ + control_loop_time_ = control_loop_time; +} + +void JointDynamixelProfileControl::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixelProfileControl::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixelProfileControl::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixelProfileControl::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixelProfileControl::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixelProfileControl::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixelProfileControl::getId() +{ + return dynamixel_.id; +} + +void JointDynamixelProfileControl::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixelProfileControl::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixelProfileControl::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + result = JointDynamixelProfileControl::writeGoalProfilingControlValue(actuator_id, value_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixelProfileControl::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixelProfileControl::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Profile Control Functions +*****************************************************************************/ +bool JointDynamixelProfileControl::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setTimeBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixelProfileControl::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3; + const uint32_t acceleration = uint32_t(control_loop_time_*1000); + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixelProfileControl::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixelProfileControl::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + return true; +} + +bool JointDynamixelProfileControl::writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_value[actuator_id.size()]; + + //add tarajectory eq. + for(uint8_t index = 0; index < actuator_id.size(); index++) + { + float result_position; + float time_control = control_loop_time_; //ms + + if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end()) + { + previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index))); + } + + result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2; + + id_array[index] = actuator_id.at(index); + goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position); + + previous_goal_value_[actuator_id.at(index)] = value_vector.at(index); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log); + if (result == false) + { + log::error(log); + } + return true; +} + +std::vector JointDynamixelProfileControl::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Tool Dynamixel Control Functions +*****************************************************************************/ +void ToolDynamixel::init(uint8_t actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = ToolDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void ToolDynamixel::setMode(const void *arg) +{ + bool result = false; +// const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = ToolDynamixel::setOperatingMode(get_arg_[0]); + if (result == false) + return; + } + else + { + result = ToolDynamixel::writeProfileValue(get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + + result = ToolDynamixel::setSDKHandler(); + if (result == false) + return; +} + +uint8_t ToolDynamixel::getId() +{ + return dynamixel_.id.at(0); +} + +void ToolDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = true; +} + +void ToolDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = false; +} + +bool ToolDynamixel::sendToolActuatorValue(robotis_manipulator::ActuatorValue value) +{ + return ToolDynamixel::writeGoalPosition(value.position); +} + +robotis_manipulator::ActuatorValue ToolDynamixel::receiveToolActuatorValue() +{ + robotis_manipulator::ActuatorValue result; + result.position = ToolDynamixel::receiveDynamixelValue(); + result.velocity = 0.0; + result.acceleration = 0.0; + result.effort = 0.0; + return result; +} + + +/***************************************************************************** +** Functions called in Tool Dynamixel Profile Control Functions +*****************************************************************************/ +bool ToolDynamixel::initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + const char* log = NULL; + bool result = false; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id.push_back(actuator_id); + dynamixel_.num = 1; + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Tool Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0)); + strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0))); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + + return true; +} + +bool ToolDynamixel::setOperatingMode(STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 200; + + if (dynamixel_mode == "position_mode") + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log); + if (result == false) + { + log::error(log); + } + } + else + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool ToolDynamixel::writeProfileValue(STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::setSDKHandler() +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0), "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0), + "Present_Position", + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::writeGoalPosition(double radian) +{ + bool result = false; + const char* log = NULL; + + int32_t goal_position = 0; + + goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian); + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, &goal_position, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +double ToolDynamixel::receiveDynamixelValue() +{ + bool result = false; + const char* log = NULL; + + int32_t get_value = 0; + uint8_t id_array[1] = {dynamixel_.id.at(0)}; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &get_value, + &log); + if (result == false) + { + log::error(log); + } + + return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_kinematics.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_kinematics.cpp new file mode 100644 index 000000000..1b6de7c82 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/delta_libs/src/delta_kinematics.cpp @@ -0,0 +1,140 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/delta_libs/delta_kinematics.h" + +using namespace robotis_manipulator; +using namespace delta_kinematics; + +/***************************************************************************** +** Kinematics Solver +*****************************************************************************/ +void SolverUsingGeometry::setOption(const void *arg) {} + +Eigen::MatrixXd SolverUsingGeometry::jacobian(Manipulator *manipulator, Name tool_name) +{ + return Eigen::MatrixXd::Identity(6, manipulator->getDOF()); +} + +void SolverUsingGeometry::solveForwardKinematics(Manipulator *manipulator) {} + +bool SolverUsingGeometry::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + return inverseKinematicsSolverUsingGeometry(manipulator, tool_name, target_pose, goal_joint_value); +} + +/***************************************************************************** +** Private +*****************************************************************************/ +bool SolverUsingGeometry::inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + std::vector target_angle_vector; + + double temp_angle[3]; + double temp_angle2[3]; + JointValue target_angle[12]; + JointValue target_angle2[3]; + const double link[3] = {0.100, 0.224, 0.020}; + double start_x[3], start_y[3], start_z[3]; + double goal_x[3], goal_y[3], goal_z[3]; + double diff_x[3], diff_y[3], diff_z[3]; + double temp[3], temp2[3], temp3[3], temp4[3], temp5[3] , temp6[3]; + double a[3], b[3], c[3]; + double target_pose_length[3]; + double elbow_x[3], elbow_y[3], elbow_z[3]; + double diff_x2[3], diff_y2[3], diff_z2[3]; + + // Start pose for each set of two joints + for (int i=0; i<3; i++) + { + start_x[i] = cos(PI*2.0/3.0*i)*(-0.055); + start_y[i] = sin(PI*2.0/3.0*i)*(-0.055); + start_z[i] = 0.17875; + } + + // Goal pose for each set of two joints + for (int i=0; i<3; i++) + { + goal_x[i] = target_pose.kinematic.position(0) + cos(PI*2.0/3.0*i)*(-link[2]); + goal_y[i] = target_pose.kinematic.position(1) + sin(PI*2.0/3.0*i)*(-link[2]); + goal_z[i] = target_pose.kinematic.position(2); + } + + // Pose difference for each set of two joints + for (int i=0; i<3; i++) + { + diff_x[i] = goal_x[i] - start_x[i]; + diff_y[i] = goal_y[i] - start_y[i]; + diff_z[i] = goal_z[i] - start_z[i]; + } + + for (int i=0; i<3; i++) + { + temp[i] = diff_x[i]*cos(-PI*2.0/3.0*i) - diff_y[i]*sin(-PI*2.0/3.0*i); + temp2[i] = diff_x[i]*sin(-PI*2.0/3.0*i) + diff_y[i]*cos(-PI*2.0/3.0*i); + temp3[i] = sqrt(pow(link[1],2) - pow(temp2[i],2)); + temp4[i] = sqrt(pow(temp[i],2) + pow(diff_z[i],2)); + temp5[i] = pow(temp3[i],2) - pow(temp4[i],2) - pow(link[0],2); + target_angle[i].position = asin(temp5[i] / (2*temp4[i]*link[0])) - acos(-diff_z[i]/temp4[i]); + } + + target_angle_vector.push_back(target_angle[0]); + target_angle_vector.push_back(target_angle[1]); + target_angle_vector.push_back(target_angle[2]); + + for (int i=0; i<3; i++){ + elbow_x[i] = start_x[i] + cos(PI*2.0/3.0*i)*link[0]*(-cos(target_angle[i].position)); + elbow_y[i] = start_y[i] + sin(PI*2.0/3.0*i)*link[0]*(-cos(target_angle[i].position)); + elbow_z[i] = start_z[i] + link[0]*sin(target_angle[i].position); + } + + // Pose difference for each set of two joints + for (int i=0; i<3; i++){ + diff_x2[i] = goal_x[i] - elbow_x[i]; + diff_y2[i] = goal_y[i] - elbow_y[i]; + diff_z2[i] = goal_z[i] - elbow_z[i]; + } + + for (int i=0; i<3; i++){ + temp6[i] = diff_x2[i]*sin(-PI*2.0/3.0*i) + diff_y2[i]*cos(-PI*2.0/3.0*i); + target_angle[2*i+4].position = asin(temp6[i] / link[1]); + target_angle[2*i+3].position = -PI + asin(-diff_z2[i] / cos(target_angle[2*i+4].position) / link[1]) - target_angle[i].position; + target_angle[2*i+3].position += PI*127.0/180.0; + } + + target_angle_vector.push_back(target_angle[3]); + target_angle_vector.push_back(target_angle[4]); + target_angle_vector.push_back(target_angle[5]); + target_angle_vector.push_back(target_angle[6]); + target_angle_vector.push_back(target_angle[7]); + target_angle_vector.push_back(target_angle[8]); + + target_angle[3].position -= PI*127.0/180.0; + + target_angle[9].position = -target_angle[4].position; + target_angle[10].position = -PI - target_angle[0].position - target_angle[3].position; + target_angle[10].position += PI*53.0/180.0; + + target_angle_vector.push_back(target_angle[9]); + target_angle_vector.push_back(target_angle[10]); + + *goal_joint_value = target_angle_vector; + manipulator->setAllJointValue(target_angle_vector); + + return true; +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs.h new file mode 100644 index 000000000..6b4f8bee4 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs.h @@ -0,0 +1 @@ +#include "linear_libs/include/linear_libs/linear.h" \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear.h new file mode 100644 index 000000000..3b0b02caf --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear.h @@ -0,0 +1,79 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef LINEAR_H_ +#define LINEAR_H_ + +#include "linear_custom_trajectory.h" +#include "linear_dynamixel.h" +#include "linear_kinematics.h" + +#define CUSTOM_TRAJECTORY_SIZE 4 +#define CUSTOM_TRAJECTORY_LINE "custom_trajectory_line" +#define CUSTOM_TRAJECTORY_CIRCLE "custom_trajectory_circle" +#define CUSTOM_TRAJECTORY_RHOMBUS "custom_trajectory_rhombus" +#define CUSTOM_TRAJECTORY_HEART "custom_trajectory_heart" + +#define DXL_SIZE 4 +#define JOINT_DYNAMIXEL "joint_dxl" +#define TOOL_DYNAMIXEL "tool_dxl" + +#define RECEIVE_RATE 0.100 // unit: s +#define CONTROL_RATE 0.010 // unit: s + +#define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) +#define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) +#define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) + +class Linear : public robotis_manipulator::RobotisManipulator +{ +private: + robotis_manipulator::Kinematics *kinematics_; + robotis_manipulator::JointActuator *joint_; + robotis_manipulator::ToolActuator *tool_; + robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE]; + + bool using_actual_robot_state_; + bool receive_data_flag_ = false; + double prev_receive_time_ = 0.0; + double prev_control_time_ = 0.0; + +public: + Linear(); + virtual ~Linear(); + + void initDebug(); + void initOpenManipulator(bool using_actual_robot_state, + STRING usb_port = "/dev/ttyUSB0", + STRING baud_rate = "1000000", + float control_rate = CONTROL_RATE); + void processOpenManipulator(double present_time); + + bool getUsingActualRobotState(); + bool getReceiveDataFlag(); + double getPrevReceiveTime(); + + void setReceiveDataFlag(bool receive_data_flag); + void setPrevReceiveTime(double prev_receive_time); +}; + +#endif // LINEAR_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_custom_trajectory.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_custom_trajectory.h new file mode 100644 index 000000000..24db96ef4 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_custom_trajectory.h @@ -0,0 +1,158 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef LINEAR_CUSTOM_TRAJECTORY_H_ +#define LINEAR_CUSTOM_TRAJECTORY_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace robotis_manipulator; +using namespace Eigen; + +namespace linear_custom_trajectory +{ + +enum AXIS{ + X_AXIS, + Y_AXIS, + Z_AXIS, +}; + +/***************************************************************************** +** Line +*****************************************************************************/ +class Line : public robotis_manipulator::CustomTaskTrajectory +{ +private: + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double acc_dec_time_; + double move_time_; + std::vector vel_max_; + +public: + Line() {} + virtual ~Line() {} + + void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta); + TaskWaypoint drawLine(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Circle +*****************************************************************************/ +class Circle : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Circle() {} + virtual ~Circle() {} + + void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawCircle(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +class Rhombus : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Rhombus() {} + virtual ~Rhombus() {} + + void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawRhombus(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Heart +*****************************************************************************/ +class Heart : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Heart() {} + virtual ~Heart() {} + + void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawHeart(double tick); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +} // namespace linear_custom_trajectory +#endif // LINEAR_CUSTOM_TRAJECTORY_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_dynamixel.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_dynamixel.h new file mode 100644 index 000000000..ba721df34 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_dynamixel.h @@ -0,0 +1,189 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef LINEAR_DYNAMIXEL_H_ +#define LINEAR_DYNAMIXEL_H_ + +#if defined(__OPENCR__) + #include + #include +#else + #include + #include +#endif + +namespace linear_dynamixel +{ + +#define SYNC_WRITE_HANDLER 0 +#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 + +//#define CONTROL_LOOP_TIME 10; //ms + +// Protocol 2.0 +#define ADDR_PRESENT_CURRENT_2 126 +#define ADDR_PRESENT_VELOCITY_2 128 +#define ADDR_PRESENT_POSITION_2 132 +#define ADDR_VELOCITY_TRAJECTORY_2 136 +#define ADDR_POSITION_TRAJECTORY_2 140 +#define ADDR_PROFILE_ACCELERATION_2 108 +#define ADDR_PROFILE_VELOCITY_2 112 +#define ADDR_GOAL_POSITION_2 116 + + +#define LENGTH_PRESENT_CURRENT_2 2 +#define LENGTH_PRESENT_VELOCITY_2 4 +#define LENGTH_PRESENT_POSITION_2 4 +#define LENGTH_VELOCITY_TRAJECTORY_2 4 +#define LENGTH_POSITION_TRAJECTORY_2 4 +#define LENGTH_PROFILE_ACCELERATION_2 4 +#define LENGTH_PROFILE_VELOCITY_2 4 +#define LENGTH_GOAL_POSITION_2 4 + + +// Protocol 1.0 +#define ADDR_PRESENT_CURRENT_1 = 40; +#define ADDR_PRESENT_VELOCITY_1 = 38; +#define ADDR_PRESENT_POSITION_1 = 36; + +#define LENGTH_PRESENT_CURRENT_1 = 2; +#define LENGTH_PRESENT_VELOCITY_1 = 2; +#define LENGTH_PRESENT_POSITION_1 = 2; + +typedef struct +{ + std::vector id; + uint8_t num; +} Joint; + +class JointDynamixel : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + +public: + JointDynamixel(){} + virtual ~JointDynamixel(){} + + + /***************************************************************************** + ** Joint Dynamixel Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalPosition(std::vector actuator_id, std::vector radian_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class JointDynamixelProfileControl : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + float control_loop_time_; // unit: ms + std::map previous_goal_value_; + +public: + JointDynamixelProfileControl(float control_loop_time = 0.010); + virtual ~JointDynamixelProfileControl(){} + + + /***************************************************************************** + ** Joint Dynamixel Profile Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class ToolDynamixel : public robotis_manipulator::ToolActuator +{ + private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + + public: + ToolDynamixel() {} + virtual ~ToolDynamixel() {} + + + /***************************************************************************** + ** Tool Dynamixel Control Functions + *****************************************************************************/ + virtual void init(uint8_t actuator_id, const void *arg); + virtual void setMode(const void *arg); + virtual uint8_t getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value); + virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue(); + + + /***************************************************************************** + ** Functions called in Tool Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(STRING dynamixel_mode = "position_mode"); + bool writeProfileValue(STRING profile_mode, uint32_t value); + bool setSDKHandler(); + bool writeGoalPosition(double radian); + double receiveDynamixelValue(); +}; + +} // namespace linear_dynamixel +#endif // LINEAR_DYNAMIXEL_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_kinematics.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_kinematics.h new file mode 100644 index 000000000..d291b1875 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/include/linear_libs/linear_kinematics.h @@ -0,0 +1,55 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef LINEAR_KINEMATICS_H_ +#define LINEAR_KINEMATICS_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace Eigen; +using namespace robotis_manipulator; + +namespace linear_kinematics +{ + +/***************************************************************************** +** Kinematics Solver Using CHain Rule and Geometry +*****************************************************************************/ +class SolverUsingGeometry : public robotis_manipulator::Kinematics +{ +private: + void forwardKinematicsSolverUsingGeometry(Manipulator *manipulator, Name component_name); + bool inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); + +public: + SolverUsingGeometry() {} + virtual ~SolverUsingGeometry() {} + + virtual void setOption(const void *arg); + virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); + virtual void solveForwardKinematics(Manipulator *manipulator); + virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); +}; + +} // namespace linear_kinematics + +#endif // LINEAR_KINEMATICS_H_ diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear.cpp new file mode 100644 index 000000000..08b17705e --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear.cpp @@ -0,0 +1,214 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/linear_libs/linear.h" + +Linear::Linear() {} +Linear::~Linear() +{ + delete kinematics_; + delete joint_; + delete tool_; + for(uint8_t index = 0; index < CUSTOM_TRAJECTORY_SIZE; index++) + delete custom_trajectory_[index]; +} + +void Linear::initDebug() +{ + DEBUG.begin(57600); // Using Serial4(=SerialBT2) + log::print("OpenManipulator Debugging Port"); +} + +void Linear::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_rate) +{ + /***************************************************************************** + ** Set if using actual robot + *****************************************************************************/ + using_actual_robot_state_ = using_actual_robot_state; + + /***************************************************************************** + ** Initialize Manipulator Parameters + *****************************************************************************/ + addWorld("world", // world name + "joint1"); // child name + + addJoint("joint1", // my name + "world", // parent name + "joint2", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 1, // actuator id + 5.0/3.0*M_PI, // max_limit + -5.0/3.0*M_PI); // min_limit + + + addJoint("joint2", // my name + "joint1", // parent name + "joint3", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 2, // actuator id + 5.0/3.0*M_PI, // max_limit + -5.0/3.0*M_PI); // min_limit + + + addJoint("joint3", // my name + "joint2", // parent name + "tool", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 3, // actuator id + 2.1*M_PI, // max_limit + -2.1*M_PI);// min_limit + + addTool("tool", // my name + "joint3", // parent name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + 4, // actuator id + 0.500, // max gripper limit + -0.500); // min gripper limit + + /***************************************************************************** + ** Initialize Kinematics + *****************************************************************************/ + kinematics_ = new linear_kinematics::SolverUsingGeometry(); + addKinematics(kinematics_); + + /***************************************************************************** + ** Initialize Custom Trajectory + *****************************************************************************/ + custom_trajectory_[0] = new linear_custom_trajectory::Line(); + custom_trajectory_[1] = new linear_custom_trajectory::Circle(); + custom_trajectory_[2] = new linear_custom_trajectory::Rhombus(); + custom_trajectory_[3] = new linear_custom_trajectory::Heart(); + + addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]); + addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]); + addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]); + addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]); + + if (using_actual_robot_state_) + { + /***************************************************************************** + ** Initialize ㅓoint Actuator + *****************************************************************************/ + joint_ = new linear_dynamixel::JointDynamixelProfileControl(control_rate); + + // Set communication arguments + STRING dxl_comm_arg[2] = {usb_port, baud_rate}; + void *p_dxl_comm_arg = &dxl_comm_arg; + + // Set joint actuator id + std::vector jointDxlId; + jointDxlId.push_back(1); + jointDxlId.push_back(2); + jointDxlId.push_back(3); + addJointActuator(JOINT_DYNAMIXEL, joint_, jointDxlId, p_dxl_comm_arg); + + // Set joint actuator control mode + STRING joint_dxl_mode_arg = "position_mode"; + void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; + setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); + + /***************************************************************************** + ** Initialize Tool Actuator + *****************************************************************************/ + tool_ = new linear_dynamixel::ToolDynamixel(); + + // Set tool actuator id + uint8_t toolDxlId = 4; + addToolActuator(TOOL_DYNAMIXEL, tool_, toolDxlId, p_dxl_comm_arg); + + // Set tool actuator control mode + STRING tool_dxl_mode_arg = "position_mode"; + void *p_tool_dxl_mode_arg = &tool_dxl_mode_arg; + setToolActuatorMode(TOOL_DYNAMIXEL, p_tool_dxl_mode_arg); + + /***************************************************************************** + ** Enable actuators and Receive actuator values + *****************************************************************************/ + // Enable All Actuators + enableAllActuator(); + + // Receive current angles from all actuators + receiveAllJointActuatorValue(); + receiveAllToolActuatorValue(); + } +} + +/***************************************************************************** +** Process actuator values received from external controllers +*****************************************************************************/ +void Linear::processOpenManipulator(double present_time) +{ + if (present_time - prev_control_time_ >= CONTROL_RATE) + { + JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time); + JointWaypoint goal_tool_value = getToolGoalValue(); + + if (using_actual_robot_state_) + { + receiveAllJointActuatorValue(); + receiveAllToolActuatorValue(); + } + + if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value); + if(goal_tool_value.size() != 0) sendAllToolActuatorValue(goal_tool_value); + solveForwardKinematics(); + + // Set previous control time + prev_control_time_ = millis()/1000.0; + } +} + +/***************************************************************************** +** State Functions +*****************************************************************************/ +// Check if using acutal robot +bool Linear::getUsingActualRobotState() +{ + return using_actual_robot_state_; +} + +/* Check if the program read data within control rate) */ +bool Linear::getReceiveDataFlag() +{ + return receive_data_flag_; +} + +/* Get the previous time when data were received */ +double Linear::getPrevReceiveTime() +{ + return prev_receive_time_; +} + +/* Set whether data were received or not */ +void Linear::setReceiveDataFlag(bool receive_data_flag) +{ + receive_data_flag_ = receive_data_flag; +} + +/* Set the previous time when data were received */ +void Linear::setPrevReceiveTime(double prev_receive_time) +{ + prev_receive_time_ = prev_receive_time; +} \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_custom_trajectory.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_custom_trajectory.cpp new file mode 100644 index 000000000..8c63a2edb --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_custom_trajectory.cpp @@ -0,0 +1,346 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/linear_libs/linear_custom_trajectory.h" + +using namespace linear_custom_trajectory; +using namespace Eigen; + + +/***************************************************************************** +** Line +*****************************************************************************/ +void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta) +{ + move_time_ = move_time; + acc_dec_time_ = move_time_ * 0.2; + vel_max_.resize(3); + + TaskWaypoint start_to_goal; + + start_pose_ = start; + + goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation; + goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position; + + vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_); +} + +TaskWaypoint Line::drawLine(double time_var) +{ + TaskWaypoint pose; + + if(acc_dec_time_ >= time_var) + { + pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var ) + { + pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_)) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + } + else if(time_var <= move_time_) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS); + } + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Line::getTaskWaypoint(double tick) +{ + return drawLine(tick); +} + + +void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + TaskWaypoint *c_arg = (TaskWaypoint *)arg; + initLine(move_time, start, c_arg[0]); +} +void Line::setOption(const void *arg) {} + + +/***************************************************************************** +** Circle +*****************************************************************************/ +void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Circle::drawCircle(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + + double diff_pose[2]; + + diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_); + diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Circle::getTaskWaypoint(double tick) +{ + return drawCircle(tick); +} + +void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +void Circle::setOption(const void *arg){} + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + + +TaskWaypoint Rhombus::drawRhombus(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + while(true) + { + if (get_time_var < PI*2) break; + get_time_var = get_time_var - PI*2; + } + + if (get_time_var >= 0 && get_time_var < PI/2){ + traj[0] = - get_time_var / (PI/2); + traj[1] = - get_time_var / (PI/2); + } else if (get_time_var >= PI/2 && get_time_var < PI){ + traj[0] = - get_time_var / (PI/2); + traj[1] = get_time_var / (PI/2) - 2; + } else if (get_time_var >= PI && get_time_var < PI*3/2){ + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = get_time_var / (PI/2) - 2; + } else { + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = - get_time_var / (PI/2) + 4; + } + + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + + +void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +TaskWaypoint Rhombus::getTaskWaypoint(double tick) +{ + return drawRhombus(tick); +} +void Rhombus::setOption(const void *arg){} + + +/***************************************************************************** +** Heart +*****************************************************************************/ +void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Heart::drawHeart(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + double shift_offset = - 5.0; + + traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16; + traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16; + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} +void Heart::setOption(const void *arg){} + +TaskWaypoint Heart::getTaskWaypoint(double tick) +{ + return drawHeart(tick); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_dynamixel.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_dynamixel.cpp new file mode 100644 index 000000000..851844b88 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_dynamixel.cpp @@ -0,0 +1,973 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/linear_libs/linear_dynamixel.h" + +using namespace linear_dynamixel; +using namespace robotis_manipulator; + +/***************************************************************************** +** Joint Dynamixel Control Functions +*****************************************************************************/ +void JointDynamixel::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixel::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixel::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixel::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixel::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixel::getId() +{ + return dynamixel_.id; +} + +void JointDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixel::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + std::vector radian_vector; + for(uint32_t index = 0; index < value_vector.size(); index++) + { + radian_vector.push_back(value_vector.at(index).position); + } + result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixel::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixel::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Control Functions +*****************************************************************************/ +bool JointDynamixel::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixel::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixel::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixel::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool JointDynamixel::writeGoalPosition(std::vector actuator_id, std::vector radian_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_position[actuator_id.size()]; + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + id_array[index] = actuator_id.at(index); + goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index)); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +std::vector JointDynamixel::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Joint Dynamixel Profile Control Functions +*****************************************************************************/ +JointDynamixelProfileControl::JointDynamixelProfileControl(float control_loop_time) +{ + control_loop_time_ = control_loop_time; +} + +void JointDynamixelProfileControl::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixelProfileControl::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixelProfileControl::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixelProfileControl::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixelProfileControl::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixelProfileControl::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixelProfileControl::getId() +{ + return dynamixel_.id; +} + +void JointDynamixelProfileControl::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixelProfileControl::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixelProfileControl::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + result = JointDynamixelProfileControl::writeGoalProfilingControlValue(actuator_id, value_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixelProfileControl::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixelProfileControl::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Profile Control Functions +*****************************************************************************/ +bool JointDynamixelProfileControl::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setTimeBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixelProfileControl::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3; + const uint32_t acceleration = uint32_t(control_loop_time_*1000); + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixelProfileControl::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixelProfileControl::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + return true; +} + +bool JointDynamixelProfileControl::writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_value[actuator_id.size()]; + + //add tarajectory eq. + for(uint8_t index = 0; index < actuator_id.size(); index++) + { + float result_position; + float time_control = control_loop_time_; //ms + + if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end()) + { + previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index))); + } + + result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2; + + id_array[index] = actuator_id.at(index); + goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position); + + previous_goal_value_[actuator_id.at(index)] = value_vector.at(index); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log); + if (result == false) + { + log::error(log); + } + return true; +} + +std::vector JointDynamixelProfileControl::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Tool Dynamixel Control Functions +*****************************************************************************/ +void ToolDynamixel::init(uint8_t actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = ToolDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void ToolDynamixel::setMode(const void *arg) +{ + bool result = false; +// const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = ToolDynamixel::setOperatingMode(get_arg_[0]); + if (result == false) + return; + } + else + { + result = ToolDynamixel::writeProfileValue(get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + + result = ToolDynamixel::setSDKHandler(); + if (result == false) + return; +} + +uint8_t ToolDynamixel::getId() +{ + return dynamixel_.id.at(0); +} + +void ToolDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = true; +} + +void ToolDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = false; +} + +bool ToolDynamixel::sendToolActuatorValue(robotis_manipulator::ActuatorValue value) +{ + return ToolDynamixel::writeGoalPosition(value.position); +} + +robotis_manipulator::ActuatorValue ToolDynamixel::receiveToolActuatorValue() +{ + robotis_manipulator::ActuatorValue result; + result.position = ToolDynamixel::receiveDynamixelValue(); + result.velocity = 0.0; + result.acceleration = 0.0; + result.effort = 0.0; + return result; +} + + +/***************************************************************************** +** Functions called in Tool Dynamixel Profile Control Functions +*****************************************************************************/ +bool ToolDynamixel::initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + const char* log = NULL; + bool result = false; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id.push_back(actuator_id); + dynamixel_.num = 1; + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Tool Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0)); + strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0))); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + + return true; +} + +bool ToolDynamixel::setOperatingMode(STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 200; + + if (dynamixel_mode == "position_mode") + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log); + if (result == false) + { + log::error(log); + } + } + else + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool ToolDynamixel::writeProfileValue(STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::setSDKHandler() +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0), "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0), + "Present_Position", + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::writeGoalPosition(double radian) +{ + bool result = false; + const char* log = NULL; + + int32_t goal_position = 0; + + goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian); + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, &goal_position, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +double ToolDynamixel::receiveDynamixelValue() +{ + bool result = false; + const char* log = NULL; + + int32_t get_value = 0; + uint8_t id_array[1] = {dynamixel_.id.at(0)}; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &get_value, + &log); + if (result == false) + { + log::error(log); + } + + return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_kinematics.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_kinematics.cpp new file mode 100644 index 000000000..030d2ab52 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/linear_libs/src/linear_kinematics.cpp @@ -0,0 +1,158 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/linear_libs/linear_kinematics.h" + +using namespace robotis_manipulator; +using namespace linear_kinematics; + +/***************************************************************************** +** Kinematics Solver +*****************************************************************************/ +void SolverUsingGeometry::setOption(const void *arg) {} + +Eigen::MatrixXd SolverUsingGeometry::jacobian(Manipulator *manipulator, Name tool_name) +{ + return Eigen::MatrixXd::Identity(6, manipulator->getDOF()); +} + +void SolverUsingGeometry::solveForwardKinematics(Manipulator *manipulator) +{ + forwardKinematicsSolverUsingGeometry(manipulator, manipulator->getWorldChildName()); +} + +bool SolverUsingGeometry::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + return inverseKinematicsSolverUsingGeometry(manipulator, tool_name, target_pose, goal_joint_value); +} + +/***************************************************************************** +** Private +*****************************************************************************/ +void SolverUsingGeometry::forwardKinematicsSolverUsingGeometry(Manipulator *manipulator, Name component_name) +{ + // // Get my name, parent name and number of children + // Name my_name = component_name; + // Name parent_name = manipulator->getComponentParentName(my_name); + // int8_t number_of_child = manipulator->getComponentChildName(my_name).size(); + + // // Define my & parent position and orientation + // Eigen::Vector3d parent_position_from_world; + // Eigen::Matrix3d parent_orientation_from_world; + // Eigen::Vector3d my_position_from_world; + // Eigen::Matrix3d my_orientation_from_world; + + + // // Get parent position and orientation + // // when parent is world + // if (parent_name == manipulator->getWorldName()) + // { + // parent_position_from_world = manipulator->getWorldPosition(); + // parent_orientation_from_world = manipulator->getWorldOrientation(); + // } + // // when parent is not world + // else + // { + // parent_position_from_world = manipulator->getComponentPositionFromWorld(parent_name); + // parent_orientation_from_world = manipulator->getComponentOrientationFromWorld(parent_name); + // } + + // Calculate my position and orientation + double wheel_radius = 0.02818; + double motor_angle[4]; + // std::vector child_name = manipulator->getComponentChildName(my_name); + + // for (int8_t index=0; index<2; index++) + // { + motor_angle[0] = manipulator->getJointValue("joint1").position; + motor_angle[1] = manipulator->getJointValue("joint2").position; + motor_angle[2] = manipulator->getJointValue("joint3").position; + motor_angle[3] = manipulator->getJointValue("tool").position; + // } + + Eigen::Vector3d pos_joint1; + Eigen::Vector3d pos_joint2; + Eigen::Vector3d pos_joint3; + Eigen::Vector3d pos_joint4; + + pos_joint1 << wheel_radius * motor_angle[0], 0 , 0; + pos_joint2 << wheel_radius * motor_angle[0], wheel_radius * motor_angle[1], 0; + pos_joint3 << wheel_radius * motor_angle[0], wheel_radius * motor_angle[1], -0,01; + pos_joint4 << wheel_radius * motor_angle[0], wheel_radius * motor_angle[1], wheel_radius * motor_angle[2]; + + // Set my position and orientation + manipulator->setComponentPositionFromWorld("joint1", pos_joint1); + manipulator->setComponentPositionFromWorld("joint2", pos_joint2); + manipulator->setComponentPositionFromWorld("joint3", pos_joint3); + manipulator->setComponentPositionFromWorld("tool", pos_joint4); + + Eigen::Vector3d pos_joint11, pos_joint22, pos_joint33; + pos_joint11 = manipulator->getComponentPositionFromWorld("joint1"); + pos_joint22 = manipulator->getComponentPositionFromWorld("joint2"); + pos_joint33 = manipulator->getComponentPositionFromWorld("joint3"); + + // Serial.print(pos_joint11(0)); + // Serial.print(pos_joint11(1)); + // Serial.print(pos_joint11(2)); + // Serial.print(pos_joint22(0)); + // Serial.print(pos_joint22(1)); + // Serial.print(pos_joint22(2)); + // Serial.print(pos_joint33(0)); + // Serial.print(pos_joint33(1)); + // Serial.println(pos_joint33(2)); + // Serial.flush(); + +} + +bool SolverUsingGeometry::inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + JointValue target_angle[3]; + std::vector target_joint_value; + double x_limit = 0.300; + double y_limit = 0.300; + double z_limit = 0.300; + double wheel_radius = 0.02818; + + Manipulator _manipulator = *manipulator; + + // // Set x, y, z limits + // if (target_pose.position(0) > x_limit) target_pose.position(0) = x_limit; + // if (target_pose.position(0) < -x_limit) target_pose.position(0) = -x_limit; + // if (target_pose.position(1) > y_limit) target_pose.position(1) = y_limit; + // if (target_pose.position(1) < -y_limit) target_pose.position(1) = -y_limit; + // if (target_pose.position(2) > z_limit) target_pose.position(1) = z_limit; + // if (target_pose.position(2) < 0 ) target_pose.position(1) = 0; + + for (int8_t index=0; index<3; index++) + { + // Calculate the target pose of each joint. + target_angle[index].position = target_pose.kinematic.position(index) / wheel_radius; + + // Calculate the target pose of each joint. + target_joint_value.push_back(target_angle[index]); + // *goal_joint_value.push_back(target_angle[index]); <-- can be like this??? + } + *goal_joint_value = target_joint_value; + + return true; +} + + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs.h new file mode 100644 index 000000000..09e520093 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs.h @@ -0,0 +1 @@ +#include "planar_libs/include/planar_libs/planar.h" diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar.h new file mode 100644 index 000000000..be7dbb194 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar.h @@ -0,0 +1,77 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef PLANAR_H_ +#define PLANAR_H_ + +#include "planar_custom_trajectory.h" +#include "planar_dynamixel.h" +#include "planar_kinematics.h" + +#define CUSTOM_TRAJECTORY_SIZE 4 +#define CUSTOM_TRAJECTORY_LINE "custom_trajectory_line" +#define CUSTOM_TRAJECTORY_CIRCLE "custom_trajectory_circle" +#define CUSTOM_TRAJECTORY_RHOMBUS "custom_trajectory_rhombus" +#define CUSTOM_TRAJECTORY_HEART "custom_trajectory_heart" + +#define DXL_SIZE 3 +#define JOINT_DYNAMIXEL "joint_dxl" + +#define RECEIVE_RATE 0.100 //s +#define CONTROL_RATE 0.010 //s + +#define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) +#define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) +#define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) + +class Planar : public robotis_manipulator::RobotisManipulator +{ +private: + robotis_manipulator::Kinematics *kinematics_; + robotis_manipulator::JointActuator *joint_; + robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE]; + + bool using_actual_robot_state_; + bool receive_data_flag_ = false; + double prev_receive_time_ = 0.0; + double prev_control_time_ = 0.0; + +public: + Planar(); + virtual ~Planar(); + + void initDebug(); + void initOpenManipulator(bool using_actual_robot_state, + STRING usb_port = "/dev/ttyUSB0", + STRING baud_rate = "1000000", + float control_rate = CONTROL_RATE); + void processOpenManipulator(double present_time); + + bool getUsingActualRobotState(); + bool getReceiveDataFlag(); + double getPrevReceiveTime(); + + void setReceiveDataFlag(bool receive_data_flag); + void setPrevReceiveTime(double prev_receive_time); +}; + +#endif // PLANAR_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_custom_trajectory.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_custom_trajectory.h new file mode 100644 index 000000000..6c75fafc2 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_custom_trajectory.h @@ -0,0 +1,159 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef PLANAR_CUSTOM_TRAJECTORY_H_ +#define PLANAR_CUSTOM_TRAJECTORY_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace robotis_manipulator; +using namespace Eigen; + +namespace planar_custom_trajectory +{ + +enum AXIS{ + X_AXIS, + Y_AXIS, + Z_AXIS, +}; + + +/***************************************************************************** +** Line +*****************************************************************************/ +class Line : public robotis_manipulator::CustomTaskTrajectory +{ +private: + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double acc_dec_time_; + double move_time_; + std::vector vel_max_; + +public: + Line() {} + virtual ~Line() {} + + void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta); + TaskWaypoint drawLine(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Circle +*****************************************************************************/ +class Circle : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Circle() {} + virtual ~Circle() {} + + void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawCircle(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +class Rhombus : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Rhombus() {} + virtual ~Rhombus() {} + + void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawRhombus(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Heart +*****************************************************************************/ +class Heart : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Heart() {} + virtual ~Heart() {} + + void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawHeart(double tick); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +} // namespace planar_custom_trajectory +#endif // PLANAR_CUSTOM_TRAJECTORY_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_dynamixel.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_dynamixel.h new file mode 100644 index 000000000..6eb1c878c --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_dynamixel.h @@ -0,0 +1,189 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef PLANAR_DYNAMIXEL_H_ +#define PLANAR_DYNAMIXEL_H_ + +#if defined(__OPENCR__) + #include + #include +#else + #include + #include +#endif + +namespace planar_dynamixel +{ + +#define SYNC_WRITE_HANDLER 0 +#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 + +//#define CONTROL_LOOP_TIME 10; //ms + +// Protocol 2.0 +#define ADDR_PRESENT_CURRENT_2 126 +#define ADDR_PRESENT_VELOCITY_2 128 +#define ADDR_PRESENT_POSITION_2 132 +#define ADDR_VELOCITY_TRAJECTORY_2 136 +#define ADDR_POSITION_TRAJECTORY_2 140 +#define ADDR_PROFILE_ACCELERATION_2 108 +#define ADDR_PROFILE_VELOCITY_2 112 +#define ADDR_GOAL_POSITION_2 116 + + +#define LENGTH_PRESENT_CURRENT_2 2 +#define LENGTH_PRESENT_VELOCITY_2 4 +#define LENGTH_PRESENT_POSITION_2 4 +#define LENGTH_VELOCITY_TRAJECTORY_2 4 +#define LENGTH_POSITION_TRAJECTORY_2 4 +#define LENGTH_PROFILE_ACCELERATION_2 4 +#define LENGTH_PROFILE_VELOCITY_2 4 +#define LENGTH_GOAL_POSITION_2 4 + + +// Protocol 1.0 +#define ADDR_PRESENT_CURRENT_1 = 40; +#define ADDR_PRESENT_VELOCITY_1 = 38; +#define ADDR_PRESENT_POSITION_1 = 36; + +#define LENGTH_PRESENT_CURRENT_1 = 2; +#define LENGTH_PRESENT_VELOCITY_1 = 2; +#define LENGTH_PRESENT_POSITION_1 = 2; + +typedef struct +{ + std::vector id; + uint8_t num; +} Joint; + +class JointDynamixel : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + +public: + JointDynamixel(){} + virtual ~JointDynamixel(){} + + + /***************************************************************************** + ** Joint Dynamixel Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalPosition(std::vector actuator_id, std::vector radian_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class JointDynamixelProfileControl : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + float control_loop_time_; // unit: ms + std::map previous_goal_value_; + +public: + JointDynamixelProfileControl(float control_loop_time = 0.010); + virtual ~JointDynamixelProfileControl(){} + + + /***************************************************************************** + ** Joint Dynamixel Profile Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class ToolDynamixel : public robotis_manipulator::ToolActuator +{ + private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + + public: + ToolDynamixel() {} + virtual ~ToolDynamixel() {} + + + /***************************************************************************** + ** Tool Dynamixel Control Functions + *****************************************************************************/ + virtual void init(uint8_t actuator_id, const void *arg); + virtual void setMode(const void *arg); + virtual uint8_t getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value); + virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue(); + + + /***************************************************************************** + ** Functions called in Tool Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(STRING dynamixel_mode = "position_mode"); + bool writeProfileValue(STRING profile_mode, uint32_t value); + bool setSDKHandler(); + bool writeGoalPosition(double radian); + double receiveDynamixelValue(); +}; + +} // namespace planar_dynamixel +#endif // PLANAR_DYNAMIXEL_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_kinematics.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_kinematics.h new file mode 100644 index 000000000..dab6debc9 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/include/planar_libs/planar_kinematics.h @@ -0,0 +1,54 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef PLANAR_KINEMATICS_H_ +#define PLANAR_KINEMATICS_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace Eigen; +using namespace robotis_manipulator; + +namespace planar_kinematics +{ + +/***************************************************************************** +** Kinematics Solver Using Geometry +*****************************************************************************/ +class SolverUsingGeometry : public robotis_manipulator::Kinematics +{ +private: + bool inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); + +public: + SolverUsingGeometry(){} + virtual ~SolverUsingGeometry(){} + + virtual void setOption(const void *arg); + virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); + virtual void solveForwardKinematics(Manipulator *manipulator); + virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); +}; + +} // namespace planar_kinematics + +#endif // PLANAR_KINEMATICS_H_ diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar.cpp new file mode 100644 index 000000000..9dbffd492 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar.cpp @@ -0,0 +1,215 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/planar_libs/planar.h" + +Planar::Planar() {} +Planar::~Planar() +{ + delete kinematics_; + delete joint_; + for(uint8_t index = 0; index < CUSTOM_TRAJECTORY_SIZE; index++) + delete custom_trajectory_[index]; +} + +void Planar::initDebug() +{ + DEBUG.begin(57600); // Using Serial4(= SerialBT2) + log::print("OpenManipulator Debugging Port"); +} + +void Planar::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_rate) +{ + /***************************************************************************** + ** Set if using actual robot + *****************************************************************************/ + using_actual_robot_state_ = using_actual_robot_state; + + /***************************************************************************** + ** Initialize Manipulator Parameters + *****************************************************************************/ + addWorld("world", // world name + "joint1"); // child name + + addJoint("joint1", // my name + "world", // parent name + "joint4", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 1); // actuator id + + addJoint("joint2", // my name + "world", // parent name + "joint5", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Y_AXIS, // axis of rotation + 2); // actuator id + + addJoint("joint3", // my name + "world", // parent name + "joint6", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Y_AXIS, // axis of rotation + 3); // actuator id + + addJoint("joint4", // my name + "joint1", // parent name + "joint7", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Y_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint5", // my name + "joint2", // parent name + "joint8", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Y_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint6", // my name + "joint3", // parent name + "joint9", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Y_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint7", // my name + "joint4", // parent name + "tool", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Y_AXIS, // axis of rotation + -1); // actuator id + + addTool("tool", // my name + "joint7", // Not used + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + -1); // actuator id + + /***************************************************************************** + ** Initialize Kinematics + *****************************************************************************/ + kinematics_ = new planar_kinematics::SolverUsingGeometry(); + addKinematics(kinematics_); + + /***************************************************************************** + ** Initialize Custom Trajectory + *****************************************************************************/ + custom_trajectory_[0] = new planar_custom_trajectory::Line(); + custom_trajectory_[1] = new planar_custom_trajectory::Circle(); + custom_trajectory_[2] = new planar_custom_trajectory::Rhombus(); + custom_trajectory_[3] = new planar_custom_trajectory::Heart(); + + addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]); + addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]); + addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]); + addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]); + + if (using_actual_robot_state_) + { + /***************************************************************************** + ** Initialize ㅓoint Actuator + *****************************************************************************/ + joint_ = new planar_dynamixel::JointDynamixelProfileControl(control_rate); + + // Set communication arguments + STRING dxl_comm_arg[2] = {usb_port, baud_rate}; + void *p_dxl_comm_arg = &dxl_comm_arg; + + // Set joint actuator id + std::vector jointDxlId; + jointDxlId.push_back(1); + jointDxlId.push_back(2); + jointDxlId.push_back(3); + addJointActuator(JOINT_DYNAMIXEL, joint_, jointDxlId, p_dxl_comm_arg); + + // set joint actuator parameter + STRING joint_dxl_mode_arg = "position_mode"; + void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; + setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); + + /***************************************************************************** + ** Enable actuators and Receive actuator values + *****************************************************************************/ + // Eanble all actuators + enableAllActuator(); + + // Receive current angles from all actuators + receiveAllJointActuatorValue(); + } +} + +/***************************************************************************** +** Process actuator values received from external controllers +*****************************************************************************/ +void Planar::processOpenManipulator(double present_time) +{ + if (present_time - prev_control_time_ >= CONTROL_RATE) + { + JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time); + if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value); + + // Set previous control time + prev_control_time_ = millis()/1000.0; + } +} + +/***************************************************************************** +** State Functions +*****************************************************************************/ +// Check if using acutal robot +bool Planar::getUsingActualRobotState() +{ + return using_actual_robot_state_; +} + +/* Check if the program read data within control rate) */ +bool Planar::getReceiveDataFlag() +{ + return receive_data_flag_; +} + +/* Get the previous time when data were received */ +double Planar::getPrevReceiveTime() +{ + return prev_receive_time_; +} + +/* Set whether data were received or not */ +void Planar::setReceiveDataFlag(bool receive_data_flag) +{ + receive_data_flag_ = receive_data_flag; +} + +/* Set the previous time when data were received */ +void Planar::setPrevReceiveTime(double prev_receive_time) +{ + prev_receive_time_ = prev_receive_time; +} + + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_custom_trajectory.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_custom_trajectory.cpp new file mode 100644 index 000000000..2e4a73e63 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_custom_trajectory.cpp @@ -0,0 +1,346 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/planar_libs/planar_custom_trajectory.h" + +using namespace planar_custom_trajectory; +using namespace Eigen; + + +/***************************************************************************** +** Line +*****************************************************************************/ +void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta) +{ + move_time_ = move_time; + acc_dec_time_ = move_time_ * 0.2; + vel_max_.resize(3); + + TaskWaypoint start_to_goal; + + start_pose_ = start; + + goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation; + goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position; + + vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_); +} + +TaskWaypoint Line::drawLine(double time_var) +{ + TaskWaypoint pose; + + if(acc_dec_time_ >= time_var) + { + pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var ) + { + pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_)) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + } + else if(time_var <= move_time_) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS); + } + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Line::getTaskWaypoint(double tick) +{ + return drawLine(tick); +} + + +void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + TaskWaypoint *c_arg = (TaskWaypoint *)arg; + initLine(move_time, start, c_arg[0]); +} +void Line::setOption(const void *arg) {} + + +/***************************************************************************** +** Circle +*****************************************************************************/ +void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Circle::drawCircle(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + + double diff_pose[2]; + + diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_); + diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Circle::getTaskWaypoint(double tick) +{ + return drawCircle(tick); +} + +void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +void Circle::setOption(const void *arg){} + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + + +TaskWaypoint Rhombus::drawRhombus(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + while(true) + { + if (get_time_var < PI*2) break; + get_time_var = get_time_var - PI*2; + } + + if (get_time_var >= 0 && get_time_var < PI/2){ + traj[0] = - get_time_var / (PI/2); + traj[1] = - get_time_var / (PI/2); + } else if (get_time_var >= PI/2 && get_time_var < PI){ + traj[0] = - get_time_var / (PI/2); + traj[1] = get_time_var / (PI/2) - 2; + } else if (get_time_var >= PI && get_time_var < PI*3/2){ + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = get_time_var / (PI/2) - 2; + } else { + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = - get_time_var / (PI/2) + 4; + } + + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + + +void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +TaskWaypoint Rhombus::getTaskWaypoint(double tick) +{ + return drawRhombus(tick); +} +void Rhombus::setOption(const void *arg){} + + +/***************************************************************************** +** Heart +*****************************************************************************/ +void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Heart::drawHeart(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + double shift_offset = - 5.0; + + traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16; + traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16; + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} +void Heart::setOption(const void *arg){} + +TaskWaypoint Heart::getTaskWaypoint(double tick) +{ + return drawHeart(tick); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_dynamixel.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_dynamixel.cpp new file mode 100644 index 000000000..341c04c32 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_dynamixel.cpp @@ -0,0 +1,973 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/planar_libs/planar_dynamixel.h" + +using namespace planar_dynamixel; +using namespace robotis_manipulator; + +/***************************************************************************** +** Joint Dynamixel Control Functions +*****************************************************************************/ +void JointDynamixel::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixel::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixel::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixel::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixel::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixel::getId() +{ + return dynamixel_.id; +} + +void JointDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixel::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + std::vector radian_vector; + for(uint32_t index = 0; index < value_vector.size(); index++) + { + radian_vector.push_back(value_vector.at(index).position); + } + result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixel::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixel::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Control Functions +*****************************************************************************/ +bool JointDynamixel::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixel::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixel::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixel::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool JointDynamixel::writeGoalPosition(std::vector actuator_id, std::vector radian_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_position[actuator_id.size()]; + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + id_array[index] = actuator_id.at(index); + goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index)); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +std::vector JointDynamixel::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Joint Dynamixel Profile Control Functions +*****************************************************************************/ +JointDynamixelProfileControl::JointDynamixelProfileControl(float control_loop_time) +{ + control_loop_time_ = control_loop_time; +} + +void JointDynamixelProfileControl::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixelProfileControl::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixelProfileControl::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixelProfileControl::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixelProfileControl::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixelProfileControl::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixelProfileControl::getId() +{ + return dynamixel_.id; +} + +void JointDynamixelProfileControl::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixelProfileControl::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixelProfileControl::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + result = JointDynamixelProfileControl::writeGoalProfilingControlValue(actuator_id, value_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixelProfileControl::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixelProfileControl::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Profile Control Functions +*****************************************************************************/ +bool JointDynamixelProfileControl::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setTimeBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixelProfileControl::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3; + const uint32_t acceleration = uint32_t(control_loop_time_*1000); + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixelProfileControl::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixelProfileControl::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + return true; +} + +bool JointDynamixelProfileControl::writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_value[actuator_id.size()]; + + //add tarajectory eq. + for(uint8_t index = 0; index < actuator_id.size(); index++) + { + float result_position; + float time_control = control_loop_time_; //ms + + if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end()) + { + previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index))); + } + + result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2; + + id_array[index] = actuator_id.at(index); + goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position); + + previous_goal_value_[actuator_id.at(index)] = value_vector.at(index); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log); + if (result == false) + { + log::error(log); + } + return true; +} + +std::vector JointDynamixelProfileControl::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Tool Dynamixel Control Functions +*****************************************************************************/ +void ToolDynamixel::init(uint8_t actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = ToolDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void ToolDynamixel::setMode(const void *arg) +{ + bool result = false; +// const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = ToolDynamixel::setOperatingMode(get_arg_[0]); + if (result == false) + return; + } + else + { + result = ToolDynamixel::writeProfileValue(get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + + result = ToolDynamixel::setSDKHandler(); + if (result == false) + return; +} + +uint8_t ToolDynamixel::getId() +{ + return dynamixel_.id.at(0); +} + +void ToolDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = true; +} + +void ToolDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = false; +} + +bool ToolDynamixel::sendToolActuatorValue(robotis_manipulator::ActuatorValue value) +{ + return ToolDynamixel::writeGoalPosition(value.position); +} + +robotis_manipulator::ActuatorValue ToolDynamixel::receiveToolActuatorValue() +{ + robotis_manipulator::ActuatorValue result; + result.position = ToolDynamixel::receiveDynamixelValue(); + result.velocity = 0.0; + result.acceleration = 0.0; + result.effort = 0.0; + return result; +} + + +/***************************************************************************** +** Functions called in Tool Dynamixel Profile Control Functions +*****************************************************************************/ +bool ToolDynamixel::initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + const char* log = NULL; + bool result = false; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id.push_back(actuator_id); + dynamixel_.num = 1; + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Tool Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0)); + strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0))); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + + return true; +} + +bool ToolDynamixel::setOperatingMode(STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 200; + + if (dynamixel_mode == "position_mode") + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log); + if (result == false) + { + log::error(log); + } + } + else + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool ToolDynamixel::writeProfileValue(STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::setSDKHandler() +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0), "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0), + "Present_Position", + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::writeGoalPosition(double radian) +{ + bool result = false; + const char* log = NULL; + + int32_t goal_position = 0; + + goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian); + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, &goal_position, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +double ToolDynamixel::receiveDynamixelValue() +{ + bool result = false; + const char* log = NULL; + + int32_t get_value = 0; + uint8_t id_array[1] = {dynamixel_.id.at(0)}; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &get_value, + &log); + if (result == false) + { + log::error(log); + } + + return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_kinematics.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_kinematics.cpp new file mode 100644 index 000000000..454d4738c --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/planar_libs/src/planar_kinematics.cpp @@ -0,0 +1,125 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/planar_libs/planar_kinematics.h" + +using namespace robotis_manipulator; +using namespace planar_kinematics; + +/***************************************************************************** +** Kinematics Solver +*****************************************************************************/ +void SolverUsingGeometry::setOption(const void *arg) {} + +Eigen::MatrixXd SolverUsingGeometry::jacobian(Manipulator *manipulator, Name tool_name) +{ + return Eigen::MatrixXd::Identity(6, manipulator->getDOF()); +} + +void SolverUsingGeometry::solveForwardKinematics(Manipulator *manipulator) {} + +bool SolverUsingGeometry::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + return inverseKinematicsSolverUsingGeometry(manipulator, tool_name, target_pose, goal_joint_value); +} + +/***************************************************************************** +** Private +*****************************************************************************/ +bool SolverUsingGeometry::inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value) +{ + const double link[3] = {0.120, 0.098, 0.0366}; + JointValue target_angle[7]; + std::vector target_angle_vector; + double start_x[3], start_y[3]; + double temp_x[3], temp_y[3]; + double goal_x[3], goal_y[3]; + double diff_x[3], diff_y[3]; + Matrix3d goal_orientation; + double target_pose_length[3]; + double alpha[3]; + double temp_diff[3]; + + // Start pose for each set of two joints + for (uint8_t i=0; i<3; i++) + { + start_x[i] = cos(PI*2.0/3.0*i)*(-0.1705); + start_y[i] = sin(PI*2.0/3.0*i)*(-0.1705); + } + + // Goal pose for each set of two joints after tool rotation + for (uint8_t i=0; i<3; i++){ + temp_x[i] = target_pose.kinematic.position(0) + cos(PI*2.0/3.0*i)*(-link[2]); + temp_y[i] = target_pose.kinematic.position(1) + sin(PI*2.0/3.0*i)*(-link[2]); + } + + goal_orientation = target_pose.kinematic.orientation; + if (goal_orientation(0,0) || goal_orientation(0,1) || goal_orientation(0,2) + || goal_orientation(1,0) || goal_orientation(1,1) || goal_orientation(1,2) + || goal_orientation(2,0) || goal_orientation(2,1) || goal_orientation(2,2)) + { + goal_orientation(0,0) = 1; + goal_orientation(1,1) = 1; + goal_orientation(2,2) = 1; + } + for (uint8_t i=0; i<3; i++) + { + goal_x[i] = goal_orientation(0,0)*temp_x[i] + goal_orientation(0,1)*temp_y[i]; + goal_y[i] = goal_orientation(1,0)*temp_x[i] + goal_orientation(1,1)*temp_y[i]; + diff_x[i] = goal_x[i] - start_x[i]; + diff_y[i] = goal_y[i] - start_y[i]; + target_pose_length[i] = sqrt(diff_x[i]*diff_x[i] + diff_y[i]*diff_y[i]); + } + + // Compute the Length of Position Difference and Target Angle + for (uint8_t i=0; i<3; i++) + { + alpha[i] = acos((target_pose_length[i]*target_pose_length[i] + link[0]*link[0] - link[1]*link[1]) + / (2*target_pose_length[i]*link[0])); + temp_diff[i] = sin(-PI*2.0/3.0*i)*diff_x[i] + cos(-PI*2.0/3.0*i)*diff_y[i]; + target_angle[i].position = acos(-temp_diff[i] / target_pose_length[i]) - alpha[i] - PI/4.0; + } + + // Set Joint Angle + target_angle_vector.push_back(target_angle[0]); + target_angle_vector.push_back(target_angle[1]); + target_angle_vector.push_back(target_angle[2]); + + for (uint8_t i=0; i<3; i++) + { + target_angle[i].position += PI/4.0; + target_angle[i+3].position = acos(-(temp_diff[i] - (-link[0]*cos(target_angle[i].position))) / link[1]) - target_angle[i].position - PI*7.0/12.0; + target_angle[i+3].position = acos((-sin(PI*2.0/3.0*i)*diff_x[i] + cos(PI*2.0/3.0*i)*diff_y[i] + + link[0]*cos(target_angle[i].position)) / -link[1]) - target_angle[i].position - PI*7.0/12.0; + } + + target_angle_vector.push_back(target_angle[3]); + target_angle_vector.push_back(target_angle[4]); + target_angle_vector.push_back(target_angle[5]); + + target_angle[3].position += PI*7.0/12.0; + + target_angle[6].position = PI/2.0 - target_angle[0].position - target_angle[3].position + PI/3.0; + + target_angle_vector.push_back(target_angle[6]); + + *goal_joint_value = target_angle_vector; + manipulator->setAllJointValue(target_angle_vector); + + return true; +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs.h new file mode 100644 index 000000000..3075ab6ab --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs.h @@ -0,0 +1 @@ +#include "scara_libs/include/scara_libs/scara.h" \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara.h new file mode 100644 index 000000000..a5c12331e --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara.h @@ -0,0 +1,79 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef SCARA_H_ +#define SCARA_H_ + +#include "scara_custom_trajectory.h" +#include "scara_dynamixel.h" +#include "scara_kinematics.h" + +#define CUSTOM_TRAJECTORY_SIZE 4 +#define CUSTOM_TRAJECTORY_LINE "custom_trajectory_line" +#define CUSTOM_TRAJECTORY_CIRCLE "custom_trajectory_circle" +#define CUSTOM_TRAJECTORY_RHOMBUS "custom_trajectory_rhombus" +#define CUSTOM_TRAJECTORY_HEART "custom_trajectory_heart" + +#define DXL_SIZE 4 +#define JOINT_DYNAMIXEL "joint_dxl" +#define TOOL_DYNAMIXEL "tool_dxl" + +#define RECEIVE_RATE 0.100 // unit: s +#define CONTROL_RATE 0.010 // unit: s + +#define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) +#define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) +#define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) + +class Scara : public robotis_manipulator::RobotisManipulator +{ +private: + robotis_manipulator::Kinematics *kinematics_; + robotis_manipulator::JointActuator *joint_; + robotis_manipulator::ToolActuator *tool_; + robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE]; + + bool using_actual_robot_state_; + bool receive_data_flag_ = false; + double prev_receive_time_ = 0.0; + double prev_control_time_ = 0.0; + +public: + Scara(); + virtual ~Scara(); + + void initDebug(); + void initOpenManipulator(bool using_actual_robot_state, + STRING usb_port = "/dev/ttyUSB0", + STRING baud_rate = "1000000", + float control_rate = CONTROL_RATE); + void processOpenManipulator(double present_time); + + bool getUsingActualRobotState(); + bool getReceiveDataFlag(); + double getPrevReceiveTime(); + + void setReceiveDataFlag(bool receive_data_flag); + void setPrevReceiveTime(double prev_receive_time); +}; + +#endif // SCARA_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_custom_trajectory.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_custom_trajectory.h new file mode 100644 index 000000000..e5864d3df --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_custom_trajectory.h @@ -0,0 +1,158 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef SCARA_CUSTOM_TRAJECTORY_H_ +#define SCARA_CUSTOM_TRAJECTORY_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace robotis_manipulator; +using namespace Eigen; + +namespace scara_custom_trajectory +{ + +enum AXIS{ + X_AXIS, + Y_AXIS, + Z_AXIS, +}; + +/***************************************************************************** +** Line +*****************************************************************************/ +class Line : public robotis_manipulator::CustomTaskTrajectory +{ +private: + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double acc_dec_time_; + double move_time_; + std::vector vel_max_; + +public: + Line() {} + virtual ~Line() {} + + void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta); + TaskWaypoint drawLine(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Circle +*****************************************************************************/ +class Circle : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Circle() {} + virtual ~Circle() {} + + void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawCircle(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +class Rhombus : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Rhombus() {} + virtual ~Rhombus() {} + + void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawRhombus(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Heart +*****************************************************************************/ +class Heart : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Heart() {} + virtual ~Heart() {} + + void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawHeart(double tick); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +} // namespace scara_custom_trajectory +#endif // SCARA_CUSTOM_TRAJECTORY_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_dynamixel.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_dynamixel.h new file mode 100644 index 000000000..f8c79706f --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_dynamixel.h @@ -0,0 +1,189 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef SCARA_DYNAMIXEL_H_ +#define SCARA_DYNAMIXEL_H_ + +#if defined(__OPENCR__) + #include + #include +#else + #include + #include +#endif + +namespace scara_dynamixel +{ + +#define SYNC_WRITE_HANDLER 0 +#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 + +//#define CONTROL_LOOP_TIME 10; //ms + +// Protocol 2.0 +#define ADDR_PRESENT_CURRENT_2 126 +#define ADDR_PRESENT_VELOCITY_2 128 +#define ADDR_PRESENT_POSITION_2 132 +#define ADDR_VELOCITY_TRAJECTORY_2 136 +#define ADDR_POSITION_TRAJECTORY_2 140 +#define ADDR_PROFILE_ACCELERATION_2 108 +#define ADDR_PROFILE_VELOCITY_2 112 +#define ADDR_GOAL_POSITION_2 116 + + +#define LENGTH_PRESENT_CURRENT_2 2 +#define LENGTH_PRESENT_VELOCITY_2 4 +#define LENGTH_PRESENT_POSITION_2 4 +#define LENGTH_VELOCITY_TRAJECTORY_2 4 +#define LENGTH_POSITION_TRAJECTORY_2 4 +#define LENGTH_PROFILE_ACCELERATION_2 4 +#define LENGTH_PROFILE_VELOCITY_2 4 +#define LENGTH_GOAL_POSITION_2 4 + + +// Protocol 1.0 +#define ADDR_PRESENT_CURRENT_1 = 40; +#define ADDR_PRESENT_VELOCITY_1 = 38; +#define ADDR_PRESENT_POSITION_1 = 36; + +#define LENGTH_PRESENT_CURRENT_1 = 2; +#define LENGTH_PRESENT_VELOCITY_1 = 2; +#define LENGTH_PRESENT_POSITION_1 = 2; + +typedef struct +{ + std::vector id; + uint8_t num; +} Joint; + +class JointDynamixel : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + +public: + JointDynamixel(){} + virtual ~JointDynamixel(){} + + + /***************************************************************************** + ** Joint Dynamixel Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalPosition(std::vector actuator_id, std::vector radian_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class JointDynamixelProfileControl : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + float control_loop_time_; // unit: ms + std::map previous_goal_value_; + +public: + JointDynamixelProfileControl(float control_loop_time = 0.010); + virtual ~JointDynamixelProfileControl(){} + + + /***************************************************************************** + ** Joint Dynamixel Profile Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class ToolDynamixel : public robotis_manipulator::ToolActuator +{ + private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + + public: + ToolDynamixel() {} + virtual ~ToolDynamixel() {} + + + /***************************************************************************** + ** Tool Dynamixel Control Functions + *****************************************************************************/ + virtual void init(uint8_t actuator_id, const void *arg); + virtual void setMode(const void *arg); + virtual uint8_t getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value); + virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue(); + + + /***************************************************************************** + ** Functions called in Tool Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(STRING dynamixel_mode = "position_mode"); + bool writeProfileValue(STRING profile_mode, uint32_t value); + bool setSDKHandler(); + bool writeGoalPosition(double radian); + double receiveDynamixelValue(); +}; + +} // namespace scara_dynamixel +#endif // SCARA_DYNAMIXEL_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_kinematics.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_kinematics.h new file mode 100644 index 000000000..8a298e2aa --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/include/scara_libs/scara_kinematics.h @@ -0,0 +1,55 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef SCARA_KINEMATICS_H_ +#define SCARA_KINEMATICS_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace Eigen; +using namespace robotis_manipulator; + +namespace scara_kinematics +{ + +/***************************************************************************** +** Kinematics Solver Using CHain Rule and Geometry +*****************************************************************************/ +class SolverUsingCRAndGeometry : public robotis_manipulator::Kinematics +{ +private: + void forwardKinematicsSolverUsingChainRule(Manipulator *manipulator, Name component_name); + bool inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); + +public: + SolverUsingCRAndGeometry() {} + virtual ~SolverUsingCRAndGeometry() {} + + virtual void setOption(const void *arg); + virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); + virtual void solveForwardKinematics(Manipulator *manipulator); + virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); +}; + +} // namespace scara_kinematics + +#endif // SCARA_KINEMATICS_H_ diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara.cpp new file mode 100644 index 000000000..a8e38e73c --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara.cpp @@ -0,0 +1,206 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/scara_libs/scara.h" + +Scara::Scara() {} +Scara::~Scara() +{ + delete kinematics_; + delete joint_; + delete tool_; + for(uint8_t index = 0; index < CUSTOM_TRAJECTORY_SIZE; index++) + delete custom_trajectory_[index]; +} + +void Scara::initDebug() +{ + DEBUG.begin(57600); // Using Serial4(=SerialBT2) + log::print("OpenManipulator Debugging Port"); +} + +void Scara::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_rate) +{ + /***************************************************************************** + ** Set if using actual robot + *****************************************************************************/ + using_actual_robot_state_ = using_actual_robot_state; + + /***************************************************************************** + ** Initialize Manipulator Parameters + *****************************************************************************/ + addWorld("world", // world name + "joint1"); // child name + + addJoint("joint1", // my name + "world", // parent name + "joint2", // child name + math::vector3(-0.241, 0.0, 0.057), // relative position + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation + Z_AXIS, // axis of rotation + 1); // actuator id + + addJoint("joint2", // my name + "joint1", // parent name + "joint3", // child name + math::vector3(0.067, 0.0, 0.0), // relative position + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation + Z_AXIS, // axis of rotation + 2); // actuator id + + addJoint("joint3", // my name + "joint2", // parent name + "tool", // child name + math::vector3(0.067, 0.0, 0.0), // relative position + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation + Z_AXIS, // axis of rotation + 3); // actuator id + + addTool("tool", // my name + "joint3", // parent name + math::vector3(0.107, 0.0, 0.0), // relative position + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation + 4, // actuator id + 0.01, // max joint limit + -0.51); // min joint limit + + /***************************************************************************** + ** Initialize Kinematics + *****************************************************************************/ + kinematics_ = new scara_kinematics::SolverUsingCRAndGeometry(); + addKinematics(kinematics_); + + /***************************************************************************** + ** Initialize Custom Trajectory + *****************************************************************************/ + custom_trajectory_[0] = new scara_custom_trajectory::Line(); + custom_trajectory_[1] = new scara_custom_trajectory::Circle(); + custom_trajectory_[2] = new scara_custom_trajectory::Rhombus(); + custom_trajectory_[3] = new scara_custom_trajectory::Heart(); + + addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]); + addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]); + addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]); + addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]); + + if (using_actual_robot_state_) + { + /***************************************************************************** + ** Initialize ㅓoint Actuator + *****************************************************************************/ + joint_ = new scara_dynamixel::JointDynamixelProfileControl(control_rate); + + // Set communication arguments + STRING dxl_comm_arg[2] = {usb_port, baud_rate}; + void *p_dxl_comm_arg = &dxl_comm_arg; + + // Set joint actuator id + std::vector jointDxlId; + jointDxlId.push_back(1); + jointDxlId.push_back(2); + jointDxlId.push_back(3); + addJointActuator(JOINT_DYNAMIXEL, joint_, jointDxlId, p_dxl_comm_arg); + + // Set joint actuator control mode + STRING joint_dxl_mode_arg = "position_mode"; + void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; + setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); + + /***************************************************************************** + ** Initialize Tool Actuator + *****************************************************************************/ + tool_ = new scara_dynamixel::ToolDynamixel(); + + // Set tool actuator id + uint8_t toolDxlId = 4; + addToolActuator(TOOL_DYNAMIXEL, tool_, toolDxlId, p_dxl_comm_arg); + + // Set tool actuator control mode + STRING tool_dxl_mode_arg = "position_mode"; + void *p_tool_dxl_mode_arg = &tool_dxl_mode_arg; + setToolActuatorMode(TOOL_DYNAMIXEL, p_tool_dxl_mode_arg); + + /***************************************************************************** + ** Enable actuators and Receive actuator values + *****************************************************************************/ + // Enable All Actuators + enableAllActuator(); + + // Receive current angles from all actuators + receiveAllJointActuatorValue(); + receiveAllToolActuatorValue(); + } +} + +/***************************************************************************** +** Process actuator values received from external controllers +*****************************************************************************/ +void Scara::processOpenManipulator(double present_time) +{ + if (present_time - prev_control_time_ >= CONTROL_RATE) + { + JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time); + JointWaypoint goal_tool_value = getToolGoalValue(); + + if (using_actual_robot_state_) + { + receiveAllJointActuatorValue(); + receiveAllToolActuatorValue(); + } + + if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value); + if(goal_tool_value.size() != 0) sendAllToolActuatorValue(goal_tool_value); + solveForwardKinematics(); + + // Set previous control time + prev_control_time_ = millis()/1000.0; + } +} + +/***************************************************************************** +** State Functions +*****************************************************************************/ +// Check if using acutal robot +bool Scara::getUsingActualRobotState() +{ + return using_actual_robot_state_; +} + +/* Check if the program read data within control rate) */ +bool Scara::getReceiveDataFlag() +{ + return receive_data_flag_; +} + +/* Get the previous time when data were received */ +double Scara::getPrevReceiveTime() +{ + return prev_receive_time_; +} + +/* Set whether data were received or not */ +void Scara::setReceiveDataFlag(bool receive_data_flag) +{ + receive_data_flag_ = receive_data_flag; +} + +/* Set the previous time when data were received */ +void Scara::setPrevReceiveTime(double prev_receive_time) +{ + prev_receive_time_ = prev_receive_time; +} \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_custom_trajectory.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_custom_trajectory.cpp new file mode 100644 index 000000000..8393824aa --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_custom_trajectory.cpp @@ -0,0 +1,346 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/scara_libs/scara_custom_trajectory.h" + +using namespace scara_custom_trajectory; +using namespace Eigen; + + +/***************************************************************************** +** Line +*****************************************************************************/ +void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta) +{ + move_time_ = move_time; + acc_dec_time_ = move_time_ * 0.2; + vel_max_.resize(3); + + TaskWaypoint start_to_goal; + + start_pose_ = start; + + goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation; + goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position; + + vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_); +} + +TaskWaypoint Line::drawLine(double time_var) +{ + TaskWaypoint pose; + + if(acc_dec_time_ >= time_var) + { + pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var ) + { + pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_)) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + } + else if(time_var <= move_time_) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS); + } + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Line::getTaskWaypoint(double tick) +{ + return drawLine(tick); +} + + +void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + TaskWaypoint *c_arg = (TaskWaypoint *)arg; + initLine(move_time, start, c_arg[0]); +} +void Line::setOption(const void *arg) {} + + +/***************************************************************************** +** Circle +*****************************************************************************/ +void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Circle::drawCircle(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + + double diff_pose[2]; + + diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_); + diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Circle::getTaskWaypoint(double tick) +{ + return drawCircle(tick); +} + +void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +void Circle::setOption(const void *arg){} + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + + +TaskWaypoint Rhombus::drawRhombus(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + while(true) + { + if (get_time_var < PI*2) break; + get_time_var = get_time_var - PI*2; + } + + if (get_time_var >= 0 && get_time_var < PI/2){ + traj[0] = - get_time_var / (PI/2); + traj[1] = - get_time_var / (PI/2); + } else if (get_time_var >= PI/2 && get_time_var < PI){ + traj[0] = - get_time_var / (PI/2); + traj[1] = get_time_var / (PI/2) - 2; + } else if (get_time_var >= PI && get_time_var < PI*3/2){ + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = get_time_var / (PI/2) - 2; + } else { + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = - get_time_var / (PI/2) + 4; + } + + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + + +void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +TaskWaypoint Rhombus::getTaskWaypoint(double tick) +{ + return drawRhombus(tick); +} +void Rhombus::setOption(const void *arg){} + + +/***************************************************************************** +** Heart +*****************************************************************************/ +void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Heart::drawHeart(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + double shift_offset = - 5.0; + + traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16; + traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16; + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} +void Heart::setOption(const void *arg){} + +TaskWaypoint Heart::getTaskWaypoint(double tick) +{ + return drawHeart(tick); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_dynamixel.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_dynamixel.cpp new file mode 100644 index 000000000..35b2b4e68 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_dynamixel.cpp @@ -0,0 +1,973 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/scara_libs/scara_dynamixel.h" + +using namespace scara_dynamixel; +using namespace robotis_manipulator; + +/***************************************************************************** +** Joint Dynamixel Control Functions +*****************************************************************************/ +void JointDynamixel::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixel::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixel::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixel::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixel::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixel::getId() +{ + return dynamixel_.id; +} + +void JointDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixel::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + std::vector radian_vector; + for(uint32_t index = 0; index < value_vector.size(); index++) + { + radian_vector.push_back(value_vector.at(index).position); + } + result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixel::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixel::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Control Functions +*****************************************************************************/ +bool JointDynamixel::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixel::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixel::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixel::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool JointDynamixel::writeGoalPosition(std::vector actuator_id, std::vector radian_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_position[actuator_id.size()]; + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + id_array[index] = actuator_id.at(index); + goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index)); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +std::vector JointDynamixel::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Joint Dynamixel Profile Control Functions +*****************************************************************************/ +JointDynamixelProfileControl::JointDynamixelProfileControl(float control_loop_time) +{ + control_loop_time_ = control_loop_time; +} + +void JointDynamixelProfileControl::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixelProfileControl::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixelProfileControl::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixelProfileControl::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixelProfileControl::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixelProfileControl::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixelProfileControl::getId() +{ + return dynamixel_.id; +} + +void JointDynamixelProfileControl::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixelProfileControl::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixelProfileControl::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + result = JointDynamixelProfileControl::writeGoalProfilingControlValue(actuator_id, value_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixelProfileControl::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixelProfileControl::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Profile Control Functions +*****************************************************************************/ +bool JointDynamixelProfileControl::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setTimeBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixelProfileControl::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3; + const uint32_t acceleration = uint32_t(control_loop_time_*1000); + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixelProfileControl::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixelProfileControl::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + return true; +} + +bool JointDynamixelProfileControl::writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_value[actuator_id.size()]; + + //add tarajectory eq. + for(uint8_t index = 0; index < actuator_id.size(); index++) + { + float result_position; + float time_control = control_loop_time_; //ms + + if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end()) + { + previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index))); + } + + result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2; + + id_array[index] = actuator_id.at(index); + goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position); + + previous_goal_value_[actuator_id.at(index)] = value_vector.at(index); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log); + if (result == false) + { + log::error(log); + } + return true; +} + +std::vector JointDynamixelProfileControl::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Tool Dynamixel Control Functions +*****************************************************************************/ +void ToolDynamixel::init(uint8_t actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = ToolDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void ToolDynamixel::setMode(const void *arg) +{ + bool result = false; +// const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = ToolDynamixel::setOperatingMode(get_arg_[0]); + if (result == false) + return; + } + else + { + result = ToolDynamixel::writeProfileValue(get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + + result = ToolDynamixel::setSDKHandler(); + if (result == false) + return; +} + +uint8_t ToolDynamixel::getId() +{ + return dynamixel_.id.at(0); +} + +void ToolDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = true; +} + +void ToolDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = false; +} + +bool ToolDynamixel::sendToolActuatorValue(robotis_manipulator::ActuatorValue value) +{ + return ToolDynamixel::writeGoalPosition(value.position); +} + +robotis_manipulator::ActuatorValue ToolDynamixel::receiveToolActuatorValue() +{ + robotis_manipulator::ActuatorValue result; + result.position = ToolDynamixel::receiveDynamixelValue(); + result.velocity = 0.0; + result.acceleration = 0.0; + result.effort = 0.0; + return result; +} + + +/***************************************************************************** +** Functions called in Tool Dynamixel Profile Control Functions +*****************************************************************************/ +bool ToolDynamixel::initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + const char* log = NULL; + bool result = false; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id.push_back(actuator_id); + dynamixel_.num = 1; + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Tool Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0)); + strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0))); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + + return true; +} + +bool ToolDynamixel::setOperatingMode(STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 200; + + if (dynamixel_mode == "position_mode") + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log); + if (result == false) + { + log::error(log); + } + } + else + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool ToolDynamixel::writeProfileValue(STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::setSDKHandler() +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0), "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0), + "Present_Position", + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::writeGoalPosition(double radian) +{ + bool result = false; + const char* log = NULL; + + int32_t goal_position = 0; + + goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian); + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, &goal_position, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +double ToolDynamixel::receiveDynamixelValue() +{ + bool result = false; + const char* log = NULL; + + int32_t get_value = 0; + uint8_t id_array[1] = {dynamixel_.id.at(0)}; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &get_value, + &log); + if (result == false) + { + log::error(log); + } + + return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_kinematics.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_kinematics.cpp new file mode 100644 index 000000000..c0f2e1d54 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/scara_libs/src/scara_kinematics.cpp @@ -0,0 +1,169 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/scara_libs/scara_kinematics.h" + +using namespace robotis_manipulator; +using namespace scara_kinematics; + +/***************************************************************************** +** Kinematics Solver +*****************************************************************************/ +void SolverUsingCRAndGeometry::setOption(const void *arg) {} + +Eigen::MatrixXd SolverUsingCRAndGeometry::jacobian(Manipulator *manipulator, Name tool_name) +{ + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, manipulator->getDOF()); + + Eigen::Vector3d joint_axis = Eigen::Vector3d::Zero(3); + + Eigen::Vector3d position_changed = Eigen::Vector3d::Zero(3); + Eigen::Vector3d orientation_changed = Eigen::Vector3d::Zero(3); + Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); + + int8_t index = 0; + Name my_name = manipulator->getWorldChildName(); + + for (int8_t size = 0; size < manipulator->getDOF(); size++) + { + Name parent_name = manipulator->getComponentParentName(my_name); + if (parent_name == manipulator->getWorldName()) + { + joint_axis = manipulator->getWorldOrientation() * manipulator->getAxis(my_name); + } + else + { + joint_axis = manipulator->getComponentOrientationFromWorld(parent_name) * manipulator->getAxis(my_name); + } + + position_changed = math::skewSymmetricMatrix(joint_axis) * + (manipulator->getComponentPositionFromWorld(tool_name) - manipulator->getComponentPositionFromWorld(my_name)); + orientation_changed = joint_axis; + + pose_changed << position_changed(0), + position_changed(1), + position_changed(2), + orientation_changed(0), + orientation_changed(1), + orientation_changed(2); + + jacobian.col(index) = pose_changed; + index++; + my_name = manipulator->getComponentChildName(my_name).at(0); // Get Child name which has active joint + } + return jacobian; +} + +void SolverUsingCRAndGeometry::solveForwardKinematics(Manipulator *manipulator) +{ + forwardKinematicsSolverUsingChainRule(manipulator, manipulator->getWorldChildName()); +} + +bool SolverUsingCRAndGeometry::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + return inverseKinematicsSolverUsingGeometry(manipulator, tool_name, target_pose, goal_joint_value); +} + +/***************************************************************************** +** Private +*****************************************************************************/ +void SolverUsingCRAndGeometry::forwardKinematicsSolverUsingChainRule(Manipulator *manipulator, Name component_name) +{ + Name my_name = component_name; + Name parent_name = manipulator->getComponentParentName(my_name); + int8_t number_of_child = manipulator->getComponentChildName(my_name).size(); + + Pose parent_pose_value; + Pose my_pose_value; + + //Get Parent Pose + if (parent_name == manipulator->getWorldName()) + { + parent_pose_value = manipulator->getWorldPose(); + } + else + { + parent_pose_value = manipulator->getComponentPoseFromWorld(parent_name); + } + + //position + my_pose_value.kinematic.position = parent_pose_value.kinematic.position + + (parent_pose_value.kinematic.orientation * manipulator->getComponentRelativePositionFromParent(my_name)); + //orientation + my_pose_value.kinematic.orientation = parent_pose_value.kinematic.orientation * math::rodriguesRotationMatrix(manipulator->getAxis(my_name), manipulator->getJointPosition(my_name)); + //linear velocity + my_pose_value.dynamic.linear.velocity = math::vector3(0.0, 0.0, 0.0); + //angular velocity + my_pose_value.dynamic.angular.velocity = math::vector3(0.0, 0.0, 0.0); + //linear acceleration + my_pose_value.dynamic.linear.acceleration = math::vector3(0.0, 0.0, 0.0); + //angular acceleration + my_pose_value.dynamic.angular.acceleration = math::vector3(0.0, 0.0, 0.0); + + manipulator->setComponentPoseFromWorld(my_name, my_pose_value); + + for (int8_t index = 0; index < number_of_child; index++) + { + Name child_name = manipulator->getComponentChildName(my_name).at(index); + forwardKinematicsSolverUsingChainRule(manipulator, child_name); + } +} + +bool SolverUsingCRAndGeometry::inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value) +{ + const double link[3] = {0.067, 0.067, 0.107}; + JointValue target_angle[3]; + std::vector target_angle_vector; + + // Compute the length from Joint1 to the end effector + double temp_target_pose[2]; + double target_pose_length; + temp_target_pose[0] = target_pose.kinematic.position(0) + 0.241; + temp_target_pose[1] = target_pose.kinematic.position(1); + target_pose_length = sqrt((temp_target_pose[0])*temp_target_pose[0] + temp_target_pose[1]*temp_target_pose[1]); + + // Compute the length of Position Difference and Target Angle + double error=1000.0; // random large initial value + + for (uint16_t count=0; count<=900; count++){ + double theta=(double)count/10*DEG2RAD; + + // Assume theta = target_angle[1] = target_angle[2] + double alpha = acos((link[1]*link[1]+link[2]*link[2]-link[0]*link[0]-target_pose_length*target_pose_length+2*link[1]*link[2]*cos(theta)) + / (-2*target_pose_length*link[0])); + double beta = acos((link[0]*link[0]+link[1]*link[1]-link[2]*link[2]-target_pose_length*target_pose_length+2*link[0]*link[1]*cos(theta)) + / (-2*target_pose_length*link[2])); + double temp_error = abs(alpha + beta - 2*theta); + + if (temp_error < error){ + target_angle[0].position = PI/2 -acos(temp_target_pose[1]/target_pose_length) - alpha; + target_angle[1].position = theta; + target_angle[2].position = theta; + error = temp_error; + } + } + + // Set joint angle + target_angle_vector.push_back(target_angle[0]); + target_angle_vector.push_back(target_angle[1]); + target_angle_vector.push_back(target_angle[2]); + + *goal_joint_value = target_angle_vector; + + return true; +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs.h new file mode 100644 index 000000000..0c2a50b18 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs.h @@ -0,0 +1 @@ +#include "stewart_libs/include/stewart_libs/stewart.h" \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart.h new file mode 100644 index 000000000..0f993a3bc --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart.h @@ -0,0 +1,78 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef STEWART_H_ +#define STEWART_H_ + +#include "stewart_custom_trajectory.h" +#include "stewart_dynamixel.h" +#include "stewart_kinematics.h" + +#define CUSTOM_TRAJECTORY_SIZE 4 +#define CUSTOM_TRAJECTORY_LINE "custom_trajectory_line" +#define CUSTOM_TRAJECTORY_CIRCLE "custom_trajectory_circle" +#define CUSTOM_TRAJECTORY_RHOMBUS "custom_trajectory_rhombus" +#define CUSTOM_TRAJECTORY_HEART "custom_trajectory_heart" + +#define DXL_SIZE 6 +#define JOINT_DYNAMIXEL "joint_dxl" + +#define RECEIVE_RATE 0.100 // unit: s +#define CONTROL_RATE 0.010 // unit: s + +#define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) +#define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) +#define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) + +class Stewart : public robotis_manipulator::RobotisManipulator +{ +private: + robotis_manipulator::Kinematics *kinematics_; + robotis_manipulator::JointActuator *joint_; + robotis_manipulator::ToolActuator *tool_; + robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE]; + + bool using_actual_robot_state_; + bool receive_data_flag_ = false; + double prev_receive_time_ = 0.0; + double prev_control_time_ = 0.0; + + public: + Stewart(); + virtual ~Stewart(); + + void initDebug(); + void initOpenManipulator(bool using_actual_robot_state, + STRING usb_port = "/dev/ttyUSB0", + STRING baud_rate = "1000000", + float control_rate = CONTROL_RATE); + void processOpenManipulator(double present_time); + + bool getUsingActualRobotState(); + bool getReceiveDataFlag(); + double getPrevReceiveTime(); + + void setReceiveDataFlag(bool receive_data_flag); + void setPrevReceiveTime(double prev_receive_time); +}; + +#endif // STEWART_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_custom_trajectory.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_custom_trajectory.h new file mode 100644 index 000000000..dc0d04338 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_custom_trajectory.h @@ -0,0 +1,158 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef STEWART_CUSTOM_TRAJECTORY_H_ +#define STEWART_CUSTOM_TRAJECTORY_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace robotis_manipulator; +using namespace Eigen; + +namespace stewart_custom_trajectory +{ + +enum AXIS{ + X_AXIS, + Y_AXIS, + Z_AXIS, +}; + +/***************************************************************************** +** Line +*****************************************************************************/ +class Line : public robotis_manipulator::CustomTaskTrajectory +{ +private: + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double acc_dec_time_; + double move_time_; + std::vector vel_max_; + +public: + Line() {} + virtual ~Line() {} + + void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta); + TaskWaypoint drawLine(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Circle +*****************************************************************************/ +class Circle : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Circle() {} + virtual ~Circle() {} + + void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawCircle(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +class Rhombus : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Rhombus() {} + virtual ~Rhombus() {} + + void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawRhombus(double time_var); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +/***************************************************************************** +** Heart +*****************************************************************************/ +class Heart : public robotis_manipulator::CustomTaskTrajectory +{ +private: + robotis_manipulator::MinimumJerk path_generator_; + VectorXd coefficient_; + + TaskWaypoint start_pose_; + TaskWaypoint goal_pose_; + + double radius_; + double start_angular_position_; + double revolution_; + +public: + Heart() {} + virtual ~Heart() {} + + void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); + TaskWaypoint drawHeart(double tick); + + virtual void setOption(const void *arg); + virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); + virtual TaskWaypoint getTaskWaypoint(double tick); +}; + + +} // namespace stewart_custom_trajectory +#endif // STEWART_CUSTOM_TRAJECTORY_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_dynamixel.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_dynamixel.h new file mode 100644 index 000000000..667fb5920 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_dynamixel.h @@ -0,0 +1,189 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef STEWART_DYNAMIXEL_H_ +#define STEWART_DYNAMIXEL_H_ + +#if defined(__OPENCR__) + #include + #include +#else + #include + #include +#endif + +namespace stewart_dynamixel +{ + +#define SYNC_WRITE_HANDLER 0 +#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 + +//#define CONTROL_LOOP_TIME 10; //ms + +// Protocol 2.0 +#define ADDR_PRESENT_CURRENT_2 126 +#define ADDR_PRESENT_VELOCITY_2 128 +#define ADDR_PRESENT_POSITION_2 132 +#define ADDR_VELOCITY_TRAJECTORY_2 136 +#define ADDR_POSITION_TRAJECTORY_2 140 +#define ADDR_PROFILE_ACCELERATION_2 108 +#define ADDR_PROFILE_VELOCITY_2 112 +#define ADDR_GOAL_POSITION_2 116 + + +#define LENGTH_PRESENT_CURRENT_2 2 +#define LENGTH_PRESENT_VELOCITY_2 4 +#define LENGTH_PRESENT_POSITION_2 4 +#define LENGTH_VELOCITY_TRAJECTORY_2 4 +#define LENGTH_POSITION_TRAJECTORY_2 4 +#define LENGTH_PROFILE_ACCELERATION_2 4 +#define LENGTH_PROFILE_VELOCITY_2 4 +#define LENGTH_GOAL_POSITION_2 4 + + +// Protocol 1.0 +#define ADDR_PRESENT_CURRENT_1 = 40; +#define ADDR_PRESENT_VELOCITY_1 = 38; +#define ADDR_PRESENT_POSITION_1 = 36; + +#define LENGTH_PRESENT_CURRENT_1 = 2; +#define LENGTH_PRESENT_VELOCITY_1 = 2; +#define LENGTH_PRESENT_POSITION_1 = 2; + +typedef struct +{ + std::vector id; + uint8_t num; +} Joint; + +class JointDynamixel : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + +public: + JointDynamixel(){} + virtual ~JointDynamixel(){} + + + /***************************************************************************** + ** Joint Dynamixel Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalPosition(std::vector actuator_id, std::vector radian_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class JointDynamixelProfileControl : public robotis_manipulator::JointActuator +{ +private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + float control_loop_time_; // unit: ms + std::map previous_goal_value_; + +public: + JointDynamixelProfileControl(float control_loop_time = 0.010); + virtual ~JointDynamixelProfileControl(){} + + + /***************************************************************************** + ** Joint Dynamixel Profile Control Functions + *****************************************************************************/ + virtual void init(std::vector actuator_id, const void *arg); + virtual void setMode(std::vector actuator_id, const void *arg); + virtual std::vector getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); + virtual std::vector receiveJointActuatorValue(std::vector actuator_id); + + + /***************************************************************************** + ** Functions called in Joint Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); + bool setSDKHandler(uint8_t actuator_id); + bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); + bool writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector); + std::vector receiveAllDynamixelValue(std::vector actuator_id); +}; + +class ToolDynamixel : public robotis_manipulator::ToolActuator +{ + private: + DynamixelWorkbench *dynamixel_workbench_; + Joint dynamixel_; + + public: + ToolDynamixel() {} + virtual ~ToolDynamixel() {} + + + /***************************************************************************** + ** Tool Dynamixel Control Functions + *****************************************************************************/ + virtual void init(uint8_t actuator_id, const void *arg); + virtual void setMode(const void *arg); + virtual uint8_t getId(); + + virtual void enable(); + virtual void disable(); + + virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value); + virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue(); + + + /***************************************************************************** + ** Functions called in Tool Dynamixel Profile Control Functions + *****************************************************************************/ + bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); + bool setOperatingMode(STRING dynamixel_mode = "position_mode"); + bool writeProfileValue(STRING profile_mode, uint32_t value); + bool setSDKHandler(); + bool writeGoalPosition(double radian); + double receiveDynamixelValue(); +}; + +} // namespace stewart_dynamixel +#endif // STEWART_DYNAMIXEL_H_ + + + + diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_kinematics.h b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_kinematics.h new file mode 100644 index 000000000..c8023f71c --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/include/stewart_libs/stewart_kinematics.h @@ -0,0 +1,54 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#ifndef STEWART_KINEMATICS_H_ +#define STEWART_KINEMATICS_H_ + +#if defined(__OPENCR__) + #include +#else + #include +#endif + +using namespace Eigen; +using namespace robotis_manipulator; + +namespace stewart_kinematics +{ + +/***************************************************************************** +** Kinematics Solver Using Geometry +*****************************************************************************/ +class SolverUsingGeometry : public robotis_manipulator::Kinematics +{ +private: + bool inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); + +public: + SolverUsingGeometry() {} + virtual ~SolverUsingGeometry() {} + + virtual void setOption(const void *arg); + virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); + virtual void solveForwardKinematics(Manipulator *manipulator); + virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); +}; + +} // namespace stewart_kinematics + +#endif // STEWART_KINEMATICS_H_ diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart.cpp new file mode 100644 index 000000000..41543fd45 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart.cpp @@ -0,0 +1,326 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/stewart_libs/stewart.h" + +Stewart::Stewart() {} +Stewart::~Stewart() +{ + delete kinematics_; + delete joint_; + delete tool_; + for(uint8_t index = 0; index < CUSTOM_TRAJECTORY_SIZE; index++) + delete custom_trajectory_[index]; +} + +void Stewart::initDebug() +{ + DEBUG.begin(57600); // Using Serial4(=SerialBT2) + log::print("OpenManipulator Debugging Port"); +} + +void Stewart::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_rate) +{ + /***************************************************************************** + ** Set if using actual robot + *****************************************************************************/ + using_actual_robot_state_ = using_actual_robot_state; + + /***************************************************************************** + ** Initialize Manipulator Parameters + *****************************************************************************/ + addWorld("world", // world name + "joint1"); // child name + + addJoint("joint01", // my name + "world", // parent name + "joint07", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 1); // actuator id + + addJoint("joint02", // my name + "world", // parent name + "joint08", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 2); // actuator id + + addJoint("joint03", // my name + "world", // parent name + "joint09", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 3); // actuator id + + addJoint("joint04", // my name + "world", // parent name + "joint10", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 4); // actuator id + + addJoint("joint05", // my name + "world", // parent name + "joint11", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 5); // actuator id + + addJoint("joint06", // my name + "world", // parent name + "joint12", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + 6); // actuator id + + addJoint("joint07", // my name + "joint01", // parent name + "joint13", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint07_2", // my name + "joint01", // parent name + "joint13", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint08", // my name + "joint02", // parent name + "joint14", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint08_2", // my name + "joint02", // parent name + "joint14", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint09", // my name + "joint03", // parent name + "joint15", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint09_2", // my name + "joint03", // parent name + "joint15", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint10", // my name + "joint04", // parent name + "joint16", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint10_2", // my name + "joint04", // parent name + "joint16", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint11", // my name + "joint05", // parent name + "joint17", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint11_2", // my name + "joint05", // parent name + "joint17", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint12", // my name + "joint06", // parent name + "joint18", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint12_2", // my name + "joint06", // parent name + "joint18", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint13", // my name + "joint07", // parent name + "joint19", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint13_2", // my name + "joint07", // parent name + "joint19", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addJoint("joint14", // my name + "joint07", // parent name + "tool", // child name + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + Z_AXIS, // axis of rotation + -1); // actuator id + + addTool("tool", // my name + "joint14", // Not used + math::vector3(0.0, 0.0, 0.0), // Not used + math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // Not used + -1); // actuator id + + /***************************************************************************** + ** Initialize Kinematics + *****************************************************************************/ + kinematics_ = new stewart_kinematics::SolverUsingGeometry(); + addKinematics(kinematics_); + + /***************************************************************************** + ** Initialize Custom Trajectory + *****************************************************************************/ + custom_trajectory_[0] = new stewart_custom_trajectory::Line(); + custom_trajectory_[1] = new stewart_custom_trajectory::Circle(); + custom_trajectory_[2] = new stewart_custom_trajectory::Rhombus(); + custom_trajectory_[3] = new stewart_custom_trajectory::Heart(); + + addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]); + addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]); + addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]); + addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]); + + if (using_actual_robot_state_) + { + /***************************************************************************** + ** Initialize ㅓoint Actuator + *****************************************************************************/ + joint_ = new stewart_dynamixel::JointDynamixelProfileControl(control_rate); + + // Set communication arguments + STRING dxl_comm_arg[2] = {usb_port, baud_rate}; + void *p_dxl_comm_arg = &dxl_comm_arg; + + // Set joint actuator id + std::vector jointDxlId; + jointDxlId.push_back(1); + jointDxlId.push_back(2); + jointDxlId.push_back(3); + jointDxlId.push_back(4); + jointDxlId.push_back(5); + jointDxlId.push_back(6); + addJointActuator(JOINT_DYNAMIXEL, joint_, jointDxlId, p_dxl_comm_arg); + + // Set joint actuator control mode + STRING joint_dxl_mode_arg = "position_mode"; + void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; + setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); + + /***************************************************************************** + ** Enable actuators and Receive actuator values + *****************************************************************************/ + // Enable All Actuators + enableAllActuator(); + + // Receive current angles from all actuators + receiveAllJointActuatorValue(); + } +} + +/***************************************************************************** +** Process actuator values received from external controllers +*****************************************************************************/ +void Stewart::processOpenManipulator(double present_time) +{ + if (present_time - prev_control_time_ >= CONTROL_RATE) + { + JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time); + if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value); + + // Set previous control time + prev_control_time_ = millis()/1000.0; + } +} + +/***************************************************************************** +** State Functions +*****************************************************************************/ +// Check if using acutal robot +bool Stewart::getUsingActualRobotState() +{ + return using_actual_robot_state_; +} + +/* Check if the program read data within control rate) */ +bool Stewart::getReceiveDataFlag() +{ + return receive_data_flag_; +} + +/* Get the previous time when data were received */ +double Stewart::getPrevReceiveTime() +{ + return prev_receive_time_; +} + +/* Set whether data were received or not */ +void Stewart::setReceiveDataFlag(bool receive_data_flag) +{ + receive_data_flag_ = receive_data_flag; +} + +/* Set the previous time when data were received */ +void Stewart::setPrevReceiveTime(double prev_receive_time) +{ + prev_receive_time_ = prev_receive_time; +} \ No newline at end of file diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_custom_trajectory.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_custom_trajectory.cpp new file mode 100644 index 000000000..36219599c --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_custom_trajectory.cpp @@ -0,0 +1,346 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/stewart_libs/stewart_custom_trajectory.h" + +using namespace stewart_custom_trajectory; +using namespace Eigen; + + +/***************************************************************************** +** Line +*****************************************************************************/ +void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta) +{ + move_time_ = move_time; + acc_dec_time_ = move_time_ * 0.2; + vel_max_.resize(3); + + TaskWaypoint start_to_goal; + + start_pose_ = start; + + goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation; + goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position; + + vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_); + vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_); +} + +TaskWaypoint Line::drawLine(double time_var) +{ + TaskWaypoint pose; + + if(acc_dec_time_ >= time_var) + { + pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var ) + { + pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS); + } + else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_)) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); + } + else if(time_var <= move_time_) + { + pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS); + pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS); + pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS); + } + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Line::getTaskWaypoint(double tick) +{ + return drawLine(tick); +} + + +void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + TaskWaypoint *c_arg = (TaskWaypoint *)arg; + initLine(move_time, start, c_arg[0]); +} +void Line::setOption(const void *arg) {} + + +/***************************************************************************** +** Circle +*****************************************************************************/ +void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Circle::drawCircle(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + + double diff_pose[2]; + + diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_); + diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +TaskWaypoint Circle::getTaskWaypoint(double tick) +{ + return drawCircle(tick); +} + +void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +void Circle::setOption(const void *arg){} + + +/***************************************************************************** +** Rhombus +*****************************************************************************/ +void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + + +TaskWaypoint Rhombus::drawRhombus(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + while(true) + { + if (get_time_var < PI*2) break; + get_time_var = get_time_var - PI*2; + } + + if (get_time_var >= 0 && get_time_var < PI/2){ + traj[0] = - get_time_var / (PI/2); + traj[1] = - get_time_var / (PI/2); + } else if (get_time_var >= PI/2 && get_time_var < PI){ + traj[0] = - get_time_var / (PI/2); + traj[1] = get_time_var / (PI/2) - 2; + } else if (get_time_var >= PI && get_time_var < PI*3/2){ + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = get_time_var / (PI/2) - 2; + } else { + traj[0] = get_time_var / (PI/2) - 4; + traj[1] = - get_time_var / (PI/2) + 4; + } + + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + + +void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} + +TaskWaypoint Rhombus::getTaskWaypoint(double tick) +{ + return drawRhombus(tick); +} +void Rhombus::setOption(const void *arg){} + + +/***************************************************************************** +** Heart +*****************************************************************************/ +void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) +{ + start_pose_ = start; + + radius_ = radius; + revolution_ = revolution; + start_angular_position_ = start_angular_position; + + Point drawingStart, drawingGoal; + + drawingStart.position = 0.0; + drawingStart.velocity = 0.0; + drawingStart.acceleration = 0.0; + drawingStart.effort = 0.0; + + drawingGoal.position = revolution_ * 2*M_PI; + drawingGoal.velocity = 0.0; + drawingGoal.acceleration = 0.0; + drawingGoal.effort = 0.0; + + path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); + coefficient_ = path_generator_.getCoefficient(); +} + +TaskWaypoint Heart::drawHeart(double tick) +{ + // get time variable + double get_time_var = 0.0; + + get_time_var = coefficient_(0) + + coefficient_(1) * pow(tick, 1) + + coefficient_(2) * pow(tick, 2) + + coefficient_(3) * pow(tick, 3) + + coefficient_(4) * pow(tick, 4) + + coefficient_(5) * pow(tick, 5); + + // set drawing trajectory + TaskWaypoint pose; + double diff_pose[2]; + double traj[2]; + + double shift_offset = - 5.0; + + traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16; + traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16; + + diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); + diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); + + pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; + pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; + pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); + + pose.kinematic.orientation = start_pose_.kinematic.orientation; + + pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); + pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); + + return pose; +} + +void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) +{ + double *get_arg_ = (double *)arg; + initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); +} +void Heart::setOption(const void *arg){} + +TaskWaypoint Heart::getTaskWaypoint(double tick) +{ + return drawHeart(tick); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_dynamixel.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_dynamixel.cpp new file mode 100644 index 000000000..c67984be0 --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_dynamixel.cpp @@ -0,0 +1,973 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/stewart_libs/stewart_dynamixel.h" + +using namespace stewart_dynamixel; +using namespace robotis_manipulator; + +/***************************************************************************** +** Joint Dynamixel Control Functions +*****************************************************************************/ +void JointDynamixel::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixel::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixel::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixel::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixel::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixel::getId() +{ + return dynamixel_.id; +} + +void JointDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixel::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + std::vector radian_vector; + for(uint32_t index = 0; index < value_vector.size(); index++) + { + radian_vector.push_back(value_vector.at(index).position); + } + result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixel::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixel::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Control Functions +*****************************************************************************/ +bool JointDynamixel::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixel::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixel::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixel::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool JointDynamixel::writeGoalPosition(std::vector actuator_id, std::vector radian_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_position[actuator_id.size()]; + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + id_array[index] = actuator_id.at(index); + goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index)); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +std::vector JointDynamixel::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Joint Dynamixel Profile Control Functions +*****************************************************************************/ +JointDynamixelProfileControl::JointDynamixelProfileControl(float control_loop_time) +{ + control_loop_time_ = control_loop_time; +} + +void JointDynamixelProfileControl::init(std::vector actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = JointDynamixelProfileControl::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void JointDynamixelProfileControl::setMode(std::vector actuator_id, const void *arg) +{ + bool result = false; + // const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = JointDynamixelProfileControl::setOperatingMode(actuator_id, get_arg_[0]); + if (result == false) + return; + + result = JointDynamixelProfileControl::setSDKHandler(actuator_id.at(0)); + if (result == false) + return; + } + else + { + result = JointDynamixelProfileControl::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + return; +} + +std::vector JointDynamixelProfileControl::getId() +{ + return dynamixel_.id; +} + +void JointDynamixelProfileControl::enable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = true; +} + +void JointDynamixelProfileControl::disable() +{ + const char* log = NULL; + bool result = false; + + for (uint32_t index = 0; index < dynamixel_.num; index++) + { + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); + if (result == false) + { + log::error(log); + } + } + enabled_state_ = false; +} + +bool JointDynamixelProfileControl::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + + result = JointDynamixelProfileControl::writeGoalProfilingControlValue(actuator_id, value_vector); + if (result == false) + return false; + + return true; +} + +std::vector JointDynamixelProfileControl::receiveJointActuatorValue(std::vector actuator_id) +{ + return JointDynamixelProfileControl::receiveAllDynamixelValue(actuator_id); +} + + +/***************************************************************************** +** Functions called in Joint Dynamixel Profile Control Functions +*****************************************************************************/ +bool JointDynamixelProfileControl::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + bool result = false; + const char* log = NULL; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id = actuator_id; + dynamixel_.num = actuator_id.size(); + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + for (uint8_t index = 0; index < dynamixel_.num; index++) + { + uint8_t id = dynamixel_.id.at(index); + result = dynamixel_workbench_->ping(id, &get_model_number, &log); + + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); + log::println(str); + + result = dynamixel_workbench_->setTimeBasedProfile(id, &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + } + return true; +} + +bool JointDynamixelProfileControl::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3; + const uint32_t acceleration = uint32_t(control_loop_time_*1000); + const uint32_t current = 0; + + if (dynamixel_mode == "position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); + if (result == false) + { + log::error(log); + } + } + } + else + { + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + } + + return true; +} + +bool JointDynamixelProfileControl::setSDKHandler(uint8_t actuator_id) +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, + (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool JointDynamixelProfileControl::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + for (uint8_t num = 0; num < actuator_id.size(); num++) + { + result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + } + return true; +} + +bool JointDynamixelProfileControl::writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector) +{ + bool result = false; + const char* log = NULL; + + uint8_t id_array[actuator_id.size()]; + int32_t goal_value[actuator_id.size()]; + + //add tarajectory eq. + for(uint8_t index = 0; index < actuator_id.size(); index++) + { + float result_position; + float time_control = control_loop_time_; //ms + + if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end()) + { + previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index))); + } + + result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2; + + id_array[index] = actuator_id.at(index); + goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position); + + previous_goal_value_[actuator_id.at(index)] = value_vector.at(index); + } + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log); + if (result == false) + { + log::error(log); + } + return true; +} + +std::vector JointDynamixelProfileControl::receiveAllDynamixelValue(std::vector actuator_id) +{ + bool result = false; + const char* log = NULL; + + std::vector all_actuator; + + uint8_t id_array[actuator_id.size()]; + for (uint8_t index = 0; index < actuator_id.size(); index++) + id_array[index] = actuator_id.at(index); + + int32_t get_current[actuator_id.size()]; + int32_t get_velocity[actuator_id.size()]; + int32_t get_position[actuator_id.size()]; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_CURRENT_2, + LENGTH_PRESENT_CURRENT_2, + get_current, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_VELOCITY_2, + LENGTH_PRESENT_VELOCITY_2, + get_velocity, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + actuator_id.size(), + ADDR_PRESENT_POSITION_2, + LENGTH_PRESENT_POSITION_2, + get_position, + &log); + if (result == false) + { + log::error(log); + } + + for (uint8_t index = 0; index < actuator_id.size(); index++) + { + robotis_manipulator::ActuatorValue actuator; + actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); + actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); + actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); + + all_actuator.push_back(actuator); + } + + return all_actuator; +} + + +/***************************************************************************** +** Tool Dynamixel Control Functions +*****************************************************************************/ +void ToolDynamixel::init(uint8_t actuator_id, const void *arg) +{ + STRING *get_arg_ = (STRING *)arg; + + bool result = ToolDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); + + if (result == false) + return; +} + +void ToolDynamixel::setMode(const void *arg) +{ + bool result = false; +// const char* log = NULL; + + STRING *get_arg_ = (STRING *)arg; + + if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") + { + result = ToolDynamixel::setOperatingMode(get_arg_[0]); + if (result == false) + return; + } + else + { + result = ToolDynamixel::writeProfileValue(get_arg_[0], std::atoi(get_arg_[1].c_str())); + if (result == false) + return; + } + + result = ToolDynamixel::setSDKHandler(); + if (result == false) + return; +} + +uint8_t ToolDynamixel::getId() +{ + return dynamixel_.id.at(0); +} + +void ToolDynamixel::enable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = true; +} + +void ToolDynamixel::disable() +{ + const char* log = NULL; + bool result = false; + + result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log); + if (result == false) + { + log::error(log); + } + enabled_state_ = false; +} + +bool ToolDynamixel::sendToolActuatorValue(robotis_manipulator::ActuatorValue value) +{ + return ToolDynamixel::writeGoalPosition(value.position); +} + +robotis_manipulator::ActuatorValue ToolDynamixel::receiveToolActuatorValue() +{ + robotis_manipulator::ActuatorValue result; + result.position = ToolDynamixel::receiveDynamixelValue(); + result.velocity = 0.0; + result.acceleration = 0.0; + result.effort = 0.0; + return result; +} + + +/***************************************************************************** +** Functions called in Tool Dynamixel Profile Control Functions +*****************************************************************************/ +bool ToolDynamixel::initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) +{ + const char* log = NULL; + bool result = false; + + STRING return_delay_time_st = "Return_Delay_Time"; + const char * return_delay_time_char = return_delay_time_st.c_str(); + + dynamixel_.id.push_back(actuator_id); + dynamixel_.num = 1; + + dynamixel_workbench_ = new DynamixelWorkbench; + + result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); + if (result == false) + { + log::error(log); + } + + uint16_t get_model_number; + result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel ID"); + } + else + { + char str[100]; + sprintf(str, "Tool Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0)); + strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0))); + log::println(str); + + result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log); + if(result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version (v38~)"); + } + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log); + if (result == false) + { + log::error(log); + log::error("Please check your Dynamixel firmware version"); + } + } + + return true; +} + +bool ToolDynamixel::setOperatingMode(STRING dynamixel_mode) +{ + const char* log = NULL; + bool result = false; + + const uint32_t velocity = 0; + const uint32_t acceleration = 0; + const uint32_t current = 200; + + if (dynamixel_mode == "position_mode") + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + else if (dynamixel_mode == "current_based_position_mode") + { + result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log); + if (result == false) + { + log::error(log); + } + } + else + { + result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); + if (result == false) + { + log::error(log); + } + } + + return true; +} + +bool ToolDynamixel::writeProfileValue(STRING profile_mode, uint32_t value) +{ + const char* log = NULL; + bool result = false; + + const char * char_profile_mode = profile_mode.c_str(); + + result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::setSDKHandler() +{ + bool result = false; + const char* log = NULL; + + result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0), "Goal_Position", &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0), + "Present_Position", + &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +bool ToolDynamixel::writeGoalPosition(double radian) +{ + bool result = false; + const char* log = NULL; + + int32_t goal_position = 0; + + goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian); + + result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, &goal_position, &log); + if (result == false) + { + log::error(log); + } + + return true; +} + +double ToolDynamixel::receiveDynamixelValue() +{ + bool result = false; + const char* log = NULL; + + int32_t get_value = 0; + uint8_t id_array[1] = {dynamixel_.id.at(0)}; + + result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &log); + if (result == false) + { + log::error(log); + } + + result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, + id_array, + (uint8_t)1, + &get_value, + &log); + if (result == false) + { + log::error(log); + } + + return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value); +} diff --git a/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_kinematics.cpp b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_kinematics.cpp new file mode 100644 index 000000000..82890565f --- /dev/null +++ b/arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/stewart_libs/src/stewart_kinematics.cpp @@ -0,0 +1,227 @@ +/******************************************************************************* +* Copyright 2018 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ + +#include "../include/stewart_libs/stewart_kinematics.h" + +using namespace robotis_manipulator; +using namespace stewart_kinematics; + +/***************************************************************************** +** Kinematics Solver +*****************************************************************************/ +void SolverUsingGeometry::setOption(const void *arg) {} + +Eigen::MatrixXd SolverUsingGeometry::jacobian(Manipulator *manipulator, Name tool_name) +{ + return Eigen::MatrixXd::Identity(6, manipulator->getDOF()); +} + +void SolverUsingGeometry::solveForwardKinematics(Manipulator *manipulator) {} + +bool SolverUsingGeometry::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + return inverseKinematicsSolverUsingGeometry(manipulator, tool_name, target_pose, goal_joint_value); +} + +/***************************************************************************** +** Private +*****************************************************************************/ +bool SolverUsingGeometry::inverseKinematicsSolverUsingGeometry(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) +{ + std::vector target_angle_vector; + + double temp_angle[6]; + double temp_angle2[6]; + JointValue target_angle[21]; + double link[2] = {0.026, 0.1227}; + double start_x[6], start_y[6], start_z[6], + temp_x[6], temp_y[6], temp_z[6], + target_x[6], target_y[6], target_z[6], + diff_x[6], diff_y[6], diff_z[6]; + double temp[6], temp2[6]; + double target_pose_length[6]; + Matrix3d goal_orientation; + double elbow_x[6], elbow_y[6], elbow_z[6], + temp_elbow_x[6], temp_elbow_y[6], temp_elbow_z[6], + temp_target_x[6], temp_target_y[6], temp_target_z[6], + diff_x2[6], diff_y2[6], diff_z2[6], + diff_x3[6], diff_y3[6], diff_z3[6], + temp_target_angle[6], temp_target_angle2[6], + temp_diffx3[6], temp_diffy3[6], temp_diffz3[6], + temp_target_angle3[6], temp_target_angle4[6]; + + // Start pose for each set of two joints + for (int i=0; i<6; i++){ + start_x[i] = cos(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436)*(-0.0774); + start_y[i] = sin(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436)*(-0.0774); + start_z[i] = -0.1057; + } + + // Goal pose for each set of two joints + for (int i=0; i<6; i++) + { + temp_x[i] = target_pose.kinematic.position(0) + cos(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.911)*(-0.07825); + temp_y[i] = target_pose.kinematic.position(1) + sin(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.911)*(-0.07825); + temp_z[i] = target_pose.kinematic.position(2); + } + + // Goal Pose for each set of two joints after tool rotation + goal_orientation = target_pose.kinematic.orientation; + if (goal_orientation(0,0) && goal_orientation(0,1) && goal_orientation(0,2) + && goal_orientation(1,0) && goal_orientation(1,1) && goal_orientation(1,2) + && goal_orientation(2,0) && goal_orientation(2,1) && goal_orientation(2,2)) + { + goal_orientation(0,0) = 1; + goal_orientation(1,1) = 1; + goal_orientation(2,2) = 1; + } + for (int i=0; i<6; i++) + { + target_x[i] = goal_orientation(0,0)*temp_x[i] + goal_orientation(0,1)*temp_y[i] + goal_orientation(0,2)*temp_z[i]; + target_y[i] = goal_orientation(1,0)*temp_x[i] + goal_orientation(1,1)*temp_y[i] + goal_orientation(1,2)*temp_z[i]; + target_z[i] = goal_orientation(2,0)*temp_x[i] + goal_orientation(2,1)*temp_y[i] + goal_orientation(2,2)*temp_z[i]; + } + + // Pose difference for each set of two joints + for (int i=0; i<6; i++) + { + diff_x[i] = target_x[i] - start_x[i]; + diff_y[i] = target_y[i] - start_y[i]; + diff_z[i] = target_z[i] - start_z[i]; + } + + for (int i=0; i<6; i++) + { + temp[i] = -diff_x[i]*sin(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436)+diff_y[i]*cos(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436); + temp2[i] = sqrt(temp[i]*temp[i] + diff_z[i]*diff_z[i]); + + temp_angle[i] = asin((link[1]*link[1] - link[0]*link[0] - diff_x[i]*diff_x[i] - diff_y[i]*diff_y[i] - diff_z[i]*diff_z[i]) + / (2.0*link[0]*temp2[i])); + temp_angle2[i] = pow(-1,i%2)*asin(temp[i] / temp2[i]); + + target_angle[i].position = -temp_angle[i] + temp_angle2[i]; + } + target_angle[1].position = -target_angle[1].position; + target_angle[3].position = -target_angle[3].position; + target_angle[5].position = -target_angle[5].position; + + target_angle_vector.push_back(target_angle[0]); + target_angle_vector.push_back(target_angle[1]); + target_angle_vector.push_back(target_angle[2]); + target_angle_vector.push_back(target_angle[3]); + target_angle_vector.push_back(target_angle[4]); + target_angle_vector.push_back(target_angle[5]); + + // Elbow pose + for (int i=0; i<6; i++) + { + // Adjust motor rotation direction + target_angle[i].position = -target_angle[i].position; + + elbow_x[i] = start_x[i] - sin(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436)*link[0]*(-pow(-1,i%2))*cos(target_angle[i].position); + elbow_y[i] = start_y[i] + cos(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436)*link[0]*(-pow(-1,i%2))*cos(target_angle[i].position); + elbow_z[i] = start_z[i] + link[0]*(-pow(-1,i%2))*sin(target_angle[i].position); + + temp_elbow_x[i] = cos(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*elbow_x[i] - sin(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*elbow_y[i]; + temp_elbow_y[i] = sin(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*elbow_x[i] + cos(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*elbow_y[i]; + temp_elbow_z[i] = elbow_z[i]; + temp_target_x[i] = cos(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*target_x[i] - sin(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*target_y[i]; + temp_target_y[i] = sin(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*target_x[i] + cos(-(PI*2.0/3.0*(i/2) - pow(-1,i%2)*0.436))*target_y[i]; + temp_target_z[i] = target_z[i]; + } + + for (int i=0; i<6; i++) + { + diff_x2[i] = temp_target_x[i] - temp_elbow_x[i]; + diff_y2[i] = temp_target_y[i] - temp_elbow_y[i]; + diff_z2[i] = temp_target_z[i] - temp_elbow_z[i]; + + target_angle[2*i+7].position = asin(diff_x2[i] / (pow(-1,i%2)*link[1])); + target_angle[2*i+6].position = +target_angle[i].position + pow(-1,i%2)*PI + asin(diff_z2[i]/(-link[1]*cos(target_angle[2*i+7].position)*pow(-1,i%2))); + + // Set initial value to 0 + target_angle[2*i+6].position = target_angle[2*i+6].position - pow(-1,i%2)*2.100; + target_angle[2*i+7].position = target_angle[2*i+7].position - pow(-1,i%2)*0.064; + + target_angle_vector.push_back(target_angle[2*i+6]); + target_angle_vector.push_back(target_angle[2*i+7]); + } + + target_angle[18].position = target_pose.kinematic.position(0); + target_angle[19].position = target_pose.kinematic.position(1); + target_angle[20].position = target_pose.kinematic.position(2); + target_angle_vector.push_back(target_angle[18]); + target_angle_vector.push_back(target_angle[19]); + target_angle_vector.push_back(target_angle[20]); + + *goal_joint_value = target_angle_vector; + manipulator->setAllJointValue(target_angle_vector); + + return true; +} + + + + + + + + // for (int i=0; i<6; i++) + // { + // diff_x3[i] = temp_elbow_x[i] - (-0.0774); + // diff_y3[i] = temp_elbow_y[i] - 0; + // diff_z3[i] = temp_elbow_z[i] - (-0.1057); + // } + + // for (int i=0; i<6; i++) + // { + // temp_target_angle[i] = acos(diff_z2[i] / sqrt(diff_y3[i]*diff_y3[i] + diff_z3[i]*diff_z3[i]) * link[0] / link[1]); + // temp_target_angle2[i] = acos(diff_z3[i] / sqrt(diff_y3[i]*diff_y3[i] + diff_z3[i]*diff_z3[i])); + + // if (i%2 == 0) + // { + // target_angle[2*i+6].position = -(temp_target_angle[i] + temp_target_angle2[i]); + // } + // else + // { + // target_angle[2*i+6].position = (temp_target_angle[i] + temp_target_angle2[i]); + // } + // } + + // for (int i=0; i<6; i++) + // { + // temp_diffx3[i] = diff_x3[i]; + // temp_diffy3[i] = cos(target_angle[2*i+6].position)*diff_y3[i] - sin(target_angle[2*i+6].position)*diff_z3[i]; + // temp_diffz3[i] = sin(target_angle[2*i+6].position)*diff_y3[i] + cos(target_angle[2*i+6].position)*diff_z3[i]; + // } + + // for (int i=0; i<6; i++){ + // temp_target_angle3[i] = acos(diff_x2[i] / sqrt(temp_diffx3[i]*temp_diffx3[i] + temp_diffy3[i]*temp_diffy3[i]) * link[0] / link[1]); + // temp_target_angle4[i] = acos(temp_diffx3[i] / sqrt(temp_diffx3[i]*temp_diffx3[i] + temp_diffy3[i]*temp_diffy3[i])); + // target_angle[2*i+7].position = temp_target_angle3[i] - temp_target_angle4[i]; + // } + + // for (int i=0; i<6; i++) + // { + // // target_angle[2*i+6].position = 0; + // // target_angle[2*i+7].position = 0; + // // target_angle[2*i+6].position = target_angle[2*i+6].position - 2.103914; + // // target_angle[2*i+7].position = target_angle[2*i+7].position*75/125.8 + 0.075; + // target_angle_vector.push_back(target_angle[2*i+6]); + // target_angle_vector.push_back(target_angle[2*i+7]); + // }