-
Notifications
You must be signed in to change notification settings - Fork 2
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Execute trajectory #154
base: main
Are you sure you want to change the base?
Execute trajectory #154
Conversation
Thought to keep in mind: what if communication with the robot lags: will we get large jumps with ServoJ? |
|
||
self._assert_joint_configuration_nearby(joint_trajectory.path.positions[0]) | ||
|
||
if joint_trajectory.path.velocities is not None: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
might want to calculate the leading axis velocities and check if they are not too high?
Is the velocity profile used currently? Bc the servoJ does not implement it iirc?
# ) | ||
|
||
# This avoids the abrupt stop and "thunk" sounds at the end of paths that end with non-zero velocity | ||
# TODO: This is not an attribute of PositionManipulator, but is specific to URrtde. How do we implement this correctly? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
could add this function to the interface? But do we want to have this as default behavior? shouldn't the user ensure the trajectory is well-behaved?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If we do not add the function to the interface (and use it here) I believe a warning or at the very least a comment in the method's docstring would be appropriate, to avoid mistakes when new users execute trajectories that end with non-zero velocities.
|
||
# Servo can overshoot. Do a final move to the last configuration. | ||
# manipulator._assert_joint_configuration_nearby(joint_trajectory.path.positions[-1]) | ||
self.move_to_joint_configuration(joint_trajectory.path.positions[-1]).wait() |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
do we always want this? I can't immediately think of a scenario though.
joint_trajectory: the joint trajectory to execute.""" | ||
self._assert_joint_trajectory_is_executable(joint_trajectory) | ||
|
||
period = 0.005 # Time per servo, approximately. This may be slightly changed because of rounding errors. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
how did you determine this value?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I took it from the cloth competition codebase, I suppose 0.002 would also be a fair choice (500Hz)
Seems like a very good first version! some thoughts on the considerations: code duplication I think this is valid. Maybe add comment to explain? sleeping additional questions:
|
yes. One way to avoid this would be to get the current joint config and verify it is not too far from the target config, but that might be too expensive to run at 500Hz? |
for reference, this is the ros2_control "trajectory execution controller": https://control.ros.org/rolling/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html Way more generic and advanced thatn what we are building, but useful as reference. |
Describe your changes
This pull request (WIP) monitors the progress on implementing issue #150.
It includes a new
execute_trajectory
method for thePositionManipulator
andDualArmPositionManipulator
classes.Current progress
Currently, there is an implementation of the
execute_trajectory
method for thePositionManipulator
andDualArmPositionManipulator
classes.No implementation for
execute_trajectory
for theMobileRobot
is (currently) provided, as this is not in scope of issue #150, but more on this below.The implementation is currently blocking. We hadn't decided yet if we want it to be non-blocking.
Considerations
Code duplication
To ensure that the trajectories of both manipulators in the
DualArmPositionManipulator
are executed in a synchronized manner, the implementation ofexecute_trajectory
in this class does not delegate to the_left_manipulator
(resp._right
)'s implementation, but is its own implementation with some code duplication. An alternative would be to make thePositionManipulator
's implementation non-blocking (running it in a separate thread), and await both arm's executions. However, this could lead to synchronization issues based on thread scheduling (also, take into account the GIL's impact on multithreaded code), and trajectory execution for both grippers would still need to be implemented in the current way.Sleeping in trajectory execution
The current implementation does not sleep, but sends servo commands to the robot as fast as it can. This is done to avoid execution lag. This should be fine, as the control box will only execute the latest servo command. Is this the behaviour we want?
Testing
These methods need to be tested extensively on different set-ups. Currently, the
DualArmPositionManipulator
implementation has not been tested on real hardware. The gripper trajectory execution has also not yet been tested.Trajectory types and implementation effort
airo-typing
defines two main types:This makes the difference between a trajectory for one arm and for two arms explicit and allows for the execution of trajectories for two arms simultaneously.
However, when we add the mobile platform to the equation, we would need to consider all combinations of arms and the mobile platform (
SingleArmOnMobilePlatformTrajectory
,DualArmOnMobilePlatformTrajectory
) if we want to move the arm(s) and platform concurrently.This results in a large amount of duplicated code to execute trajectories.
Can we avoid it somehow, perhaps by making trajectory execution non-blocking? (This would require testing if multithreaded execution results in unexpected behaviour.)
Creating trajectories
The trajectories we obtain from
airo-planner
are instances of Drake'sPathParameterizedTrajectory
class.These can be converted to
airo-typing
'sSingleArmTrajectory
class with this function or the following code block:Checklist