#VRML_SIM R2022b utf8
# license: Copyright Cyberbotics Ltd. Licensed for use only with Webots.
# license url: https://cyberbotics.com/webots_assets_license
# documentation url: https://www.cyberbotics.com/doc/guide/irb4600-40
# The "IRB 4600/40" is a 6 DOF arm developed by ABB: https://new.abb.com/products/robotics/industrial-robots/irb-4600
# It has a payload of 40kg.
# template language: javascript

EXTERNPROTO "webots://projects/appearances/protos/BrushedAluminium.proto"

PROTO Irb4600-40 [
  field SFVec3f    translation     0 0 0                 # Is `Transform.translation`.
  field SFRotation rotation        0 0 1 0               # Is `Transform.rotation`.
  field SFString   name            "IRB 4600/40"         # Is `Solid.name`.
  field SFString   controller      "inverse_kinematics"  # Is `Robot.controller`.
  field MFString   controllerArgs  []                    # Is `Robot.controllerArgs`.
  field SFString   window          "<generic>"           # Is `Robot.window`.
  field SFString   customData      ""                    # Is `Robot.customData`.
  field SFBool     supervisor      FALSE                 # Is `Robot.supervisor`.
  field SFBool     synchronization TRUE                  # Is `Robot.synchronization`.
  field SFColor    color           1 0.45 0              # Is `PBRAppearance.baseColor`.
  field MFNode     handSlot        []                    # Extends the arm with new nodes in the hand slot.
  field SFBool     staticBase      FALSE                 # Defines if the robot base should be pinned to the static environment.
]
{
  Robot {
    translation IS translation
    rotation IS rotation
    name IS name
    children [
      Shape {
        appearance DEF IRB_APPEARANCE PBRAppearance {
          baseColor IS color
          metalness 0
          roughness 0.1
        }
        geometry Mesh {
          url "Irb4600-40/meshes/base.obj"
        }
      }
      DEF LINK1_TRANSFORM HingeJoint {
        jointParameters HingeJointParameters {
          axis 0 0 1
          anchor 0 0 0.159498
        }
        device [
          RotationalMotor {
            name "A motor"
            maxTorque 10000
            minPosition -3.1415
            maxPosition 3.1415
          }
          PositionSensor {
            name "A sensor"
          }
        ]
        endPoint Solid {
          translation 0 0 0.159498
          children [
            Shape {
              appearance USE IRB_APPEARANCE
              geometry Mesh {
                url "Irb4600-40/meshes/link_1.obj"
              }
            }
            DEF LINK2_TRANSFORM HingeJoint {
              jointParameters HingeJointParameters {
                axis 0 1 0
                anchor 0.178445 -0.122498 0.334888
              }
              device [
                RotationalMotor {
                  name "B motor"
                  minPosition -1.5708
                  maxPosition 2.61799
                  maxTorque 10000
                }
                PositionSensor {
                  name "B sensor"
                }
              ]
              endPoint Solid {
                translation 0.178445 -0.122498 0.334888
                children [
                  Shape {
                    appearance USE IRB_APPEARANCE
                    geometry Mesh {
                      url "Irb4600-40/meshes/link_2.obj"
                    }
                  }
                  DEF LINK3_TRANSFORM HingeJoint {
                    jointParameters HingeJointParameters {
                      axis 0 1 0
                      anchor -0.003447 -0.0267 1.095594
                    }
                    device [
                      RotationalMotor {
                        name "C motor"
                        minPosition -3.1415
                        maxPosition 1.309
                        maxTorque 10000
                      }
                      PositionSensor {
                        name "C sensor"
                      }
                    ]
                    endPoint Solid {
                      translation -0.003447 -0.0267 1.095594
                      children [
                        Shape {
                          appearance USE IRB_APPEARANCE
                          geometry Mesh {
                            url "Irb4600-40/meshes/link_3.obj"
                          }
                        }
                        DEF LINK4_TRANSFORM HingeJoint {
                          jointParameters HingeJointParameters {
                            anchor 0.340095 0.149198 0.174998
                          }
                          device [
                            RotationalMotor {
                              name "D motor"
                              minPosition -6.98132
                              maxPosition 6.98132
                              maxTorque 10000
                            }
                            PositionSensor {
                              name "D sensor"
                            }
                          ]
                          endPoint Solid {
                            translation 0.340095 0.149198 0.174998
                            children [
                              Shape {
                                appearance USE IRB_APPEARANCE
                                geometry Mesh {
                                  url "Irb4600-40/meshes/link_4.obj"
                                }
                              }
                              DEF LINK5_TRANSFORM HingeJoint {
                                jointParameters HingeJointParameters {
                                  axis 0 1 0
                                  anchor 0.929888 0 0
                                }
                                device [
                                  RotationalMotor {
                                    name "E motor"
                                    minPosition -2.18166
                                    maxPosition 2.0944
                                    maxTorque 10000
                                  }
                                  PositionSensor {
                                    name "E sensor"
                                  }
                                ]
                                endPoint Solid {
                                  translation 0.929888 0 0
                                  children [
                                    Shape {
                                      appearance USE IRB_APPEARANCE
                                      geometry Mesh {
                                        url "Irb4600-40/meshes/link_5.obj"
                                      }
                                    }
                                    DEF LINK6_TRANSFORM HingeJoint {
                                      jointParameters HingeJointParameters {
                                        anchor 0.125 0 0
                                      }
                                      device [
                                        RotationalMotor {
                                          name "F motor"
                                          minPosition -6.98132
                                          maxPosition 6.98132
                                          maxTorque 10000
                                        }
                                        PositionSensor {
                                          name "F sensor"
                                        }
                                      ]
                                      endPoint Solid {
                                        translation 0.125 0 0
                                        children [
                                          Transform {
                                            translation 0.06 0 0
                                            rotation 0.5773502691896257 0.5773502691896257 0.5773502691896257 2.094399693899575
                                            children IS handSlot
                                          }
                                          Shape {
                                            appearance BrushedAluminium {
                                            }
                                            geometry Mesh {
                                              url "Irb4600-40/meshes/link_6.obj"
                                            }
                                          }
                                        ]
                                        boundingObject Transform {
                                          translation -0.025004 0.002215 -0.000958
                                          children [
                                            Box {
                                              size 0.069993 0.125817 0.125113
                                            }
                                          ]
                                        }
                                        physics Physics {
                                          density 100
                                        }
                                      }
                                    }
                                  ]
                                  boundingObject Transform {
                                    translation 0 -0.0015 0
                                    children [
                                      Box {
                                        size 0.159731 0.130998 0.160194
                                      }
                                    ]
                                  }
                                  physics Physics {
                                    density 200
                                  }
                                }
                              }
                            ]
                            boundingObject Transform {
                              translation 0.511874 0.003021 0
                              children [
                                Box {
                                  size 1.040973 0.228038 0.195956
                                }
                              ]
                            }
                            physics Physics {
                              density 300
                            }
                          }
                        }
                      ]
                      boundingObject Transform {
                        translation 0.085806 0.199325 0.094072
                        children [
                          Box {
                            size 0.533578 0.398651 0.472912
                          }
                        ]
                      }
                      physics Physics {
                        density 400
                      }
                    }
                  }
                ]
                boundingObject Transform {
                  translation -0.003447 -0.061473 0.529095
                  children [
                    Box {
                      size 0.279997 0.256948 1.335994
                    }
                  ]
                }
                physics Physics {
                  density 500
                }
              }
            }
          ]
          boundingObject Transform {
            translation 0.046699 0.049037 0.270246
            children [
              Box {
                size 0.621392 0.540918 0.552493
              }
            ]
          }
          physics Physics {
            density 1150
          }
        }
      }
    ]
    boundingObject Transform {
      translation -0.063119 0 0.106497
      children [
        Box {
          size 0.676133 0.514411 0.213001
        }
      ]
    }
    %< if (!fields.staticBase.value) { >%
    physics Physics {
      density 1500
    }
    %< } >%
    controller IS controller
    controllerArgs IS controllerArgs
    window IS window
    customData IS customData
    supervisor IS supervisor
    synchronization IS synchronization
  }
}