I'm writing a double joint robots and its controller for a forward kinematics example.
An interesting thing is that the second rotational motor attached to the first rotational motor with a link starts moving to the target position only after the first one has reached the target position.
I expected two rotation motors would move independently.
The world file and the controller are as follows:
#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()
The links have 0 mass values, so I don't think it's caused by the torque limit.
What am I missing?