Webots simulation platform

This platform is very comfortable for testing kinematics.

You can convert SolidWorks to URDF (ROS officially provides the sw2urdf tool), and then convert URDF to Webots (Webots officially provides the urdf2webots tool).

https://github.com/ros/solidworks_urdf_exporter

https://github.com/cyberbotics/urdf2webots

In this case, even the motor has already been selected for you; you only need to write the kinematics.

Motion control programs can be written in languages such as C/C++, Python, and Java. When controlling, you only need to pay attention to the method and units for controlling the motor angle and speed.

If you setPosition(INFINITY) on a motor, then this motor will become an angle motor, and the function originally used to set speed will become one that sets angular velocity.

If you have not set setPosition(INFINITY) for the motor, then this motor is a speed motor.

The angle unit is rad (uncertain).

The unit of speed is rad/s (uncertain).

        for (int i = 0;i < 4;i++)
        {
                //舵电机仿真中默认顺时针为正
                swerve_motor[i] = robot->getMotor("swerve_joint" + std::to_string(i));
                //swerve_motor[i]->setPosition(INFINITY);       // 注释掉：不启用速度控制
                swerve_motor[i]->setPosition(0.0);       // 初始角度为0
                swerve_motor[i]->setVelocity(50.0);       // 设置角速度

                //驱动电机仿真中默认向后滚为正
                drive_motor[i] = robot->getMotor("drive_joint" + std::to_string(i));
                drive_motor[i]->setPosition(INFINITY);   // 启用速度控制
                drive_motor[i]->setVelocity(-0.0);        // 初始速度为0
        }