双关节机器人的旋转电机

Question

我正在编写一个双关节机器人及其控制器，作为正向运动学示例。

有趣的是，通过连杆连接到第一旋转电动机的第二旋转电动机只有在第一旋转电动机到达目标位置之后才开始移动到目标位置。

我希望两个旋转电机能够独立运动。

世界文件和控制器如下：

#VRML_SIM R2019b utf8
WorldInfo {
}
Viewpoint {
  orientation 1 0 0 4.71238898038469
  position 0 1.86 0
}
TexturedBackground {
}
TexturedBackgroundLight {
}
RectangleArena {
}
DEF TARGET Transform {
  translation 0.2 0 0
  children [
    Transform {
      translation 0 0.005 0
      children [
        Shape {
          appearance Appearance {
            material Material {
              diffuseColor 1 0 0
              transparency 0.2
            }
          }
          geometry Cylinder {
            height 0.02
            radius 0.01
          }
        }
      ]
    }
  ]
}
DEF ROBOT Robot {
  translation 0 0.005 0
  children [
    Shape {
      geometry Cylinder {
        height 0.02
        radius 0.001
      }
    }
    HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 1 0
      }
      device [
        RotationalMotor {
          name "j0"
        }
      ]
      endPoint Solid {
        translation 0.05 0 0
        children [
          Shape {
            appearance Appearance {
              material Material {
                diffuseColor 0 0 0
              }
            }
            geometry Box {
              size 0.1 0.01 0.01
            }
          }
          SliderJoint {
            jointParameters JointParameters {
              axis 1 0 0
            }
            device [
              LinearMotor {
                name "d0"
              }
            ]
            endPoint Solid {
              children [
                Transform {
                  translation 0.05 0 0
                  children [
                    Shape {
                      geometry Cylinder {
                        height 0.02
                        radius 0.001
                      }
                    }
                    HingeJoint {
                      jointParameters HingeJointParameters {
                        axis 0 1 0
                      }
                      device [
                        RotationalMotor {
                          name "j1"
                        }
                      ]
                      endPoint Solid {
                        translation 0.05 0 0
                        children [
                          Shape {
                            appearance Appearance {
                              material Material {
                                diffuseColor 0 0 0
                              }
                            }
                            geometry Box {
                              size 0.1 0.01 0.01
                            }
                          }
                          SliderJoint {
                            jointParameters JointParameters {
                              axis 1 0 0
                            }
                            device [
                              LinearMotor {
                                name "d1"
                              }
                            ]
                            endPoint Solid {
                              children [
                                Shape {
                                  appearance Appearance {
                                    material Material {
                                      diffuseColor 1 1 1
                                    }
                                  }
                                  geometry Box {
                                    size 0.1 0.009 0.009
                                  }
                                }
                                Transform {
                                  translation 0.05 0 0
                                  children [
                                    Shape {
                                      appearance Appearance {
                                        material Material {
                                          diffuseColor 0 0 1
                                        }
                                      }
                                      geometry Box {
                                        size 0.011 0.011 0.011
                                      }
                                    }
                                  ]
                                }
                              ]
                            }
                          }
                        ]
                      }
                    }
                  ]
                }
                Shape {
                  appearance Appearance {
                    material Material {
                      diffuseColor 1 1 1
                    }
                  }
                  geometry Box {
                    size 0.1 0.009 0.009
                  }
                }
              ]
            }
          }
        ]
      }
    }
  ]
  controller "my_controller"
  supervisor TRUE
}

from controller import Supervisor

import math
import random
import numpy as np

def calculateEndPoint(j0, d0, j1, d1):
    m0 = np.array([[math.cos(j0), -math.sin(j0), d0 * math.cos(j0)],
                   [math.sin(j0), math.cos(j0),  d0 * math.sin(j0)],
                   [0,            0,             1]])
    m1 = np.array([[math.cos(j1), -math.sin(j1), d1 * math.cos(j1)],
                   [math.sin(j1), math.cos(j1),  d1 * math.sin(j1)],
                   [0,            0,             1]])
    v = np.array([0, 0, 1]).transpose()

    ret = np.matmul(np.matmul(m0, m1), v)

    return [ret[0], ret[1]]

class MyController(Supervisor):
    TIME_CHANGE_CONFIGURATION = 2
    D_OFFSET = 0.1
    J_MIN = 0
    J_MAX = 2 * math.pi
    D_MIN = 0
    D_MAX = 0.1

    def __init__(self):
        super().__init__()

        self.target = self.getFromDef("TARGET")

        self.j0 = self.getMotor("j0")
        self.d0 = self.getMotor("d0")
        self.j1 = self.getMotor("j1")
        self.d1 = self.getMotor("d1")

    def run(self):
        last_configuration_changed = 0

        while self.step(int(self.getBasicTimeStep())) != -1:
            if self.getTime() - last_configuration_changed > self.TIME_CHANGE_CONFIGURATION:
                # generate random configuration
                j0 = random.uniform(self.J_MIN, self.J_MAX)
                d0 = random.uniform(self.D_MIN, self.D_MAX)
                j1 = random.uniform(self.J_MIN, self.J_MAX)
                d1 = random.uniform(self.D_MIN, self.D_MAX)

                # call assignment function to get the coordinate of the end effector
                [x, y] = calculateEndPoint(j0, d0 + self.D_OFFSET,
                                           j1, d1 + self.D_OFFSET)

                # move the red box to the result
                self.target.getField("translation").setSFVec3f([x, 0, -y])

                # control motor
                self.j0.setPosition(j0)
                self.d0.setPosition(d0)
                self.j1.setPosition(j1)
                self.d1.setPosition(d1)

                # update last updated time
                last_configuration_changed = last_configuration_changed + self.TIME_CHANGE_CONFIGURATION

controller = MyController()
controller.run()

链接的质量值为0，所以我认为这不是由扭矩限制引起的。

我想念什么？