search

TechmanRobotInc / tmr_ros2

TM Robots supporting ROS2 drivers and some extended external applications. (experimental) (not support the new TM S-Series)
Other
37 stars 21 forks source link

Simulation when using Moveit with tm_driver doesn't move. #17

Open JensVanhooydonck opened 2 years ago

JensVanhooydonck commented 2 years ago

When using the moveit integration, the robot doesn't move in simulation. When changing the funtion set_joint_states from

void TmRobotState::set_joint_states(const std::vector<double> &pos, const std::vector<double> &vel, const std::vector<double> &tor)
{
    joint_angle() = pos;
    joint_speed() = vel;
    joint_torque() = tor;
}

to

void TmRobotState::set_joint_states(const std::vector<double> &pos, const std::vector<double> &vel, const std::vector<double> &tor)
{
    for (size_t i = 0; i < 6; ++i) {
        tmRobotStateDataFromEthernet.joint_angle[i] = pos[i] * (180.0 / M_PI);
        tmRobotStateDataFromEthernet.joint_speed[i] = vel[i];
        tmRobotStateDataFromEthernet.joint_torque[i] = tor[i];
    }
    multiThreadCache.set_catch_data(tmRobotStateDataFromEthernet);
}

The simulation works again.