search

Unity-Technologies / Unity-Robotics-Hub

Central repository for tools, tutorials, resources, and documentation for robotics simulation in Unity.
Apache License 2.0
2.02k stars 416 forks source link

Collision Detection for Articulation Body Components #301

Closed NathanielRose closed 3 years ago

NathanielRose commented 3 years ago

Is your feature request related to a problem? Please describe. During testing of my custom URDF, often I want to detect the instances in which a failure occurs in my simulation. For the pick and place task, I would like to e able to detect when my arm collides with the table, which joint and which object it collides with.

Describe the solution you'd like In an instance where my robotic arm collides with a table, I would like an array and metrics of which arm joint and component collided with my table object in the unity scene.

https://user-images.githubusercontent.com/8294326/130960810-d408d118-10bc-4111-8b7b-e377a09663da.mp4

Additionally, I would like a better way to understand why my robotic arm gets dismounted during certain trials of a pick and place task.

https://user-images.githubusercontent.com/8294326/130961360-e2c11e7b-a44a-4280-b875-6590520b33bd.mp4

Describe alternatives you've considered Is the mesh collider something that I should leverage for this sort of information? https://docs.unity3d.com/Manual/class-MeshCollider.html

kosswald commented 3 years ago

Hi. Thanks for reporting this. It seems very strange that your arm is getting dismounted. Can you give more context as to what makes the arm dismount? Under the conditions of the pick-and-place tutorial, this shouldn't be possible so please let me know how to recreate the issue.

For collision detection, I would use colliders to determine when the arm is hitting the table or other objects. You can then use the collider as triggers to record the position of the arm and which object it hit.

NathanielRose commented 3 years ago

HI @kosswald Thanks for the response.

The dismounting occurs quite often, presumably 1/8 times when I conduct a pick and place.

Here is my URDF: https://github.com/NathanielRose/UniversalRobot10_RobotiqGripper/blob/main/ur3_with_gripper/ur10_with_gripper.urdf

And here is a capture of my URDF settings in my scene:

capture

It is identical to the tutorial.

kosswald commented 3 years ago

Ok just want to check. Did you make the baselink Articulation Body Immovable?

NathanielRose commented 3 years ago

Yes the base link is also immovable

capture
kosswald commented 3 years ago

Thank @NathanielRose, since you're using a custom URDF, can you also provide the scripts you use to manipulate the arm. It might be that the script is adjusting the position of the arm, making it move out of sight.

NathanielRose commented 3 years ago

@kosswald The execution of the articulation joints are identical to the sample scripts the repo provides. I add my script here which includes some minor changes - https://github.com/NathanielRose/UniversalRobot10_RobotiqGripper/blob/main/UR10TrajectoryPlanner.cs

ZhuoyunZhong commented 3 years ago

For detection the collision, you could try the codes I am using right now. CollisionReader.cs ArticulationCollisionDetection.cs

The CollisionReader Script will add a component ArticulationCollisionDetection to each ArticulationBody. ArticulationCollisionDetection is in charge of capturing collision, and send back the collision info to the CollisionReader. When the CollisionReader receives a collision, it will play a collision sound and store the collision info.

But be careful that these codes only work on Unity 2021. In the previous Unity version, I found out that the ArticulationBody could not capture the collision properly (OnCollisionEnter() does not work). In this case, you will need to assign a collision detector to the each Collidor instead of the ArticulationBody.

mqt0029 commented 3 years ago

Not sure if related but for the arm dismounting i.e. glitch and fly off into the unknown, I ran into it when setting shoulder lift to some negative angle e.g. -90 on starting the simulation. As it turns out it was solved by setting the primary controller stiffness, damping, and force limit to 1/10 of what you are using.

I am using UR5 URDF generated from ROS Xacro from fmauch/universal_robots so I'm not sure it would be comparable to your specific case.

NathanielRose commented 3 years ago

@ZhuoyunZhong Thank you for your input! I will upgrade from 2020 to unity 2021 to see if my simulator still works in that environment.

@mqt0029 Does that mean you're URDF config would be equivalent to the following vs the recommended config in the tutorial?

Stiffness:1000
Damping:100
Force Limit:100
Speed:3
Torque:10
Acceleration:.5
capture
peifeng-unity commented 3 years ago

Hi @NathanielRose, the dismount issue seems to be caused by an extremely large force or impulse applied to the articulation body. This may be caused by different factors. Could you double-check if you are using the TGS solver? It may also help to increase the default solver iteration and default solver velocity iteration to larger values. I've tried to reproduce this issue, but it works well in my project without any dismount happening. It would be very helpful for us to take a closer look if you don't mind sharing the project and ROS packages.

For the collision detection, agree with @ZhuoyunZhong that you may need to get the collider component on the articulation body (e.g. GetComponent<Collider>()) and detect the collision in the OnCollisionEnter()

mqt0029 commented 3 years ago

Attached is my Controller script settings and the code piece that allow me to rapidly snaps the arm into location (jumping from one joint value to another) to sync up with MoveIt on the ROS backend. As @peifeng-unity mentioned this is not the only cause, I also met similar issues when ROS TCP server cannot keep up with the amount of service request e.g. I tried to request all joint values from MoveIt every frame i.e. Update() and my arm exploded while doing it using IvokeRepeating every 0.1 seconds seems to work just fine.

// sync up just fine
InvokeRepeating("RequestJointSynchronization", 0f, 0.08f);

// arm exploded, complained about no response from service
void Update()
{
    RequestJointSynchronization();
}

2021-09-08_19-54

NathanielRose commented 3 years ago

@mqt0029 Thanks for sharing your config for the controller. I gave your setting a try in my scene and the UR10 for me immediately jumps off at the start. At the start of my script I set my UR10 to a neutral position using the code below.

https://user-images.githubusercontent.com/8294326/132987757-9a459dcf-7724-4ceb-8b1e-b71ecb48c142.mp4

 void Start()
    {
        // start the ROS connection
        ros = ROSConnection.instance;

        // Adjust UR10 into a default position from Ali: [0, -2.79, 2.356, -1.134, -1.57, 0] --> In Radians
        var joint1XDrive = jointArticulationBodies[1].xDrive;
        joint1XDrive.target = -160;
        jointArticulationBodies[1].xDrive = joint1XDrive;

        var joint2XDrive = jointArticulationBodies[2].xDrive;
        joint2XDrive.target = 135;
        jointArticulationBodies[2].xDrive = joint2XDrive;

        var joint3XDrive = jointArticulationBodies[3].xDrive;
        joint3XDrive.target = -65;
        jointArticulationBodies[3].xDrive = joint3XDrive;

        var joint4XDrive = jointArticulationBodies[4].xDrive;
        joint4XDrive.target = -90;
        jointArticulationBodies[4].xDrive = joint4XDrive;
        }

By adjusting the damping, I get a bit more improvement but the gripping of my object during the pick and place task is too early. I believe this can be controlled with increasing the assignmentwait parameter I am using. This significantly increases the time it takes to complete the task.

https://user-images.githubusercontent.com/8294326/132988610-155c6e43-471c-4a70-af20-8a25942bf5a0.mp4

mqt0029 commented 3 years ago

That is really strange! I tried your code snipper with UR10 and it worked just fine, even when using higher stiffness and force limit settings e.g. 10000. I'll try with your URDF instead and see if I can reproduce your problem.

NathanielRose commented 3 years ago

@mqt0029 Any update on the URDF?

mqt0029 commented 3 years ago

@NathanielRose as it turns out the collision did came from the gripper, and doing either #216's solution or running the URDF through MoveIt setup assistant and generate a Gazebo compatible URDF disabled all the necessary contacts so it won't happen again.

For reference, I am using jr-robotics/robotiq and fmauch's UR description running on ROS Noetic on Ubuntu 20.04 Docker container. Here's what my xacro looked like, referencing a modified robotiq_arg2f_140_macro.xacro disabling the base_link to avoid duplicate link errors.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur10_robotiq_2f_140_gripper">

    <xacro:include filename="$(find unity_robot_description)/xacro/macros/robotiq_arg2f_140_macro.xacro"/>
    <xacro:include filename="$(find ur_description)/urdf/inc/ur10_macro.xacro"/>

    <xacro:macro name="ur10_with_gripper" params="prefix">
        <!-- Universal Robot UR 10 Arm -->
        <xacro:ur10_robot prefix="${prefix}"/>

        <!-- Robotiq 2F-140 2-Finger Gripper -->
        <xacro:robotiq_arg2f_140 prefix="${prefix}" />

        <!-- attach gripper to tool0 -->
        <joint name="ur10_tool0-gripper_base" type="fixed">
            <parent link= "${prefix}tool0" />
            <child link = "${prefix}robotiq_arg2f_base_link" />
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </joint>

    </xacro:macro>

    <xacro:ur10_with_gripper prefix=""/>
</robot>

And here's the snippet that disable all the links and joints that MoveIt! detected and disabled in the generated SRDF. This seems to be an extended version of #216's solution.

    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
    <disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Never"/>
    <disable_collisions link1="base_link_inertia" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
    <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
    <disable_collisions link1="left_inner_finger" link2="left_inner_finger_pad" reason="Adjacent"/>
    <disable_collisions link1="left_inner_finger" link2="left_inner_knuckle" reason="Default"/>
    <disable_collisions link1="left_inner_finger" link2="left_outer_finger" reason="Adjacent"/>
    <disable_collisions link1="left_inner_finger" link2="left_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="right_inner_finger" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="right_inner_finger_pad" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="right_inner_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="robotiq_arg2f_base_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="left_inner_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="left_outer_finger" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="left_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="right_inner_finger" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="right_inner_finger_pad" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="right_inner_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="robotiq_arg2f_base_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="left_inner_finger_pad" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="left_outer_finger" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="left_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="right_inner_finger" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="right_inner_finger_pad" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="right_inner_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="robotiq_arg2f_base_link" reason="Adjacent"/>
    <disable_collisions link1="left_inner_knuckle" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="left_inner_knuckle" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="left_outer_knuckle" reason="Adjacent"/>
    <disable_collisions link1="left_outer_finger" link2="right_inner_finger" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="right_inner_finger_pad" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="right_inner_knuckle" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="robotiq_arg2f_base_link" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="left_outer_finger" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="right_inner_finger" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="right_inner_finger_pad" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="right_inner_knuckle" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="robotiq_arg2f_base_link" reason="Adjacent"/>
    <disable_collisions link1="left_outer_knuckle" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="left_outer_knuckle" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger" link2="right_inner_finger_pad" reason="Adjacent"/>
    <disable_collisions link1="right_inner_finger" link2="right_inner_knuckle" reason="Default"/>
    <disable_collisions link1="right_inner_finger" link2="right_outer_finger" reason="Adjacent"/>
    <disable_collisions link1="right_inner_finger" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="right_inner_finger" link2="robotiq_arg2f_base_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="right_inner_knuckle" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="robotiq_arg2f_base_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="right_inner_finger_pad" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="right_inner_knuckle" link2="right_outer_finger" reason="Never"/>
    <disable_collisions link1="right_inner_knuckle" link2="right_outer_knuckle" reason="Never"/>
    <disable_collisions link1="right_inner_knuckle" link2="robotiq_arg2f_base_link" reason="Adjacent"/>
    <disable_collisions link1="right_inner_knuckle" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="right_inner_knuckle" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="right_inner_knuckle" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="right_outer_finger" link2="right_outer_knuckle" reason="Adjacent"/>
    <disable_collisions link1="right_outer_finger" link2="robotiq_arg2f_base_link" reason="Never"/>
    <disable_collisions link1="right_outer_finger" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="right_outer_finger" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="right_outer_finger" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="right_outer_knuckle" link2="robotiq_arg2f_base_link" reason="Adjacent"/>
    <disable_collisions link1="right_outer_knuckle" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="right_outer_knuckle" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="right_outer_knuckle" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="robotiq_arg2f_base_link" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="robotiq_arg2f_base_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="robotiq_arg2f_base_link" link2="wrist_3_link" reason="Adjacent"/>
    <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
    <disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never"/>
    <disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never"/>
    <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/>
    <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/>
    <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>

For more info have a read at MoveIt! Setup Assistant tutorial.

mqt0029 commented 2 years ago

While the issue is closed, a minor notice that may be helpful for ROS users.

If you are lazy like I am, you probably used MoveIt! Setup Assistance to generate the collision matrix, like I did in https://github.com/Unity-Technologies/Unity-Robotics-Hub/issues/301#issuecomment-928397938. There are three changes I would make before importing the URDF into Unity.

  1. The supported tag is <disable_collision>, and not <disable_collisions> (no s at the end)
  2. Close the tag with </disable_collision>, and not />
  3. (Optionally) Remove the reason part.

Took me forever to figure this out, because as https://github.com/Unity-Technologies/Unity-Robotics-Hub/issues/216#issuecomment-865325462 pointed out, internal collision is probably the first issue a lot of developers bumps into.