Skip to content

Commit

Permalink
Support angle4th on Moveit! for swiftpro
Browse files Browse the repository at this point in the history
  • Loading branch information
@asukiaaa
asukiaaa committed Feb 7, 2019
1 parent 87c16b4 commit 9e00609
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 22 deletions.
2 changes: 1 addition & 1 deletion pro_moveit_config/config/joint_limits.yaml
View file
Expand Up @@ -17,7 +17,7 @@ joint_limits:
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
Joint8:
Joint10:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
Expand Down
15 changes: 15 additions & 0 deletions pro_moveit_config/launch/moveit.rviz
View file
Expand Up @@ -117,6 +117,11 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
Link10:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: true
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Expand Down Expand Up @@ -212,6 +217,11 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
Link10:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 0.5
Show Robot Collision: false
Show Robot Visual: true
Expand Down Expand Up @@ -276,6 +286,11 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
Link10:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Expand Down
43 changes: 32 additions & 11 deletions swiftpro/src/swiftpro_moveit_node.cpp
View file
Expand Up @@ -9,6 +9,7 @@
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <swiftpro/position.h>
#include <swiftpro/angle4th.h>

#define MATH_PI 3.141592653589793238463
#define MATH_TRANS 57.2958
Expand All @@ -17,14 +18,15 @@
#define MATH_LOWER_ARM 142.07
#define MATH_UPPER_ARM 158.81

float motor_angle[3] = {90.0, 90.0, 0.0};

#define ANGLE_NUM 4
float motor_angle[ANGLE_NUM] = {90.0, 90.0, 0.0, 90.0};

/*
* Description: forward kinematics of swift pro
* Inputs: angle[3] 3 motor angles(degree)
* Inputs: angle[4] 4 motor angles(degree)
* Outputs: position[3] 3 cartesian coordinates: x, y, z(mm)
*/
void swift_fk(float angle[3], float position[3])
void swift_fk(float angle[ANGLE_NUM], float position[3])
{
double stretch = MATH_LOWER_ARM * cos(angle[1] / MATH_TRANS)
+ MATH_UPPER_ARM * cos(angle[2] / MATH_TRANS) + MATH_L2 + 56.55;
Expand All @@ -40,14 +42,29 @@ void swift_fk(float angle[3], float position[3])

/*
* Description: callback when receive data from move_group/fake_controller_joint_states
* Inputs: msg 3 necessary joints for kinematic chain(degree)
* Outputs: motor_angle[3] 3 motor angles(degree)
* Inputs: msg 4 necessary joints for kinematic chain(degree)
* Outputs: motor_angle[4] 4 motor angles(degree)
*/
void joint_Callback(const sensor_msgs::JointState& msg)
{
motor_angle[0] = msg.position[0] * 57.2958 + 90;
motor_angle[1] = 90 - msg.position[1] * 57.2958;
motor_angle[2] = (msg.position[1] + msg.position[2]) * 57.2958;
float raw_angle[ANGLE_NUM];
for(int i=0; i<ANGLE_NUM; ++i) {
int raw_index;
if (msg.name[i] == "Joint1") {
raw_index = 0;
} else if (msg.name[i] == "Joint2") {
raw_index = 1;
} else if (msg.name[i] == "Joint3") {
raw_index = 2;
} else {
raw_index = 3;
}
raw_angle[raw_index] = msg.position[i];
}
motor_angle[0] = raw_angle[0] * 57.2958 + 90;
motor_angle[1] = 90 - raw_angle[1] * 57.2958;
motor_angle[2] = (raw_angle[1] + raw_angle[2]) * 57.2958;
motor_angle[3] = 90 - raw_angle[3] * 57.2958;
}


Expand All @@ -65,12 +82,14 @@ int main(int argc, char **argv)
{
float position[3];
swiftpro::position pos;
swiftpro::angle4th a4;

ros::init(argc, argv, "swiftpro_moveit_node");
ros::NodeHandle n;

ros::Subscriber sub = n.subscribe("move_group/fake_controller_joint_states", 1, joint_Callback);
ros::Publisher pub = n.advertise<swiftpro::position>("position_write_topic", 1);
ros::Publisher pub_pos = n.advertise<swiftpro::position>("write_position_topic", 1);
ros::Publisher pub_angle4 = n.advertise<swiftpro::angle4th>("write_angle4th_topic", 1);
ros::Rate loop_rate(20);

while (ros::ok())
Expand All @@ -79,7 +98,9 @@ int main(int argc, char **argv)
pos.x = position[0];
pos.y = position[1];
pos.z = position[2];
pub.publish(pos);
pub_pos.publish(pos);
a4.angle4th = motor_angle[3];
pub_angle4.publish(a4);

ros::spinOnce();
loop_rate.sleep();
Expand Down
20 changes: 10 additions & 10 deletions swiftpro/src/swiftpro_write_node.cpp
View file
Expand Up @@ -22,7 +22,7 @@ swiftpro::SwiftproState pos;
* Inputs: msg(float) 3 cartesian coordinates: x, y, z(mm)
* Outputs: Gcode send gcode to control swift pro
*/
void position_write_callback(const swiftpro::position& msg)
void write_position_callback(const swiftpro::position& msg)
{
std::string Gcode = "";
std_msgs::String result;
Expand All @@ -48,7 +48,7 @@ void position_write_callback(const swiftpro::position& msg)
* Inputs: msg(float) angle of 4th motor(degree)
* Outputs: Gcode send gcode to control swift pro
*/
void angle4th_callback(const swiftpro::angle4th& msg)
void write_angle4th_callback(const swiftpro::angle4th& msg)
{
std::string Gcode = "";
std_msgs::String result;
Expand All @@ -68,7 +68,7 @@ void angle4th_callback(const swiftpro::angle4th& msg)
* Inputs: msg(uint8) status of gripper: attach if 1; detach if 0
* Outputs: Gcode send gcode to control swift pro
*/
void swiftpro_status_callback(const swiftpro::status& msg)
void write_status_callback(const swiftpro::status& msg)
{
std::string Gcode = "";
std_msgs::String result;
Expand All @@ -95,7 +95,7 @@ void swiftpro_status_callback(const swiftpro::status& msg)
* Inputs: msg(uint8) status of gripper: work if 1; otherwise 0
* Outputs: Gcode send gcode to control swift pro
*/
void gripper_callback(const swiftpro::status& msg)
void write_gripper_callback(const swiftpro::status& msg)
{
std::string Gcode = "";
std_msgs::String result;
Expand All @@ -122,7 +122,7 @@ void gripper_callback(const swiftpro::status& msg)
* Inputs: msg(uint8) status of pump: work if 1; otherwise 0
* Outputs: Gcode send gcode to control swift pro
*/
void pump_callback(const swiftpro::status& msg)
void write_pump_callback(const swiftpro::status& msg)
{
std::string Gcode = "";
std_msgs::String result;
Expand Down Expand Up @@ -164,11 +164,11 @@ int main(int argc, char** argv)
ros::NodeHandle nh;
swiftpro::SwiftproState swiftpro_state;

ros::Subscriber sub1 = nh.subscribe("position_write_topic", 1, position_write_callback);
ros::Subscriber sub2 = nh.subscribe("swiftpro_status_topic", 1, swiftpro_status_callback);
ros::Subscriber sub3 = nh.subscribe("angle4th_topic", 1, angle4th_callback);
ros::Subscriber sub4 = nh.subscribe("gripper_topic", 1, gripper_callback);
ros::Subscriber sub5 = nh.subscribe("pump_topic", 1, pump_callback);
ros::Subscriber sub1 = nh.subscribe("write_position_topic", 1, write_position_callback);
ros::Subscriber sub2 = nh.subscribe("write_status_topic", 1, write_status_callback);
ros::Subscriber sub3 = nh.subscribe("write_angle4th_topic", 1, write_angle4th_callback);
ros::Subscriber sub4 = nh.subscribe("write_gripper_topic", 1, write_gripper_callback);
ros::Subscriber sub5 = nh.subscribe("write_pump_topic", 1, write_pump_callback);
ros::Publisher pub = nh.advertise<swiftpro::SwiftproState>("SwiftproState_topic", 1);
ros::Rate loop_rate(20);

Expand Down

0 comments on commit 9e00609

Please sign in to comment.