sbgisen / vesc

VESC Interface for ROS
Apache License 2.0
41 stars 33 forks source link

[vesc_hw_interface]Add parameter to enable/disable enforceLimits() #73

Open nyxrobotics opened 1 year ago

nyxrobotics commented 1 year ago

Abstract

Using enforceLimits(), the speed is limited according to the limit tag written in xacro. If there is a speed limit during position control, the step response cannot be seen. (If the speed limit is set to a large value in xacro, commands sent to the haedware interface will be delayed.) Enable/disable enforceLimits() to temporarily release the speed limit when adjusting gain, for example.

Purpose

To observe step response when adjusting PID gain.

Implementation Details

Add enable_enforce_limits param to vesc_hw_interface.cpp https://github.com/sbgisen/vesc/blob/3d5e62e736d2c3f92d26e006b3ccf48f5f5c4dc4/vesc_hw_interface/src/vesc_hw_interface.cpp#L233-L280

nyxrobotics commented 1 year ago

Position control without limit_position_interface_.enforceLimits(period);

Screenshot from 2023-01-23 14-32-05

Position control with limit_position_interface_.enforceLimits(period); and set <limit effort="100" lower="-10000" upper="10000" velocity="0.2"/> in xacro.

Screenshot from 2023-01-23 14-33-36

nyxrobotics commented 1 year ago

This function rewrites the value of the command_ and cannot be seen in the topic. To check the function, you need to print the command_ in the hardware interface.

jsupratman13 commented 1 year ago

What is the parameters/environment you used to test this?

nyxrobotics commented 1 year ago

The speed_limit in vesc_servo_controller.cpp is unnecessary because it duplicates the functionality.

https://github.com/sbgisen/vesc/blob/3d5e62e736d2c3f92d26e006b3ccf48f5f5c4dc4/vesc_hw_interface/src/vesc_servo_controller.cpp#L61

nyxrobotics commented 1 year ago

What is the parameters/environment you used to test this?

I confirmed this by putting printf in the PID controller and running the motor.

jsupratman13 commented 1 year ago

The speed_limit in vesc_servo_controller.cpp is unnecessary because it duplicates the functionality.

https://github.com/sbgisen/vesc/blob/3d5e62e736d2c3f92d26e006b3ccf48f5f5c4dc4/vesc_hw_interface/src/vesc_servo_controller.cpp#L61

My understanding is that the velocity limit given in the xacro tag is irrelevant when we are using position_controllers/JointPositionController. For VescServoController, we set the hardware interface to JointPositionInterface but internally, we are creating something similar to JointEffortInterface therefore we need the servo/speed_limit parameter to limit the velocity component.

jsupratman13 commented 1 year ago

What happens if you remove limit_position_interface_.enforceLimits(period); but set the <limit effort="100" lower="-10000" upper="10000" velocity="0.0"/> xacro tag?

nyxrobotics commented 1 year ago

What happens if you remove limit_position_interface_.enforceLimits(period); but set the <limit effort="100" lower="-10000" upper="10000" velocity="0.0"/> xacro tag?

I just checked and the xacro limits are not reflected.

jsupratman13 commented 1 year ago

Thanks for the confirmation.

I looked through the enforceLimits function and just as you said the PositionJointSaturationHandle are manually applying a soft limits

https://github.com/ros-controls/ros_control/blob/7f3a4064f8022bdcc577c443a595f42270154098/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.h#L86-L112

jsupratman13 commented 1 year ago

You can probably disable the soft limit by loading this parameter

joint_limits:
  vesc_joint:
    has_velocity_limits: false