for static obstacle avoiding

[DiomandLEE]

I read the code and have a question : you realize the static obstacle avoiding by add the obstacle urdf to the robot urdf? if yes , how do you link these two urdf , which one is the base_link and how to create the collison_pair?

[adamheins]

We actually specify the robot and obstacles in two separate URDFs, where the base link in each one is assumed to refer to the same global world frame. There ultimately get combined in a Pinocchio geometry model to do collision checking.

The robot URDF is specified in the controller.robot and simulation.robot configuration objects; see here for example. The obstacle URDF and collision pairs are specified under controller.obstacles and simulation.static_obstacles, as in this example. You can also find the documentation on the configuration options in general here.

[DiomandLEE]

我们实际上在两个单独的 URDF 中指定机器人和障碍物，其中每个 URDF 中的基本链接都假定引用相同的全局世界框架。最终组合成一个匹诺曹几何模型来进行碰撞检查。

机器人URDF在controller.robot和simulation.robot配置对象中指定；例如，请参见此处。障碍物 URDF 和碰撞对在controller.obstacles和 下指定simulation.static_obstacles，如本例所示。您还可以在此处找到有关配置选项的一般文档。 I'm very happy and excited to receive your response. After carefully reading it, here's my understanding: First, by merging the URDF files of the obstacles and the robot in the same world frame, collision detection can be performed using the Pinocchio library. Then, the self-collision constraints in OCS2 are used to avoid collisions with obstacles, is that correct? Besides, since I couldn't successfully run your repository, I couldn't print to get more details. Therefore, I still have two doubts about the implementation of the code. First, how do you handle dynamic obstacles in OCS2? I noticed that you defined the "mode" option in the configuration file for dynamic obstacles, what is this for? Second, how is the time information of sudden obstacles in the simulation added to OCS2?

[DiomandLEE]

I'm very happy and excited to receive your response !!! After carefully reading it, here's my understanding: First, by merging the URDF files of the obstacles and the robot in the same world frame, collision detection can be performed using the Pinocchio library. Then, the self-collision constraints in OCS2 are used to avoid collisions with obstacles, is that correct? Besides, since I couldn't successfully run your repository, I couldn't print to get more details. Therefore, I still have two doubts about the implementation of the code. First, how do you handle dynamic obstacles in OCS2? I noticed that you defined the "mode" option in the configuration file for dynamic obstacles, what is this for? Second, how is the time information of sudden obstacles in the simulation added to OCS2?

[adamheins]

Yes, your understanding of the static obstacles is correct.

Regarding your subsequent questions, dynamic obstacles are handled by augmenting the robot state vector and dynamics; basically, any dynamic obstacle also adds its state (position, velocity) and dynamics to the system. For the "sudden" obstacles, the idea is that their state changes abruptly, which we specify using different modes (corresponding to different state trajectories). Each mode is associated with a time at which it becomes active. OCS2 doesn't actually know about the modes, since we want to test how well the controller can respond to sudden events (without preparing for them in advance): they only effect the simulation environment itself (or the ROS controller if working on the real robot).

[DiomandLEE]

Could you provide more details on how dynamic obstacles are handled and how to augment the robot's state and dynamics? I still don't understand.Besides,I noticed you have a function to calculate the shortest time for a projectile to travel in parabolic motion to a specific point. Could you tell me where this function is used? Also, regarding obstacle avoidance for projectiles in real-world scenarios, it seems different from the example of moving robotic arms in OCS2. I noticed you didn't modify mpc_node but rather mrt_node. the mode is used in mrt_node.what is it used for? and Is it in this mrt_node where you evaluate the trajectories generated by MPC and the predicted trajectories of the projectiles for collisions? After evaluation, what other operations are performed? I'm really interested in this aspect as well.

[adamheins]

Basically if you have the robot state $x{robot}$ with dynamics $\dot{x}=f{robot}(x{robot},u)$ and the obstacle state $x{obs}$ with dynamics $\dot{x}_{obs}=f_{obs}(x_{obs})$, we can concatenate them to make one combined system with state $x=[x{robot}^T,x{obs}^T]^T$ and dynamics $f(x,u)=[f{robot}(x{robot},u)^T,f{obs}(x{obs})^T]^T$. This is ultimately done in this class.

The parabolic motion equation is used in this file to compute the time and location at which the obstacle will be closest to a given point. This is used by the controller to compute the closest point to the end effector at each time step to facilitate collision avoidance.

The usage of the modes in the MRT node is a little ad hoc, but it's basically some simple state machine logic to transition between obstacle modes (dynamics) at different points. For example, the "sudden" obstacle changes modes (i.e., its position changes) at a prespecified time. The projectile changes modes once it reaches different heights. Overall, the MRT node runs the main controller loop, which includes tracking the MPC trajectory, state estimation, etc.

[DiomandLEE]

  After carefully reading all the obstacle-related constraint constructions in controller_interface and projectile_path_constraint with your detailed guidance, I have a few new questions.   Firstly, regarding the usage of Pinocchio, in this code repository, here,the robot state used is the position and velocity of the robot plus the position, velocity, and acceleration of the projectile. Why is the entire state passed to Pinocchio's kinematic calculations here instead of just the position information?   Secondly, why add a dynamic obstacle model of type 'FLOAT' to the scene? I think you haven't used it, have you?   Thirdly,here when calculating the distance to the end effector, why is only the projectile in motion while the specified end effector does not move? I understand that this is because you've defined the desired trajectory of the end effector as fixed at a certain pose in the YAML file, but this is not general. My current idea is if the mobile manipulator moves along a trajectory instead of being fixed at a certain pose, how should I address this issue? I currently have no solutions for deployment on OCS2. Could you provide some inspiration?   Lastly, regarding the representation of dynamic obstacle constraints, you introduced an s, but I'm not sure what this is. Is it the expected velocity of the dynamic obstacle? If so, why isn't the partial derivative taken for this term when calculating the '[linear approximation?

[DiomandLEE]

And Why do ‘s’ separate at 0.5？

[adamheins]

Firstly, regarding the usage of Pinocchio, in this code repository, here,the robot state used is the position and velocity of the robot plus the position, velocity, and acceleration of the projectile. Why is the entire state passed to Pinocchio's kinematic calculations here instead of just the position information?

This is just how OCS2's kinematics object works; it has functions to map the full state to various quantities of interest like the Cartesian position.

Secondly, why add a dynamic obstacle model of type 'FLOAT' to the scene? I think you haven't used it, have you?

This is used to augment the Pinocchio models with the dynamic obstacles, if any are used in the scene. Each dynamic obstacle is represented as a sphere with three translational degrees of freedom.

Thirdly,here when calculating the distance to the end effector, why is only the projectile in motion while the specified end effector does not move? I understand that this is because you've defined the desired trajectory of the end effector as fixed at a certain pose in the YAML file, but this is not general. My current idea is if the mobile manipulator moves along a trajectory instead of being fixed at a certain pose, how should I address this issue? I currently have no solutions for deployment on OCS2. Could you provide some inspiration?

We are not assuming the end effector does not move; this function is called at each timestep of the control horizon, and so handles the general trajectory of the EE. However, instead of just trying to avoid the obstacle's predicted position at a given time, we found it more effective to try to avoid the obstacle's entire future path. At each timestep, imagine that a tube is drawn around the entire future trajectory of the ball and we want the EE to avoid it.

Lastly, regarding the representation of dynamic obstacle constraints, you introduced an s, but I'm not sure what this is. Is it the expected velocity of the dynamic obstacle? If so, why isn't the partial derivative taken for this term when calculating the '[linear approximation?

The s value is actually just a signal for when the system should start trying to avoid the thrown ball; it is 0 when the avoidance constraints are turned off, and switched to 1 when the avoidance constraints should be turned on. In particular, this is done when the ball has reached a certain height (for consistency, and so we can be confident that the ball is actually in free flight).

[DiomandLEE]

Thank you for your words! I appreciate your understanding. Now I have two more questions. Firstly, why include the dynamic obstacle model in Pinocchio? Does it mean that projectiles are not classified as dynamic obstacles in your code, but rather refers to suddenly appearing obstacles? Or is it that only with the introduce of the dynamic obstacle model, the kinematics can be computed here? Secondly, why is it that when s=0, the avoidance constraints for dodging projectiles are turned off? Can't the robot avoid projectiles just by avoiding the predicted positions of projectiles at given time points? Isn't it just less effective rather than impossible?

[DiomandLEE]

And why this all obstacles go first in list of q, v ,but this robot go first ? And I don`t find where define extra sphere , what is it used for ?

[adamheins]

Firstly, why include the dynamic obstacle model in Pinocchio? Does it mean that projectiles are not classified as dynamic obstacles in your code, but rather refers to suddenly appearing obstacles? Or is it that only with the introduce of the dynamic obstacle model, the kinematics can be computed here?

Yes, we need to add the projectile as a dynamic obstacle so we can use Pinocchio to compute its kinematics.

Secondly, why is it that when s=0, the avoidance constraints for dodging projectiles are turned off? Can't the robot avoid projectiles just by avoiding the predicted positions of projectiles at given time points? Isn't it just less effective rather than impossible?

I think it is true that even with s=0, the controller will still try to avoid the predicted positions at each timestep. We do however want to be able to turn off the avoidance constraints for the entire future projectile path, because we don't want the robot predicted and then trying to avoid an erroneous path just because I am moving the ball around in my hand.

And why this all obstacles go first in list of q, v ,but this robot go first ?

At the time there was some reason it was convenient to put the obstacles first in the Pinocchio model (q and v) but encode them at the end of the state x. I don't remember why.

And I don`t find where define extra sphere , what is it used for ?

I think this was an old experimental feature that is not used anywhere.

[DiomandLEE]

I'm having trouble understanding this "because we don't want the robot predicted and then trying to avoid an erroneous path just because I am moving the ball around in my hand.". Is this parameter 's' in place to prevent the robot from reacting to a ball that has entered the Vicon area but hasn't been thrown, or I remember that you said to ensure that the ball remains at a specific height in the state of "free-flight", with a value of 1 assigned

[adamheins]

Yes, that's right. Our criterion for "hasn't been thrown" is "height less than 1m". When the ball's position reaches 1m height, we assume the ball has been thrown and set s=1 so the robot avoids it.

[DiomandLEE]

I'm very happy for the replies during this period. I have learned a lot from this excellent work and look forward to more excellent works from you!

[adamheins]

Great, I'm glad this was helpful! I'll close this now; feel free to reach out if you have more questions.