Servo power is OFF. Please call start_traj_mode or start_point_queue_mode service #424
Replies: 4 comments 17 replies
-
|
Hi @LucaMIMOUNI This sounds very similar to #192. What version of MotoROS2 are you running? Can you please run the Debug Logger and reproduce the issue? This should show us what is preventing the servos from enabling. Also, please save a copy of PANELBOX.LOG from your teach pendant. This will show the configuration/setup of your robot controller. |
Beta Was this translation helpful? Give feedback.
-
|
Hey Ted, Thank you for your fast reply ! if you need more information don't hesitate. i'll have a look at #192 . Ty Here is the full log file: Best. |
Beta Was this translation helpful? Give feedback.
-
|
Yes, please try 0.2.0. The installation instructions are available here. Another thing you can try is to put the pendant in PLAY mode (middle key position) and manually press the |
Beta Was this translation helpful? Give feedback.
-
|
Hey, For the PLAY mode, we succeed to activate the servos (when pressing the button on the teach); however when we get back to 'remote control', servos are off again. For the update installation, it might be not possible for 'us' to do it; there already lots of parameters set for the robot; I fear that an update could delete every parameters set up the few past years. Can you confirm that the installation of the 0.2.0 will not erase parameters set up on the MA2010 ? Best. |
Beta Was this translation helpful? Give feedback.
-
|
Upgrading from In all cases however, when upgrading: make a full |
Beta Was this translation helpful? Give feedback.
-
|
Hey everyone, Thank you for your help. We've just updated the Motoros to 0.2.0; the error message is gone and I can activate the servo motors ! Also just saw you recommended this repo: git. Should I inspire myself from this ? My current ros node to create a follow_joint_trajectory action; through a pose goal took from rviz moveit marker: #include <chrono>
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "industrial_msgs/msg/robot_status.hpp"
#include "moveit_msgs/msg/motion_plan_request.hpp"
using namespace std::chrono_literals;
using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
using GoalHandleFJT = rclcpp_action::ClientGoalHandle<FollowJointTrajectory>;
class MotorosController : public rclcpp::Node
{
public:
MotorosController() : Node("motoros_controller")
{
// Use a QoS profile with Best Effort reliability:
auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort();
robot_status_sub_ = this->create_subscription<industrial_msgs::msg::RobotStatus>(
"robot_status", qos,
std::bind(&MotorosController::robotStatusCallback, this, std::placeholders::_1));
joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"/ctrl_groups/b1/joint_states", qos,
std::bind(&MotorosController::JointStatesCallback, this, std::placeholders::_1));
motion_plan_req_sub_ = this->create_subscription<moveit_msgs::msg::MotionPlanRequest>(
"motion_plan_request", 10,
std::bind(&MotorosController::motionPlanCallback, this, std::placeholders::_1));
action_client_ = rclcpp_action::create_client<FollowJointTrajectory>(this, "follow_joint_trajectory");
timer_ = this->create_wall_timer(1s, [this]() { send_goal(); });
}
private:
rclcpp_action::Client<FollowJointTrajectory>::SharedPtr action_client_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Subscription<industrial_msgs::msg::RobotStatus>::SharedPtr robot_status_sub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
rclcpp::Subscription<moveit_msgs::msg::MotionPlanRequest>::SharedPtr motion_plan_req_sub_;
// Keep the latest received messages.
industrial_msgs::msg::RobotStatus::SharedPtr latest_robot_status_;
sensor_msgs::msg::JointState::SharedPtr latest_joint_state_;
// The extracted target point from MoveIt request.
trajectory_msgs::msg::JointTrajectoryPoint target_point_;
void robotStatusCallback(const industrial_msgs::msg::RobotStatus::SharedPtr msg)
{
latest_robot_status_ = msg;
// RCLCPP_INFO(this->get_logger(), "Received robot_status: in_motion = %s",
// msg->in_motion.val ? "true" : "false");
}
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
latest_joint_state_ = msg;
// RCLCPP_INFO(this->get_logger(), "Received joint_states, %zu joints", msg->name.size());
}
void motionPlanCallback(const moveit_msgs::msg::MotionPlanRequest::SharedPtr msg)
{
RCLCPP_INFO(this->get_logger(), "Received MotionPlanRequest with %zu goal constraints.",
msg->goal_constraints.size());
// Check that there is at least one goal constraint.
if (msg->goal_constraints.empty())
{
RCLCPP_WARN(this->get_logger(), "Received MotionPlanRequest with no goal constraints.");
return;
}
auto& gc = msg->goal_constraints[0];
if (gc.joint_constraints.empty())
{
RCLCPP_WARN(this->get_logger(), "Goal constraints contain no joint constraints.");
return;
}
// Resize target_point positions to match the number of joint constraints.
target_point_.positions.resize(gc.joint_constraints.size());
// (Optionally, clear velocities.)
target_point_.velocities.resize(gc.joint_constraints.size(), 0.0);
// Loop over each joint_constraint and extract the target position.
for (size_t i = 0; i < gc.joint_constraints.size(); ++i)
{
target_point_.positions[i] = gc.joint_constraints[i].position;
RCLCPP_INFO(this->get_logger(), "Joint %s target: %.4f",
gc.joint_constraints[i].joint_name.c_str(), gc.joint_constraints[i].position);
}
// Set an example time duration for the trajectory.
// target_point_.time_from_start = rclcpp::Duration(5s).to_msg();;
// #include <chrono>
// using namespace std::chrono;
// auto dur = duration_cast<nanoseconds>(5s);
// target_point_.time_from_start.sec = static_cast<uint32_t>(duration_cast<seconds>(dur).count());
// target_point_.time_from_start.nanosec = static_cast<uint32_t>(dur.count() % 1000000000);
RCLCPP_INFO(this->get_logger(), "Extracted target trajectory point from MotionPlanRequest.");
}
// Function for checking errors and resetting if needed.
void checkAndResetErrors()
{
// If no status available, nothing to do.
if (!latest_robot_status_) return;
// Check error condition; adjust accessor based on your message definition.
// For example, we assume in_error is of a type with a 'val' or a boolean flag.
if (latest_robot_status_->in_error.val) // if error is active.
{
RCLCPP_WARN(this->get_logger(), "Robot in error state! Error code: %d", latest_robot_status_->error_codes);
// Create the reset_error service client (assumed to use Trigger interface).
auto reset_client = this->create_client<std_srvs::srv::Trigger>("reset_error");
if (!reset_client->wait_for_service(5s)) {
RCLCPP_ERROR(this->get_logger(), "Reset_error service not available");
return;
}
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
// Optionally, set any request fields if needed.
// Send the service request and wait for the result (synchronously for simplicity).
auto future_result = reset_client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future_result, 5s)
!= rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(this->get_logger(), "Failed to call reset_error service");
return;
}
auto result = future_result.get();
if (result->success)
{
RCLCPP_INFO(this->get_logger(), "Error reset successfully");
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to reset error: %s", result->message.c_str());
}
// Optionally wait a moment before re-checking.
rclcpp::sleep_for(500ms);
checkAndResetErrors(); // Recurse until no error.
}
else
{
RCLCPP_INFO(this->get_logger(), "No active errors in robot status.");
}
}
void send_goal()
{
//////////////////////////////////////////////////////
//////////////////////////////////////////////////////
// First, check for errors and attempt reset.
checkAndResetErrors();
// Check if a robot_status message has been received.
if (!latest_robot_status_) {
RCLCPP_WARN(this->get_logger(), "No robot_status message received yet. Waiting...");
return;
}
// Verify the robot is idle.
if (latest_robot_status_->in_motion.val) {
RCLCPP_WARN(this->get_logger(), "Robot is in motion. Waiting for an idle state...");
return;
}
// Check for current joint state.
if (!latest_joint_state_) {
RCLCPP_WARN(this->get_logger(), "No joint_states received yet. Waiting...");
return;
}
// Ensure we have received a target point from MotionPlanRequest.
if (target_point_.positions.empty())
{
RCLCPP_WARN(this->get_logger(), "No target point set from MotionPlanRequest. Waiting...");
return;
}
if (!action_client_->wait_for_action_server(5s)) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
return;
}
//////////////////////////////////////////////////////
//////////////////////////////////////////////////////
auto goal_msg = FollowJointTrajectory::Goal();
// Set joint names as required (ensure ordering matches your robot's configuration).
goal_msg.trajectory.joint_names = {"group_3/joint_1"}; // {"joint_1_s", "joint_2_l", "joint_3_u", "joint_4_r", "joint_5_b", "joint_6_t"};
trajectory_msgs::msg::JointTrajectoryPoint start_point;
// Use latest joint state as the start point.
start_point.positions = latest_joint_state_->position;
start_point.time_from_start.sec = 0;
start_point.time_from_start.nanosec = 0;
// Build the trajectory with the start and target points.
goal_msg.trajectory.points.push_back(start_point);
goal_msg.trajectory.points.push_back(target_point_);
auto send_goal_options = rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.goal_response_callback =
[this](std::shared_ptr<GoalHandleFJT> goal_handle) {
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by the action server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted, waiting for result");
}
};
send_goal_options.feedback_callback =
[this](GoalHandleFJT::SharedPtr,
const std::shared_ptr<const FollowJointTrajectory::Feedback> /*feedback*/) {
RCLCPP_INFO(this->get_logger(), "Received feedback from action server");
};
send_goal_options.result_callback =
[this](const GoalHandleFJT::WrappedResult & result) {
RCLCPP_INFO(this->get_logger(), "Result code: %d", result.code);
if (result.result) {
RCLCPP_INFO(this->get_logger(), "Result error code: %d", result.result->error_code);
RCLCPP_INFO(this->get_logger(), "Result error string: %s", result.result->error_string.c_str());
// If there are additional fields, log them as well.
} else {
RCLCPP_WARN(this->get_logger(), "Result message is null");
}
if(result.code != rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_ERROR(this->get_logger(), "Trajectory execution failed");
} else {
RCLCPP_INFO(this->get_logger(), "Trajectory execution succeeded");
}
};
action_client_->async_send_goal(goal_msg, send_goal_options);
timer_->cancel(); // Send goal only once (if desired)
}
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<MotorosController>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
} |
Beta Was this translation helpful? Give feedback.
-
When submitting a trajectory, MotoROS2 expects the goal to contain data for all joints in the system. So, even though you just want to move your base track, you still need to explicitly tell the controller what to do with the other joints. So, you can just read the
That repo is designed to interface with a custom version of the MotoROS1 driver. It is not compatible with this software. |
Beta Was this translation helpful? Give feedback.
-
|
Thx again for theses precisions; appreciate it ! I spend several hours modifying the code to make the ma2010 move. I run through severals errors:
More detail with the logs from the motoman_controller ros2 node I wrote: [INFO] [1750257788.307652237] [motoros_controller]: Processing MotionPlanRequest:
[INFO] [1750257788.307704449] [motoros_controller]: Joint: group_3/joint_1, Target Position: -2.574480
[INFO] [1750257788.307709930] [motoros_controller]: Found joint group_3/joint_1 at index 0
[INFO] [1750257788.307775315] [motoros_controller]: Sending goal message:
[INFO] [1750257788.307784705] [motoros_controller]: Joint Names: [group_3/joint_1, group_1/joint_1, group_1/joint_2, group_1/joint_3, group_1/joint_4, group_1/joint_5, group_1/joint_6, group_4/joint_1, group_2/joint_1, group_2/joint_2, group_2/joint_3, group_2/joint_4, group_2/joint_5, group_2/joint_6, group_5/joint_1, group_5/joint_2]
[INFO] [1750257788.307790686] [motoros_controller]: Point 0:
[INFO] [1750257788.307802980] [motoros_controller]: Positions: [-3.07172, 0.0550774, -1.1624, 0.00783426, 0.0768526, -1.88125, -0.155553, -5.20879e-05, 0, 0, 0, 0, 0, 0, -1.57077, 0]
[INFO] [1750257788.307809304] [motoros_controller]: Velocities: [0, 0, 0.00130831, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[INFO] [1750257788.307812762] [motoros_controller]: Time From Start: 0.0 sec
[INFO] [1750257788.307816174] [motoros_controller]: Point 1:
[INFO] [1750257788.307822090] [motoros_controller]: Positions: [-2.57448, 0.0550774, -1.1624, 0.00783426, 0.0768526, -1.88125, -0.155553, -5.20879e-05, 0, 0, 0, 0, 0, 0, -1.57077, 0]
[INFO] [1750257788.307827826] [motoros_controller]: Velocities: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
[INFO] [1750257788.307831222] [motoros_controller]: Time From Start: 5.0 sec
[INFO] [1750257788.307835640] [motoros_controller]: No active errors in robot status.
[INFO] [1750257788.326659892] [motoros_controller]: Goal accepted, waiting for result.
[INFO] [1750257788.356323411] [motoros_controller]: Result code: 6
[INFO] [1750257788.356365626] [motoros_controller]: Result error code: -100204
[INFO] [1750257788.356368893] [motoros_controller]: Result error string: The first point must match the robot's current position.
[ERROR] [1750257788.356371609] [motoros_controller]: Trajectory execution failed
I'm kind of stuck here as the states are litterally the one echo from the motoman publisher. Here is also a snipset from my follow_joint_trajectory:
action_ns: ""
type: FollowJointTrajectory
default: true
joints:
- group_3/joint_1 # b1
- group_1/joint_1 # joint_1_s (r1)
- group_1/joint_2 # joint_2_l
- group_1/joint_3 # joint_3_u
- group_1/joint_4 # joint_4_r
- group_1/joint_5 # joint_5_b
- group_1/joint_6 # joint_6_t
- group_4/joint_1 # b2
- group_2/joint_1 # joint_1_s (r2)
- group_2/joint_2 # joint_2_l
- group_2/joint_3 # joint_3_u
- group_2/joint_4 # joint_4_r
- group_2/joint_5 # joint_5_b
- group_2/joint_6 # joint_6_t
- group_5/joint_1 # roll
- group_5/joint_2 # yawSome further investigationThere is an OFFSET of about 4.788934 m between linear rail joint state Have you some ideas here ? Is there a way for me to know more precisely which of the joint(s) are incorrect ? header:
stamp:
sec: 1750258646
nanosec: 312444679
frame_id: ''
name:
- group_3/joint_1
position:
- 1.7172140451660156
velocity:
- 0.0
effort:
- 0.3243
1.7172140451660156 - 4.788934 = -3.07172 which is indeed the value on the teach. Best ! Luca |
Beta Was this translation helpful? Give feedback.
-
|
Yes, you should be able to figure out which of the joint(s) are "too far" from their initial commanded position if you use the debug logger. That information is one of the things that it logs. Please take a look at the debug log, and post it here if you're still having trouble. |
Beta Was this translation helpful? Give feedback.
-
|
Thx you again, I'm getting closer I hope. With a closer look on the logs; I could figure out that the data from Robot 2 was not in degrees but in impulse surprisingly.
As I am not using the robot 2 (neither the rail b2, or the positioner 2 axes), I try to hard_coded theses values in the starting position: However now I got some crazy values here '2147483648' By inspecting the code MotionControl.c; I can see that you expect an IMPULSE value of type Finally, as I am only working with linear rail b1 and robot r1; should I remove r2, b2, and posi from the motoros2_config.yaml ? Best. Luca |
Beta Was this translation helpful? Give feedback.
-
|
Oh but actually this is not 0, 0, 0, ... Thats maybe why there is a difference is the pulse ! |
Beta Was this translation helpful? Give feedback.
-
|
Yes, I am pretty sure that is the issue.
Let us know if passing in the R2 joint angles reported by |
Beta Was this translation helpful? Give feedback.
-
|
Okay this was indeed the culprit; thats my bad, there was an error in my code so that I was not subscribing to the right joint states ! With the right As we are not under the same time zone, this is already the end of the day for me in Europe ahah, and I can't investigate furthermore the new error unfortunatly. We can continue this conversation tomorrow. Again thank you for your time and your help, very appreciate that !
|
Beta Was this translation helpful? Give feedback.
-
|
Okay nevermind, just launch again the code; and the robot did move ! Yay ! I will do furthermore testing tomorrow and keep you update If I get new troubles. Thank you again, have a good day. |
Beta Was this translation helpful? Give feedback.
-
|
If you get this situation again, you can reset everything programmatically by calling the |
Beta Was this translation helpful? Give feedback.
Uh oh!
There was an error while loading. Please reload this page.
Uh oh!
There was an error while loading. Please reload this page.
-
System
My goal
I would like to control a MA2010 with moveit2 (REMOTE control). I'm following this specific section commanding_motion and achieved everything expect step n°6 when I have to call service
start_traj_modeError
Same for
start_point_queue_mode service.From what I know I need to close every door to be able to activate servo motors. However even with everything correclty close; I still got the following message on the teach pendant: 'Operation forbidden by external unit' (translated from french).
Beta Was this translation helpful? Give feedback.
All reactions