unstable simulation
Hi, I have a robot that moves its arm left to hit a block. It works well unless the block begins in a particular place (probably a set of particular places.) In this case the block goes flying off of the board.
I've read the general guidelines to make a stable simulation, but I'm not sure how heavy is too heavy or how light is too light, or even if that is my problem.
I've attached my code, if someone could take a quick look and tell me what is causing this to be unstable I would appreciate it.
If on line 71 you change 5.0 to another value, say 5.25 or 4.9 it works fine. But at x = 5.0 the block just flies off.
@include "Control.tz"
@include "Mobile.tz"
@include "Link.tz"
@include "Joint.tz"
@include "MultiBody.tz"
@use Stationary.
@use PhysicalControl.
@use Movie.
@use File.
# possible values of state
@define BEFORE_HIT 1.
@define MOVING 2.
@define STOPPED_AFTER 3.
@define OFF_TABLE 4.
@define BLOCK_LEFT 0.
@define BLOCK_RIGHT 1.
@define BLOCK_TOP 2.
# possible values of ball location
@define ON_TABLE -10000.
@define ON_FLOOR -20000.
Controller myControl.
PhysicalControl : myControl {
+ variables:
robot (object).
tray (object).
cloudTexture (object).
#myMovie (object).
outFile (object).
block (object).
state, ball-location (int).
+ to init:
floor (object).
self set-integration-step to .001.
state = BEFORE_HIT.
ball-location = ON_TABLE.
outFile = new File.
self point-camera at (0, 0, 0.5) from (0, 100, -7.5). # add (10,0,-7.5)
floor = new Floor.
floor catch-shadows.
floor set-color to (1.0, 0, 0).
cloudTexture = (new Image load from "images/clouds.png").
self set-background-color to (.4, .6, .9).
self set-background-texture-image to cloudTexture.
#************************************
# Create the tray
#************************************
# Now make the tray at (0,0)
tray = 1 new Wall create at (0, 10, 0) size (21, 1.0, 15).
tray set-mu to 10000.
block = 1 new Block init with-location (5.0,12,2.5).
robot = new Robot.
+ to close-file:
outFile close.
#movie close.
+ to get-block-location:
return (block get-location).
+ to get-block-velocity:
return (block get-velocity).
}
Link : Block {
+ variables:
blockShape (object).
count (int).
initial-location (vector).
+ to init with-location location (vector):
initial-location = location.
blockShape = (new Cube init-with size (2, 2, 2)).
blockShape set-mass to .01.
self set-shape to blockShape.
self set-color to (1,0,0).
self move to location. #(5,0,5). #location.
self enable-physics.
return self.
+ to iterate:
count = count + 1.
}
Link : MyLink {
+ variables:
collision_flag (int).
+ to init:
collision_flag = 0.
+ to handle-collision with foodObject (object):
collision_flag = 1.
+ to get-collision-flag:
return collision_flag.
+ to reset-collision-flag:
collision_flag = 0.
}
MultiBody : Robot {
+ variables:
torso, upperArm, slide1, slide2, forearm, hand (object).
slide1Joint (object).
slide2Joint (object).
shoulderJoint (object).
elbowJoint (object).
wristJoint (object).
xcount, ycount (int).
ux,uy (double).
tcount (int).
rand_x_duration (int).
rand_y_duration (int).
last_cx, last_cy, last_ex, last_ey (double). # These are here because I have to calculate
# derivitives manually.
+ to init:
torsoShape, upperArmShape, slideShape, slide2Shape, forearmShape , handShape (object).
UNIT, FORARM_LENGTH, UPPER_ARM_LENGTH, SLIDE_LENGTH, SLIDE2_LENGTH,
TORSO_WIDTH, TORSO_HEIGHT , HAND_WIDTH (double).
last_cx = 0. last_cy = 0. last_ex = 0. last_ey = 0.
UNIT = 1.
FORARM_LENGTH = 8.
UPPER_ARM_LENGTH = 8.
SLIDE_LENGTH = 2.
SLIDE2_LENGTH = 6.
TORSO_WIDTH = 6.
TORSO_HEIGHT = 16.
HAND_WIDTH = 2.
#ux = 0. uy = 0.
ux = -350. uy = 0.
rand_x_duration = 10.
rand_y_duration = 10.
#self enable-self-collisions.
torso = new Link.
upperArm = new Link.
slide1 = new Link.
slide2 = new Link.
forearm = new MyLink.
hand = new MyLink.
upperArmShape = (new Cube init-with size (UPPER_ARM_LENGTH,UNIT,UNIT)).
slideShape = (new Cube init-with size (SLIDE_LENGTH,UNIT,UNIT)).
slide2Shape = (new Cube init-with size (UNIT,UNIT,SLIDE2_LENGTH)).
forearmShape = (new Cube init-with size (UNIT,UNIT,FORARM_LENGTH)).
handShape = (new Cube init-with size (HAND_WIDTH,UNIT,HAND_WIDTH)).
torsoShape = (new Cube init-with size (TORSO_WIDTH,TORSO_HEIGHT,TORSO_WIDTH)).
torsoShape set-mass to 1000.
torso set-shape to torsoShape.
upperArm set-shape to upperArmShape.
slide1 set-shape to slideShape.
slide2 set-shape to slide2Shape.
forearm set-shape to forearmShape.
hand set-shape to handShape.
hand handle-collisions with-type "Block" with-method "handle-collision".
forearm handle-collisions with-type "Block" with-method "handle-collision".
torso set-color to (0,1,0).
upperArm set-color to (0,0,1).
slide1 set-color to (1,1,0).
slide2 set-color to (0,0,1). #(1,0,1).
forearm set-color to (0,0,1). #(.44,3,2).
hand set-color to (0,0,1). #(.23,.44,1).
self set-root to torso.
shoulderJoint = new FixedJoint.
slide1Joint = new PrismaticJoint.
slide2Joint = new PrismaticJoint.
#elbowJoint = new RevoluteJoint.
elbowJoint = new FixedJoint.
wristJoint = new FixedJoint.
shoulderJoint link parent torso to-child upperArm
#with-normal (1, 0, 0)
with-parent-point (TORSO_WIDTH/2,2,2)
with-child-point (-UPPER_ARM_LENGTH/2, 0,0).
slide1Joint link parent upperArm to-child slide1
with-normal (1, 0, 0)
with-parent-point (0,UNIT/2,0)
with-child-point (0,-UNIT/2,0).
slide2Joint link parent slide1 to-child slide2
with-normal (0, 0, 1)
with-parent-point (0,UNIT/2,0)
with-child-point (0, -UNIT/2,0).
elbowJoint link parent slide2 to-child forearm
#with-normal (0, 1, 0)
with-parent-point (0,0,SLIDE2_LENGTH/2)
with-child-point (0, 0,-FORARM_LENGTH/2).
wristJoint link parent forearm to-child hand
with-parent-point (0,0,FORARM_LENGTH/2)
with-child-point (0, 0,-HAND_WIDTH/2).
self move to ((-15)+10,8,(-4) - 7.5). # Because we moved with the tray at the center
slide1Joint set-joint-limits min -2 max 2.
slide1Joint set-strength-limit to 300.
slide1Joint set-joint-damping to 5.
slide2Joint set-joint-limits min -2 max 2.
slide2Joint set-strength-limit to 300.
slide2Joint set-joint-damping to 5.
+ to iterate:
hand_velocity, hand_location, block_velocity, block_location (vector).
hx,hy,hdx,hdy,bx,by,bdx,bdy, bz (double).
be, he (int). # the block and hand exist.
cx, cy, cdx, cdy, ex, ey, edx, edy (double).
#************************************************************
# Get the hand and arm locations
#************************************************************
hand_location = hand get-location.
hx = -hand_location::x. hy = hand_location::z.
hand_velocity = hand get-velocity.
hdx = -hand_velocity::x. hdy = hand_velocity::z.
block_location = controller get-block-location.
bx = -block_location::x. by = block_location::z. bz = block_location::y.
block_velocity = controller get-block-velocity.
bdx = -block_velocity::x. bdy = block_velocity::z.
#************************************************************
# Calculate the distance between the hand and the block
#************************************************************
cx = bx - hx.
cy = by - hy.
ex = |cx| - 2.0. # 2 = .5(BLOCK_WIDTH * HAND_WIDTH).
ey = |cy| - 2.0.
cdx = cx - last_cx.
cdy = cy - last_cy.
edx = ex - last_ex.
edy = ey - last_ey.
last_cx = cx.
last_cy = cy.
last_ex = ex.
last_ey = ey.
#************************************************************
# Determine if the block has fallen off of the table
#************************************************************
be = 1.
if bx > |11.5| || by > |7.5|: be = 0.
he = 1.
# Give it force
slide1Joint set-joint-force to ux.
slide2Joint set-joint-force to uy.
xcount = xcount + 1.
ycount = ycount + 1.
tcount = tcount + 1.
if tcount > 300:
controller end-simulation.
}
Stationary : Wall {
+ to create at location (vector) size sizeVector (vector):
stepShape (object).
stepShape = (new Cube init-with size sizeVector).
self register with-shape stepShape at-location location.
self set-color to (0,0,0).
return self.
}
<\code>.