diff --git a/docker_env/gazebo/Dockerfile b/docker_env/gazebo/Dockerfile index 5f799a1..f6fecd2 100644 --- a/docker_env/gazebo/Dockerfile +++ b/docker_env/gazebo/Dockerfile @@ -17,6 +17,7 @@ RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc RUN apt install -y python-rosdep RUN rosdep init RUN rosdep update +RUN apt-get install -y ros-melodic-pid # user id 1000 should be the same as the host user, so that you can access files # from inside the docker container and also on the host RUN useradd -u 1000 rrrobot diff --git a/rrrobot_src/src/arm_control/CMakeLists.txt b/rrrobot_src/src/arm_control/CMakeLists.txt index 4d10e11..c6a915a 100644 --- a/rrrobot_src/src/arm_control/CMakeLists.txt +++ b/rrrobot_src/src/arm_control/CMakeLists.txt @@ -149,6 +149,9 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_node src/arm_controller.cpp src/arm.cpp) add_executable(test test/test.cpp src/arm.cpp) +add_executable(arm_movement_demo test/arm_movement_demo.cpp src/arm.cpp) +add_executable(pub_sub test/test_pub_sub.cpp) +add_executable(angles_subscriber test/angles_subscriber.cpp src/arm.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -177,6 +180,30 @@ target_link_libraries(test ignition-math4::ignition-math4 ) +target_link_libraries(arm_movement_demo + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +target_link_libraries(pub_sub + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + +target_link_libraries(angles_subscriber + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math4::ignition-math4 +) + ############# ## Install ## ############# diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp index a7b9422..d24aaa4 100644 --- a/rrrobot_src/src/arm_control/src/arm.cpp +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -58,6 +58,15 @@ Arm::Arm(const std::string &sdf_file) const string name(link->Get("name")); links[name].link_name = name; + // Transformation from world to link coordinates + stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString()); + float x, y, z; + float roll, pitch, yaw; + frame_location >> x >> y >> z >> roll >> pitch >> yaw; + KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw); + KDL::Vector link_position(x, y, z); + links[name].link_frame = KDL::Frame(rotation, link_position); + const sdf::ElementPtr inertial_data = link->GetElement("inertial"); float mass = inertial_data->Get("mass"); links[name].mass = mass; @@ -65,7 +74,8 @@ Arm::Arm(const std::string &sdf_file) // Get the center of mass for this link stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString()); - inertial_data_ss >> links[name].com_location.data[0] >> links[name].com_location.data[1] >> links[name].com_location[2]; + inertial_data_ss >> links[name].com_location[0] >> links[name].com_location[1] >> links[name].com_location[2]; + links[name].com_location += links[name].link_frame.p; const sdf::ElementPtr inertia = inertial_data->GetElement("inertia"); links[name].rotational_inertia = KDL::RotationalInertia( @@ -76,15 +86,6 @@ Arm::Arm(const std::string &sdf_file) inertia->Get("ixz"), inertia->Get("iyz")); - // Transformation from world to link coordinates - stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString()); - float x, y, z; - float roll, pitch, yaw; - frame_location >> x >> y >> z >> roll >> pitch >> yaw; - KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw); - KDL::Vector link_position(x, y, z); - links[name].link_frame = KDL::Frame(rotation, link_position); - link = link->GetNextElement("link"); } @@ -146,6 +147,7 @@ Arm::Arm(const std::string &sdf_file) KDL::Frame prev_frame_inverse = prev_frame.Inverse(); KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world; + KDL::Vector com_in_prev = joint_relative_to_prev * links[cur_link_name].com_location; // // EXPERIMENTAL - update rotation axis // KDL::Vector uncorrected_axis(0, 0, 0); @@ -184,7 +186,7 @@ Arm::Arm(const std::string &sdf_file) // // END EXPERIMENTAL KDL::Joint::JointType next(links[cur_link_name].joint.getType()); - KDL::RigidBodyInertia inertia(links[cur_link_name].mass, links[cur_link_name].com_location, links[cur_link_name].rotational_inertia); + KDL::RigidBodyInertia inertia(links[cur_link_name].mass, com_in_prev /*links[cur_link_name].com_location*/, links[cur_link_name].rotational_inertia); KDL::Segment to_add(links[cur_link_name].link_name, KDL::Joint(links[cur_link_name].joint.getName(), to_set), joint_relative_to_prev, inertia); to_set = next; @@ -307,6 +309,9 @@ void Arm::print() const cout << seg.getName() << endl; cout << seg.getFrameToTip() << endl; cout << seg.getJoint().getTypeName() << endl; + cout << "Inertial frame:" << endl; + cout << "\tmass: " << seg.getInertia().getMass() << endl; + cout << '\t' << seg.getInertia().getCOG() << endl; cout << endl; } } diff --git a/rrrobot_src/src/arm_control/src/arm_controller.cpp b/rrrobot_src/src/arm_control/src/arm_controller.cpp index ff25c89..a54e74f 100644 --- a/rrrobot_src/src/arm_control/src/arm_controller.cpp +++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "ros/ros.h" #include @@ -14,6 +15,7 @@ using std::cout; using std::endl; +using std::vector; using namespace KDL; Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); @@ -21,10 +23,50 @@ KDL::Chain chain = arm.getArm(); KDL::Chain simple_chain; ros::Publisher publisher; +void enforce_torque_limits(simulation_env::arm_command &cmd, double limit = 2000.0) +{ + if (cmd.shoulder_pivot_force > limit) + cmd.shoulder_pivot_force = limit; + else if (cmd.shoulder_pivot_force < -limit) + cmd.shoulder_pivot_force = -limit; + + if (cmd.shoulder_joint_force > limit) + cmd.shoulder_joint_force = limit; + else if (cmd.shoulder_joint_force < -limit) + cmd.shoulder_joint_force = -limit; + + if (cmd.elbow_joint_force > limit) + cmd.elbow_joint_force = limit; + else if (cmd.elbow_joint_force < -limit) + cmd.elbow_joint_force = -limit; + + if (cmd.wrist_pivot_force > limit) + cmd.wrist_pivot_force = limit; + else if (cmd.wrist_pivot_force < -limit) + cmd.wrist_pivot_force = -limit; + + if (cmd.wrist_joint_force > limit) + cmd.wrist_joint_force = limit; + else if (cmd.wrist_joint_force < -limit) + cmd.wrist_joint_force = -limit; + + if (cmd.end_effector_pivot_force > limit) + cmd.end_effector_pivot_force = limit; + else if (cmd.end_effector_pivot_force < -limit) + cmd.end_effector_pivot_force = -limit; +} + void angle_callback(const simulation_env::arm_angles &msg) { // Create solver based on kinematic chain static ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain); + static KDL::JntArray prev_pos = KDL::JntArray(chain.getNrOfJoints()); + static KDL::JntArray prev_vel = KDL::JntArray(chain.getNrOfJoints()); + static double prev_time; // = ros::Time::now(); + static bool first_call = true; + static simulation_env::arm_command last_command; // = {.shoulder_pivot_force = 0, .shoulder_joint_force = 0, .elbow_joint_force = 0, .wrist_pivot_force = 0, .wrist_joint_force = 0, .end_effector_pivot_force = 0}; + + double cur_time = ros::Time::now().toSec(); // Create joint array unsigned int nj = chain.getNrOfJoints(); @@ -40,53 +82,122 @@ void angle_callback(const simulation_env::arm_angles &msg) // Create the frame that will contain the results KDL::Frame cartpos; - // Calculate forward position kinematics - bool kinematics_status; - kinematics_status = fksolver.JntToCart(jointpositions, cartpos); - if (kinematics_status >= 0) - { - // cout << nj << endl; - // cout << "shoulder_pivot: " << jointpositions(0) << endl; - // cout << "shoulder_joint: " << jointpositions(1) << endl; - // cout << "elbow_joint: " << jointpositions(2) << endl; - // cout << "wrist_pivot: " << jointpositions(3) << endl; - // cout << "wrist_joint: " << jointpositions(4) << endl; - // cout << "end_effector_pivot: " << jointpositions(5) << endl; - std::cout << cartpos.p << std::endl; - double roll, pitch, yaw; - cartpos.M.GetRPY(roll, pitch, yaw); - cout << "roll: " << roll << endl; - cout << "pitch: " << pitch << endl; - cout << "yaw: " << yaw << endl; - cout << endl; - // std::cout << cartpos << std::endl; - //printf("%s \n", "Succes, thanks KDL!"); - } - else - { - //printf("%s \n", "Error: could not calculate forward kinematics :("); - } + // // Calculate forward position kinematics + // bool kinematics_status; + // kinematics_status = fksolver.JntToCart(jointpositions, cartpos); + // if (kinematics_status >= 0) + // { + // // cout << nj << endl; + // // cout << "shoulder_pivot: " << jointpositions(0) << endl; + // // cout << "shoulder_joint: " << jointpositions(1) << endl; + // // cout << "elbow_joint: " << jointpositions(2) << endl; + // // cout << "wrist_pivot: " << jointpositions(3) << endl; + // // cout << "wrist_joint: " << jointpositions(4) << endl; + // // cout << "end_effector_pivot: " << jointpositions(5) << endl; + + // std::cout << cartpos.p << std::endl; + // double roll, pitch, yaw; + // cartpos.M.GetRPY(roll, pitch, yaw); + // cout << "roll: " << roll << endl; + // cout << "pitch: " << pitch << endl; + // cout << "yaw: " << yaw << endl; + // cout << endl; + + // // std::cout << cartpos << std::endl; + // //printf("%s \n", "Succes, thanks KDL!"); + // } + // else + // { + // //printf("%s \n", "Error: could not calculate forward kinematics :("); + // } // try to move arm to (1, 1, 1) + double vel_default = 0.0; KDL::JntArray vel(arm.getArm().getNrOfJoints()); KDL::JntArray accel(arm.getArm().getNrOfJoints()); + + // for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint) + // { + // // vel(joint) = 0; + // accel(joint) = 0; + // } + + vector last_command_values; + last_command_values.push_back(last_command.shoulder_pivot_force); + last_command_values.push_back(last_command.shoulder_joint_force); + last_command_values.push_back(last_command.elbow_joint_force); + last_command_values.push_back(last_command.wrist_pivot_force); + last_command_values.push_back(last_command.wrist_joint_force); + last_command_values.push_back(last_command.end_effector_pivot_force); + + for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint) + { + KDL::Vector axis = arm.getArm().getSegment(joint).getJoint().JointAxis(); + double inertia = KDL::dot(arm.getArm().getSegment(joint).getInertia().getRotationalInertia() * axis, axis); /*axis of rotation */ + accel(joint) = (true || first_call || (inertia == 0)) ? 0.0 : last_command_values[joint] / inertia; + } + + vel(0) = msg.shoulder_pivot_velocity; + vel(1) = msg.shoulder_joint_velocity; + vel(2) = msg.elbow_joint_velocity; + vel(3) = 0; //msg.wrist_pivot_velocity; + vel(4) = msg.wrist_joint_velocity; + vel(5) = 0; //msg.end_effector_pivot_velocity; + // vel(0) = 0; //(jointpositions(0) - prev_pos(0); //vel_default; //jointpositions(0) - msg.shoulder_pivot_angle; + // vel(1) = 0; //vel_default; //jointpositions(1) - msg.shoulder_joint_angle; + // vel(2) = 0; //vel_default; //jointpositions(2) - msg.elbow_joint_angle; + // vel(3) = 0; //vel_default; //jointpositions(3) - msg.wrist_pivot_angle; + // vel(4) = 0; //vel_default; //jointpositions(4) - msg.wrist_joint_angle; + // vel(5) = 0; //vel_default; //jointpositions(5) - msg.end_effector_pivot_angle; + // } + // else + // { + // for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint) + // { + // vel(joint) = (jointpositions(0) - prev_pos(0)) / (cur_time - prev_time); + // accel(joint) = (vel(joint) - prev_vel(joint)) / (cur_time - prev_time); + // } + // // vel(0) = (jointpositions(0) - prev_pos(0)) / (cur_time - prev_time); //vel_default; //jointpositions(0) - msg.shoulder_pivot_angle; + // // vel(1) = vel_default; //jointpositions(1) - msg.shoulder_joint_angle; + // // vel(2) = vel_default; //jointpositions(2) - msg.elbow_joint_angle; + // // vel(3) = vel_default; //jointpositions(3) - msg.wrist_pivot_angle; + // // vel(4) = vel_default; //jointpositions(4) - msg.wrist_joint_angle; + // // vel(5) = vel_default; //jointpositions(5) - msg.end_effector_pivot_angle; + // } KDL::Wrenches ext_force(arm.getArm().getNrOfSegments()); KDL::JntArray required_force(arm.getArm().getNrOfJoints()); KDL::Frame desired(KDL::Vector(1, 1, 1)); KDL::JntArray required_positions(arm.getArm().getNrOfJoints()); - bool failed = arm.calculateInverseKinematics(jointpositions, desired, required_positions); - int error_val = arm.calculateInverseDynamics(required_positions, vel, accel, ext_force, required_force); + // bool failed = arm.calculateInverseKinematics(jointpositions, desired, required_positions); + // int error_val = arm.calculateInverseDynamics(required_positions, vel, accel, ext_force, required_force); + int error_val = arm.calculateInverseDynamics(jointpositions, vel, accel, ext_force, required_force); simulation_env::arm_command cmd; - cmd.shoulder_pivot_force = required_force(0); - cmd.shoulder_joint_force = -required_force(1); - cmd.elbow_joint_force = required_force(2); - cmd.wrist_pivot_force = required_force(3); - cmd.wrist_joint_force = required_force(4); - cmd.end_effector_pivot_force = required_force(5); + cmd.shoulder_pivot_force = 0; ///* -*/ required_force(0); // last added + cmd.shoulder_joint_force = required_force(1); + cmd.elbow_joint_force = /* -*/ required_force(2); + cmd.wrist_pivot_force = 0; ///*-*/ -required_force(3); + cmd.wrist_joint_force = /* -*/ -required_force(4); + cmd.end_effector_pivot_force = 0; ///*-*/ required_force(5); + + enforce_torque_limits(cmd, 10000); + + // cout << "Command force:" << endl; + // cout << "\tShoulder pivot: " << cmd.shoulder_pivot_force << endl; + // cout << "\tShoulder joint: " << cmd.shoulder_joint_force << endl; + // cout << "\tElbow joint: " << cmd.elbow_joint_force << endl; + // cout << "\tWrist pivot: " << cmd.wrist_pivot_force << endl; + // cout << "\tWrist joint: " << cmd.wrist_joint_force << endl; + // cout << "\tEnd effector pivot: " << cmd.end_effector_pivot_force << endl; publisher.publish(cmd); ros::spinOnce(); + + first_call = false; + prev_pos = jointpositions; + prev_vel = vel; + prev_time = cur_time; + last_command = cmd; } int main(int argc, char **argv) @@ -94,6 +205,8 @@ int main(int argc, char **argv) cout << "Starting arm controller..." << endl; ros::init(argc, argv, "arm_control_test"); + arm.print(); + // correct.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0)))); // correct.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); // correct.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); diff --git a/rrrobot_src/src/arm_control/test/angles_subscriber.cpp b/rrrobot_src/src/arm_control/test/angles_subscriber.cpp new file mode 100644 index 0000000..0dedd1b --- /dev/null +++ b/rrrobot_src/src/arm_control/test/angles_subscriber.cpp @@ -0,0 +1,153 @@ +#include +#include "ros/ros.h" +#include + +#include + +#include +#include +#include + +using std::cout; +using std::endl; +using std::vector; + +std_msgs::Float64 shoulder_pivot_angle; +std_msgs::Float64 shoulder_joint_angle; +std_msgs::Float64 elbow_joint_angle; +std_msgs::Float64 wrist_pivot_angle; +std_msgs::Float64 wrist_joint_angle; +std_msgs::Float64 end_effector_pivot_angle; +Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); +KDL::JntArray pos(arm.getArm().getNrOfJoints()); +char received_angles = 0; + +void shoulder_pivot_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + shoulder_pivot_angle = cmd; + pos(0) = cmd.data; + cout << "shoulder pivot: " << shoulder_pivot_angle.data << endl; + received_angles |= 0x01; +} + +void shoulder_joint_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + shoulder_joint_angle = cmd; + pos(1) = cmd.data; + cout << "shoulder joint: " << shoulder_joint_angle.data << endl; + received_angles |= 0x02; +} + +void elbow_joint_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + elbow_joint_angle = cmd; + pos(2) = cmd.data; + cout << "elbow joint: " << elbow_joint_angle.data << endl; + received_angles |= 0x04; +} + +void wrist_pivot_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + wrist_pivot_angle = cmd; + pos(3) = cmd.data; + cout << "wrist pivot: " << wrist_pivot_angle.data << endl; + received_angles |= 0x08; +} + +void wrist_joint_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + wrist_joint_angle = cmd; + pos(4) = cmd.data; + cout << "wrist joint: " << wrist_joint_angle.data << endl; + received_angles |= 0x10; +} + +void end_effector_pivot_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + end_effector_pivot_angle = cmd; + pos(5) = cmd.data; + cout << "end effector pivot: " << end_effector_pivot_angle.data << endl; + received_angles |= 0x20; +} + +void fix_angle(double cur_pos, double &desired) +{ + while (desired > M_PI) + { + desired -= 2 * M_PI; + } + while (desired < -M_PI) + { + desired += 2 * M_PI; + } + + double cur_pos_truncated = cur_pos; // % (M_PI); + while (cur_pos_truncated > M_PI) + { + cur_pos_truncated -= 2 * M_PI; + } + while (cur_pos_truncated < -M_PI) + { + cur_pos_truncated += 2 * M_PI; + } + // say desired = 1.5708 + // 4 cases: + // 1. cur_pos = 0.2 --> set target as cur_pos + 1.3708 + // 2. cur_pos = -0.2 --> set target as cur_pos + 1.7708 + // 3. cur_pos = -2.5 --> set target as cur_pos + 2.21239 + // 4. cur_pos = 2.5 --> set target as cur_pos + 0.9292 + + if ((cur_pos_truncated - desired) < M_PI && (cur_pos_truncated - desired) > 0) + { + desired = cur_pos - (cur_pos_truncated - desired); + } + else if ((desired - cur_pos_truncated) < M_PI && (desired - cur_pos_truncated) > 0) + { + desired = cur_pos + (desired - cur_pos_truncated); + } + else if ((cur_pos_truncated - desired) > -M_PI && (cur_pos_truncated - desired) < 0) + { + desired = cur_pos - (cur_pos_truncated - desired); + } + else if ((desired - cur_pos_truncated) > -M_PI && (desired - cur_pos_truncated) < 0) + { + desired = cur_pos + (desired - cur_pos_truncated); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "arm_control_demo"); + + ros::Subscriber shoulder_pivot_sub; + ros::Subscriber shoulder_joint_sub; + ros::Subscriber elbow_joint_sub; + ros::Subscriber wrist_pivot_sub; + ros::Subscriber wrist_joint_sub; + ros::Subscriber end_effector_sub; + + ros::NodeHandle nh; + ros::Rate pub_rate(0.25); // publish every 10 seconds + ros::Rate quick(1); + + shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback); + shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback); + elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback); + wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback); + wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback); + end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback); + + ros::spin(); + // while (ros::ok()) + // { + // quick.sleep(); + + // ros::spinOnce(); + // } +} \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp new file mode 100644 index 0000000..e35cef5 --- /dev/null +++ b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp @@ -0,0 +1,259 @@ +#include +#include "ros/ros.h" +#include + +#include + +#include +#include +#include + +using std::cout; +using std::endl; +using std::vector; + +std_msgs::Float64 shoulder_pivot_angle; +std_msgs::Float64 shoulder_joint_angle; +std_msgs::Float64 elbow_joint_angle; +std_msgs::Float64 wrist_pivot_angle; +std_msgs::Float64 wrist_joint_angle; +std_msgs::Float64 end_effector_pivot_angle; +Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); +KDL::JntArray pos(arm.getArm().getNrOfJoints()); +char received_angles = 0; + +void shoulder_pivot_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + shoulder_pivot_angle = cmd; + pos(0) = cmd.data; + // cout << "shoulder pivot: " << shoulder_pivot_angle.data << endl; + received_angles |= 0x01; +} + +void shoulder_joint_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + shoulder_joint_angle = cmd; + pos(1) = cmd.data; + // cout << "shoulder joint: " << shoulder_joint_angle.data << endl; + received_angles |= 0x02; +} + +void elbow_joint_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + elbow_joint_angle = cmd; + pos(2) = cmd.data; + // cout << "elbow joint: " << elbow_joint_angle.data << endl; + received_angles |= 0x04; +} + +void wrist_pivot_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + wrist_pivot_angle = cmd; + pos(3) = cmd.data; + // cout << "wrist pivot: " << wrist_pivot_angle.data << endl; + received_angles |= 0x08; +} + +void wrist_joint_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + wrist_joint_angle = cmd; + pos(4) = cmd.data; + // cout << "wrist joint: " << wrist_joint_angle.data << endl; + received_angles |= 0x10; +} + +void end_effector_pivot_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + end_effector_pivot_angle = cmd; + pos(5) = cmd.data; + // cout << "end effector pivot: " << end_effector_pivot_angle.data << endl; + received_angles |= 0x20; +} + +void fix_angle(double cur_pos, double &desired) +{ + while (desired > M_PI) + { + desired -= 2 * M_PI; + } + while (desired < -M_PI) + { + desired += 2 * M_PI; + } + + double cur_pos_truncated = cur_pos; // % (M_PI); + while (cur_pos_truncated > M_PI) + { + cur_pos_truncated -= 2 * M_PI; + } + while (cur_pos_truncated < -M_PI) + { + cur_pos_truncated += 2 * M_PI; + } + // say desired = 1.5708 + // 4 cases: + // 1. cur_pos = 0.2 --> set target as cur_pos + 1.3708 + // 2. cur_pos = -0.2 --> set target as cur_pos + 1.7708 + // 3. cur_pos = -2.5 --> set target as cur_pos + 2.21239 + // 4. cur_pos = 2.5 --> set target as cur_pos + 0.9292 + + if ((cur_pos_truncated - desired) < M_PI && (cur_pos_truncated - desired) > 0) + { + desired = cur_pos - (cur_pos_truncated - desired); + } + else if ((desired - cur_pos_truncated) < M_PI && (desired - cur_pos_truncated) > 0) + { + desired = cur_pos + (desired - cur_pos_truncated); + } + else if ((cur_pos_truncated - desired) > -M_PI && (cur_pos_truncated - desired) < 0) + { + desired = cur_pos - (cur_pos_truncated - desired); + } + else if ((desired - cur_pos_truncated) > -M_PI && (desired - cur_pos_truncated) < 0) + { + desired = cur_pos + (desired - cur_pos_truncated); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "arm_control_demo"); + + KDL::SetToZero(pos); + + ros::Publisher shoulder_pivot_publisher; + ros::Publisher shoulder_joint_publisher; + ros::Publisher elbow_joint_publisher; + ros::Publisher wrist_pivot_publisher; + ros::Publisher wrist_joint_publisher; + ros::Publisher end_effector_pivot_publisher; + + ros::Subscriber shoulder_pivot_sub; + ros::Subscriber shoulder_joint_sub; + ros::Subscriber elbow_joint_sub; + ros::Subscriber wrist_pivot_sub; + ros::Subscriber wrist_joint_sub; + ros::Subscriber end_effector_sub; + + std_msgs::Float64 shoulder_pivot_target; + shoulder_pivot_target.data = 0; + std_msgs::Float64 shoulder_joint_target; + shoulder_joint_target.data = 0; + std_msgs::Float64 elbow_joint_target; + elbow_joint_target.data = 0; + std_msgs::Float64 wrist_pivot_target; + wrist_pivot_target.data = 0; + std_msgs::Float64 wrist_joint_target; + wrist_joint_target.data = 0; + std_msgs::Float64 end_effector_pivot_target; + end_effector_pivot_target.data = 0; + + ros::NodeHandle nh; + ros::Rate pub_rate(0.1); // publish every 10 seconds + ros::Rate quick(1); + + shoulder_pivot_publisher = nh.advertise("/arm_node/shoulder_pivot_setpoint", 1000); + shoulder_joint_publisher = nh.advertise("/arm_node/shoulder_joint_setpoint", 1000); + elbow_joint_publisher = nh.advertise("/arm_node/elbow_joint_setpoint", 1000); + wrist_pivot_publisher = nh.advertise("/arm_node/wrist_pivot_setpoint", 1000); + wrist_joint_publisher = nh.advertise("/arm_node/wrist_joint_setpoint", 1000); + end_effector_pivot_publisher = nh.advertise("/arm_node/end_effector_pivot_setpoint", 1000); + + shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback); + shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback); + elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback); + wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback); + wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback); + end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback); + + int state = 0; + KDL::JntArray required_angles(arm.getArm().getNrOfJoints()); + KDL::SetToZero(required_angles); + + vector desired_positions; + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.5, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.4, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.3, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.2, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.1, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.0, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.1, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.2, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.3, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.4, 1))); + // desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.5, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 1, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 2))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 1, 3))); + + while (ros::ok()) + { + // ros::spinOnce(); + // ros::spinOnce(); + // quick.sleep(); + + // pos(0) = shoulder_pivot_angle.data; + // pos(1) = shoulder_joint_angle.data; + // pos(2) = elbow_joint_angle.data; + // pos(3) = wrist_pivot_angle.data; + // pos(4) = wrist_joint_angle.data; + // pos(5) = end_effector_pivot_angle.data; + + // if (state) //fabs(shoulder_joint_target.data) < 0.001) + // { + arm.calculateInverseKinematics(pos, desired_positions[state] /*KDL::Frame(KDL::Vector(1, 0, 0))*/, required_angles); + //shoulder_joint_target.data = 0.7; + // } + // else + // { + // arm.calculateInverseKinematics(pos, KDL::Frame(KDL::Vector(-1, -1, 1)), required_angles); + // //shoulder_joint_target.data = 0.0; + // } + + cout << desired_positions[state].p.x() << " " << desired_positions[state].p.y() << " " << desired_positions[state].p.z() << endl; + cout << "\t" << required_angles(0) << "\t" << required_angles(1) << "\t" << required_angles(2) << "\t" << required_angles(3) << "\t" << required_angles(4) << "\t" << required_angles(5) << endl; + cout << endl; + + for (int idx = 0; idx < arm.getArm().getNrOfJoints(); ++idx) + { + // cout << "Cur: " << pos(idx) << "\tRequired: " << required_angles(idx) << endl; + fix_angle(pos(idx), required_angles(idx)); + // cout << "\tPost fix: Required: " << required_angles(idx) << endl; + } + + shoulder_pivot_target.data = required_angles(0); + shoulder_joint_target.data = required_angles(1); + elbow_joint_target.data = required_angles(2); + wrist_pivot_target.data = required_angles(3); + wrist_joint_target.data = required_angles(4); + end_effector_pivot_target.data = required_angles(5); + + // cout << "Received angles: " << (int)received_angles << endl; + // if (received_angles == 0b00111111) + // { + shoulder_pivot_publisher.publish(shoulder_pivot_target); + shoulder_joint_publisher.publish(shoulder_joint_target); + elbow_joint_publisher.publish(elbow_joint_target); + wrist_pivot_publisher.publish(wrist_pivot_target); + wrist_joint_publisher.publish(wrist_joint_target); + end_effector_pivot_publisher.publish(end_effector_pivot_target); + + state = (state + 1) % desired_positions.size(); + // } + // else + // { + // ros::spinOnce(); + // } + + pub_rate.sleep(); + + ros::spinOnce(); + } +} \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/test/test.cpp b/rrrobot_src/src/arm_control/test/test.cpp index 142235b..f8c2d2d 100644 --- a/rrrobot_src/src/arm_control/test/test.cpp +++ b/rrrobot_src/src/arm_control/test/test.cpp @@ -13,12 +13,16 @@ #include #include #include +#include +#include using namespace KDL; using std::cin; using std::cout; using std::endl; using std::ofstream; +using std::string; +using std::vector; int main(int argc, char **argv) { @@ -155,4 +159,48 @@ int main(int argc, char **argv) { cout << "Failed to find required torques, error #" << error_val << endl; } + + // record required torques for a variety of joint positions + KDL::JntArray pos(arm.getArm().getNrOfJoints()); + for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint) + { + pos(joint) = 0; + } + + ofstream f("required_" + std::to_string(which_joint) + "_torque.txt"); + for (double angle = -3.14159; angle < 3.14159; angle += 0.1) + { + pos(which_joint) = angle; + arm.calculateInverseDynamics(pos, vel, accel, ext_force, required_force); + + f << angle << "," << required_force(0) << "," << required_force(1) << "," << required_force(2) << "," << required_force(3) << "," << required_force(4) << "," << required_force(5) << "\n"; + } + + // KDL::JntArray pos(arm.getArm().getNrOfJoints()); + KDL::JntArray required_angles(arm.getArm().getNrOfJoints()); + + KDL::SetToZero(pos); + KDL::SetToZero(required_angles); + + vector desired_positions; + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.5, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.4, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.3, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.2, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.1, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.0, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.1, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.2, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.3, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.4, 1))); + desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.5, 1))); + + ofstream ik_f("inverse_kinematics.txt"); + for (int state = 0; state < desired_positions.size(); ++state) + { + arm.calculateInverseKinematics(pos, desired_positions[state], required_angles); + + ik_f << desired_positions[state].p.x() << "," << desired_positions[state].p.y() << "," << desired_positions[state].p.z() << ","; + ik_f << required_angles(0) << "," << required_angles(1) << "," << required_angles(2) << "," << required_angles(3) << "," << required_angles(4) << "," << required_angles(5) << "\n"; + } } \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/test/test_pub_sub.cpp b/rrrobot_src/src/arm_control/test/test_pub_sub.cpp new file mode 100644 index 0000000..3d93180 --- /dev/null +++ b/rrrobot_src/src/arm_control/test/test_pub_sub.cpp @@ -0,0 +1,97 @@ +#include "ros/ros.h" +#include "std_msgs/Float64.h" + +#include + +using std::cout; +using std::endl; + +void msg_callback(const std_msgs::Float64 &cmd) +{ + // update the torque applied to each joint when a message is received + cout << "received: " << cmd.data << endl; +} + +/** + * This tutorial demonstrates simple sending of messages over the ROS system. + */ +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "talker"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + ros::Publisher pub = n.advertise("chatter", 1000); + ros::Subscriber sub = n.subscribe("chatter", 1000, &msg_callback); + + ros::Rate loop_rate(10); + + /** + * A count of how many messages we have sent. This is used to create + * a unique string for each message. + */ + int count = 0; + double data = -5e-5; //.1254987465413251e-5; + while (ros::ok()) + { + /** + * This is a message object. You stuff it with data, and then publish it. + */ + std_msgs::Float64 msg; + msg.data = data; + + cout << "Publishing: " << data << '\t' << msg.data << endl; + + /** + * The publish() function is how you send messages. The parameter + * is the message object. The type of this object must agree with the type + * given as a template parameter to the advertise<>() call, as was done + * in the constructor above. + */ + pub.publish(msg); + + ros::spinOnce(); + + loop_rate.sleep(); + ++count; + + data += 1e-5; + if (data > 5.0) + { + data = -5.1254987465413251; + } + } + + return 0; +} \ No newline at end of file diff --git a/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf b/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf index 3bbaf31..7eb02fd 100644 --- a/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf +++ b/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf @@ -17,7 +17,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -116,7 +116,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -215,7 +215,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -314,7 +314,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -413,7 +413,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -512,7 +512,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -611,7 +611,7 @@ 1 0 - 1 + 0 0 0 0 0 -0 0 @@ -704,7 +704,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -737,7 +737,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -770,7 +770,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -803,7 +803,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -836,7 +836,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 @@ -869,7 +869,7 @@ -1.79769e+308 1.79769e+308 - -2000 + -20000000 -1 diff --git a/rrrobot_src/src/simulation_env/msg/arm_angles.msg b/rrrobot_src/src/simulation_env/msg/arm_angles.msg index a6e84c6..98d9a46 100644 --- a/rrrobot_src/src/simulation_env/msg/arm_angles.msg +++ b/rrrobot_src/src/simulation_env/msg/arm_angles.msg @@ -4,3 +4,10 @@ float32 elbow_joint_angle float32 wrist_pivot_angle float32 wrist_joint_angle float32 end_effector_pivot_angle + +float32 shoulder_pivot_velocity +float32 shoulder_joint_velocity +float32 elbow_joint_velocity +float32 wrist_pivot_velocity +float32 wrist_joint_velocity +float32 end_effector_pivot_velocity diff --git a/rrrobot_src/src/simulation_env/src/arm_angle_sensor_plugin.cpp b/rrrobot_src/src/simulation_env/src/arm_angle_sensor_plugin.cpp index 806743b..eb4ab1b 100644 --- a/rrrobot_src/src/simulation_env/src/arm_angle_sensor_plugin.cpp +++ b/rrrobot_src/src/simulation_env/src/arm_angle_sensor_plugin.cpp @@ -2,9 +2,11 @@ #include #include #include +#include #include "ros/ros.h" #include "simulation_env/arm_angles.h" +#include using std::cout; using std::endl; @@ -29,7 +31,33 @@ public: } nh.reset(new ros::NodeHandle("arm_node")); - publisher = nh->advertise("arm_positions", 1); + // publisher = nh->advertise("arm_positions", 1000); + + shoulder_pivot_pub = nh->advertise("/arm_node/arm_positions/shoulder_pivot_angle", 1); + shoulder_joint_pub = nh->advertise("/arm_node/arm_positions/shoulder_joint_angle", 1); + elbow_joint_pub = nh->advertise("/arm_node/arm_positions/elbow_joint_angle", 1); + wrist_pivot_pub = nh->advertise("/arm_node/arm_positions/wrist_pivot_angle", 1); + wrist_joint_pub = nh->advertise("/arm_node/arm_positions/wrist_joint_angle", 1); + end_effector_pivot_pub = nh->advertise("/arm_node/arm_positions/end_effector_pivot_angle", 1); + + std_msgs::Float64 shoulder_pivot_angle; + shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle; + std_msgs::Float64 shoulder_joint_angle; + shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle; + std_msgs::Float64 elbow_joint_angle; + elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle; + std_msgs::Float64 wrist_pivot_angle; + wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle; + std_msgs::Float64 wrist_joint_angle; + wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle; + std_msgs::Float64 end_effector_pivot_angle; + end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle; + // cout << "shoulder_pivot: " << shoulder_pivot_angle.data << endl; + // cout << "shoulder_joint: " << shoulder_joint_angle.data << endl; + // cout << "elbow_joint: " << elbow_joint_angle.data << endl; + // cout << "wrist_pivot: " << wrist_pivot_angle.data << endl; + // cout << "wrist_joint: " << wrist_joint_angle.data << endl; + // cout << "end_effector_pivot: " << end_effector_pivot_angle.data << endl; } private: @@ -37,16 +65,53 @@ private: { simulation_env::arm_angles msg; - // read in the joint angle for each joint in the arm - msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position(); - msg.shoulder_joint_angle = model->GetJoint("shoulder_joint")->Position(); - msg.elbow_joint_angle = model->GetJoint("elbow_joint")->Position(); - msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position(); - msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position(); - msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position(); + // // read in the joint angle for each joint in the arm + // msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position(); + // msg.shoulder_joint_angle = -model->GetJoint("shoulder_joint")->Position(); + // msg.elbow_joint_angle = -model->GetJoint("elbow_joint")->Position(); + // msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position(); + // msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position() + M_PI_2; + // msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position(); - // publish the updated sensor measurements - publisher.publish(msg); + // // read in the angular velocity for each joint + // msg.shoulder_pivot_velocity = model->GetJoint("shoulder_pivot")->GetVelocity(0); + // msg.shoulder_joint_velocity = -model->GetJoint("shoulder_joint")->GetVelocity(0); + // msg.elbow_joint_velocity = -model->GetJoint("elbow_joint")->GetVelocity(0); + // msg.wrist_pivot_velocity = model->GetJoint("wrist_pivot")->GetVelocity(0); + // msg.wrist_joint_velocity = model->GetJoint("wrist_joint")->GetVelocity(0); + // msg.end_effector_pivot_velocity = model->GetJoint("end_effector_pivot")->GetVelocity(0); + + // // publish the updated sensor measurements + // publisher.publish(msg); + // ros::spinOnce(); + + std_msgs::Float64 shoulder_pivot_angle; + shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle; + std_msgs::Float64 shoulder_joint_angle; + shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle; + std_msgs::Float64 elbow_joint_angle; + elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle; + std_msgs::Float64 wrist_pivot_angle; + wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle; + std_msgs::Float64 wrist_joint_angle; + wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle; + std_msgs::Float64 end_effector_pivot_angle; + end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle; + + // cout << "Publishing: " << shoulder_pivot_angle.data << '\t' << shoulder_joint_angle.data << '\t' << elbow_joint_angle.data << '\t' << wrist_pivot_angle.data << '\t' << wrist_joint_angle.data << '\t' << end_effector_pivot_angle.data << endl; + + shoulder_pivot_pub.publish(shoulder_pivot_angle); + // ros::spinOnce(); + shoulder_joint_pub.publish(shoulder_joint_angle); + // ros::spinOnce(); + elbow_joint_pub.publish(elbow_joint_angle); + // ros::spinOnce(); + wrist_pivot_pub.publish(wrist_pivot_angle); + // ros::spinOnce(); + wrist_joint_pub.publish(wrist_joint_angle); + // ros::spinOnce(); + end_effector_pivot_pub.publish(end_effector_pivot_angle); + // ros::spinOnce(); // make sure the message gets published ros::spinOnce(); @@ -56,6 +121,13 @@ private: event::ConnectionPtr updateConnection; std::unique_ptr nh; ros::Publisher publisher; + + ros::Publisher shoulder_pivot_pub; + ros::Publisher shoulder_joint_pub; + ros::Publisher elbow_joint_pub; + ros::Publisher wrist_pivot_pub; + ros::Publisher wrist_joint_pub; + ros::Publisher end_effector_pivot_pub; }; GZ_REGISTER_MODEL_PLUGIN(JointAngle) } // namespace gazebo diff --git a/rrrobot_src/src/simulation_env/src/arm_motors.cpp b/rrrobot_src/src/simulation_env/src/arm_motors.cpp index 099f036..f4fa526 100644 --- a/rrrobot_src/src/simulation_env/src/arm_motors.cpp +++ b/rrrobot_src/src/simulation_env/src/arm_motors.cpp @@ -5,6 +5,7 @@ #include "ros/ros.h" #include "simulation_env/arm_command.h" +#include using std::cout; using std::endl; @@ -14,15 +15,63 @@ namespace gazebo class ArmControl : public ModelPlugin { public: + void shoulder_pivot_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("shoulder_pivot")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void shoulder_joint_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("shoulder_joint")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void elbow_joint_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("elbow_joint")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void wrist_pivot_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("wrist_pivot")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void wrist_joint_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("wrist_joint")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + + void end_effector_pivot_callback(const std_msgs::Float64 &cmd) + { + // update the torque applied to each joint when a message is received + model->GetJoint("end_effector_pivot")->SetForce(0, cmd.data); + + ros::spinOnce(); + } + void arm_command_callback(const simulation_env::arm_command &cmd) { // update the torque applied to each joint when a message is received - model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force); - model->GetJoint("shoulder_joint")->SetForce(0, cmd.shoulder_joint_force); - model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force); - model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force); - model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force); - model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force); + // model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force); + // model->GetJoint("shoulder_joint")->SetForce(0, -cmd.shoulder_joint_force); + // model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force); + // model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force); + // model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force); + // model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force); ros::spinOnce(); } @@ -42,12 +91,27 @@ public: nh.reset(new ros::NodeHandle("arm_node")); subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this); + shoulder_pivot_sub = nh->subscribe("/arm_node/arm_commands/shoulder_pivot_torque", 1000, &ArmControl::shoulder_pivot_callback, this); + shoulder_joint_sub = nh->subscribe("/arm_node/arm_commands/shoulder_joint_torque", 1000, &ArmControl::shoulder_joint_callback, this); + elbow_joint_sub = nh->subscribe("/arm_node/arm_commands/elbow_joint_torque", 1000, &ArmControl::elbow_joint_callback, this); + wrist_pivot_sub = nh->subscribe("/arm_node/arm_commands/wrist_pivot_torque", 1000, &ArmControl::wrist_pivot_callback, this); + wrist_joint_sub = nh->subscribe("/arm_node/arm_commands/wrist_joint_torque", 1000, &ArmControl::wrist_joint_callback, this); + end_effector_sub = nh->subscribe("/arm_node/arm_commands/end_effector_pivot_torque", 1000, &ArmControl::end_effector_pivot_callback, this); + + cout << "Subscribed to all torque channels" << endl; } private: physics::ModelPtr model; std::unique_ptr nh; ros::Subscriber subscriber; + + ros::Subscriber shoulder_pivot_sub; + ros::Subscriber shoulder_joint_sub; + ros::Subscriber elbow_joint_sub; + ros::Subscriber wrist_pivot_sub; + ros::Subscriber wrist_joint_sub; + ros::Subscriber end_effector_sub; }; GZ_REGISTER_MODEL_PLUGIN(ArmControl) } // namespace gazebo diff --git a/rrrobot_src/world/rrrobot_setpoint.world b/rrrobot_src/world/rrrobot_setpoint.world new file mode 100644 index 0000000..ba4b4e6 --- /dev/null +++ b/rrrobot_src/world/rrrobot_setpoint.world @@ -0,0 +1,1514 @@ + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0 3.14159 -0 0 + + 86.082 + + 4.3404 + 0 + 0.0020207 + 2.4866 + 0 + 6.1574 + + -0.003895 -0.062844 -0.11068 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + 0.001715 0.003779 0.661683 1.5708 -0 0 + + 266.27 + + 279.51 + 0 + 4.6477 + 43.386 + 34.265 + 255.26 + + 0.093634 -0.51398 -0.1614 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + -0.00025 -0.94659 5.45119 1.5708 -0 0 + + 63.612 + + 6.6124 + 0.099584 + 0 + 5.9295 + 0 + 1.9517 + + 0.00756 -3.4862 -0.40714 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + -0.068455 0.275759 0.315754 -1.571 -0 0 + + 60.795 + + 8.1939 + 0 + 0 + 0.76659 + 0 + 8.0226 + + 0.25344 -1.009 -0.57114 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + -0.000257 -3.97477 2.07064 -1.5708 7e-06 3.14159 + + 6.6264 + + 0.089718 + 0 + 0.017666 + 0.092881 + 3e-08 + 0.028564 + + -0.028814 0 -2.454 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + 0.000743 -4.767 2.06498 -1.5708 7e-06 3.14159 + + 5.1229 + + 0.038195 + 4e-07 + 0 + 0.066675 + 6e-07 + 0.04821 + + -0.010371 1e-06 -3.0669 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + -2.06426 -5.37015 4.13075 1.88426 1.57078 0.313465 + + 0.72893 + + 0.00131 + 0 + 0 + 0.0012978 + 0 + 0.0024341 + + 2.0657 2.0642 -3.5441 0 -0 0 + + 1 + 0 + 0 + + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL + 1 1 1 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL + 1 1 1 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + 0 + + + link_3 + link_4 + 0 0 -2.7 0 -0 0 + + 0 0 1 + 0 + + -1.79769e+308 + 1.79769e+308 + -2e+07 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_1 + link_2 + 0 -0.41 -0.61 0 -0 0 + + 1 0 0 + 0 + + -1.79769e+308 + 1.79769e+308 + -2e+07 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_2 + link_3 + 0 -3.65 -0.6 0 -0 0 + + 1 0 0 + 0 + + -1.79769e+308 + 1.79769e+308 + -2e+07 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_4 + link_5 + 0 0 -3.15 0 -0 0 + + 1 0 0 + 0 + + -1.79769e+308 + 1.79769e+308 + -2e+07 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + base + link_1 + 0 0 0 0 -0 0 + + 0 1 0 + 0 + + -1.79769e+308 + 1.79769e+308 + -2e+07 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_5 + link_6 + 2.065 2.065 -3.5 0 -0 0 + + 0 0 1 + 0 + + -1.79769e+308 + 1.79769e+308 + -2e+07 + -1 + + + 0 + 0 + 0 + 0 + + + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + world + base + + 0 + 0 + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 84 128000000 + 227 16092534 + 1586714255 897951104 + 84128 + + 0 1e-06 0 2e-06 -0 -3e-06 + 1 1 1 + + 0 1e-06 0 3.14159 0 -3e-06 + -2e-06 4e-06 -1.9e-05 5e-06 5.3e-05 3e-06 + -0.014451 -0.003203 -0.002579 0.065768 -0.16485 0.013118 + -1.24396 -0.275747 -0.221966 0 -0 0 + + + 0.001716 0.003778 0.661683 1.57079 -4e-06 2.3478 + 0.000225 7.9e-05 -9e-06 -0.000224 0.000249 -0.010059 + -0.013679 -0.151361 0.064888 0.206531 0.253364 2.70926 + -3.64238 -40.3028 17.2776 0 -0 0 + + + -0.200644 -0.293623 0.474736 -1.32775 -4e-06 2.3478 + -0.002409 0.002512 -0.001684 0.001575 -0.001632 -0.010095 + -0.653366 -0.603443 0.055824 0.107976 -0.798416 -0.477122 + -39.7214 -36.6863 3.39379 0 -0 0 + + + 2.76295 2.71626 3.51127 2.48747 -4e-06 2.3478 + 0.035596 -0.019179 -0.025963 -0.005696 0.005776 -0.009954 + -1.52627 -1.27903 -1.61816 -1.42175 0.410946 1.66071 + -97.0888 -81.3618 -102.934 0 -0 0 + + + 2.16384 2.12713 -0.948795 -2.5018 -0.146798 -0.599729 + 0.007014 -0.033224 -0.014559 0.130634 0.135525 -0.254293 + -5.4754 -4.48892 -6.4751 -2.49967 1.34692 -2.78923 + -36.2822 -29.7454 -42.9066 0 -0 0 + + + 1.00544 1.7326 4.00541 -0.137797 -0.146802 -0.599723 + 0.601991 -0.359545 0.086432 0.12202 0.142802 -0.257395 + -4.38455 -5.27607 -7.10143 2.86584 -1.22955 -2.98271 + -22.4616 -27.0288 -36.3799 0 -0 0 + + + 0.690541 4.69735 4.01685 0.157576 -0.125335 -2.26271 + 1.32005 -0.295835 0.504983 0.125351 0.142495 -0.244604 + -1.71691 -5.67113 -7.06861 -1.70898 -0.167015 1.74113 + -1.25151 -4.13385 -5.15252 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1 1 1 0 -0 0 + 1 1 1 + + 1 1 1 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1 1 3 0 -0 0 + 1 1 1 + + 1 1 3 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1 -1 1 0 -0 0 + 1 1 1 + + 1 -1 1 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1 -1 2 0 -0 0 + 1 1 1 + + 1 -1 2 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + + 0.208095 + + 0.0180922 + 0 + 0 + 0.0180922 + 0 + 0.0346825 + + 0 0 0 0 -0 0 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + + + 0.05 0.05 0.05 + + + + + + __default__ + + 1 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + 3.17326 0.573933 0 0 -0 0 + + + + + 0.208095 + + 0.0180922 + 0 + 0 + 0.0180922 + 0 + 0.0346825 + + 0 0 0 0 -0 0 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + + + 0.05 0.05 0.05 + + + + + + __default__ + + 1 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + 3.77153 -0.833785 0 0 -0 0 + + + + + 0.208095 + + 0.0180922 + 0 + 0 + 0.0180922 + 0 + 0.0346825 + + 0 0 0 0 -0 0 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + + + 0.05 0.05 0.05 + + + + + + __default__ + + 1 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + 3.68535 0.374919 0 0 -0 0 + + + + + 0.208095 + + 0.0180922 + 0 + 0 + 0.0180922 + 0 + 0.0346825 + + 0 0 0 0 -0 0 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + + + 0.05 0.05 0.05 + + + + + + __default__ + + 1 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 1 + 1 + 3.7805 -0.448301 0 0 -0 0 + + + + 6.87718 0.050797 2.34784 0 0.147643 2.9762 + orbit + perspective + + + +