From 48ada82fe142fa56ad7b0c7db82d366cb86db3ef Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 5 Jun 2023 16:51:15 +0200 Subject: [PATCH 01/15] r_abs for all visual elements --- src/Composition/dynamics.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index e1ac86e..b96486a 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -189,8 +189,8 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode # objIndices[i,1]: Index of r_abs of Object3D i # [i,2]: Index of R_abs of Object3D i - objIndices = Matrix{Int}(undef, length(scene.updateVisuElements), 2) - for (i,obj) in enumerate(scene.updateVisuElements) + objIndices = Matrix{Int}(undef, length(scene.allVisuElements), 2) + for (i,obj) in enumerate(scene.allVisuElements) if typeof(obj.feature) == Modia3D.Composition.EmptyObject3DFeature objIndices[i,1] = 0 objIndices[i,2] = 0 From 9f4e4783cb0effbe697930e64c8739e6cd8244b4 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 5 Jun 2023 16:52:30 +0200 Subject: [PATCH 02/15] check if gripping features are defined --- src/Composition/dynamics.jl | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index b96486a..8d9faad 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -146,11 +146,13 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode scene::Scene{F} = mbsBuild.mbs.scene #instantiatedModel = mbsBuild.mbs.instantiatedModel #gripPair = scene.gripPair - if Modia3D.checkGrippingFeatures(scene, scene.gripPair) - Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair) - else - @error("Andrea: print warning für gripping features") - # printWarnGrip(robotOrDepot, movableObj, waitingPeriod) + if isdefined(scene, :gripPair) + if Modia3D.checkGrippingFeatures(scene, scene.gripPair) + Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair) + else + @error("Andrea: print warning für gripping features") + # printWarnGrip(robotOrDepot, movableObj, waitingPeriod) + end end (worldDummy, revoluteObjects, prismaticObjects, freeMotionObjects, hiddenJointObjects, forceElements, resultElements) = checkMultibodySystemAndGetWorldAndJointsAndForceElementsAndResultElements(partiallyInstantiatedModel.modelName, parameters, modelPath, F) From 5d48a083d367f45f1ba0290774337ce263b78941 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 5 Jun 2023 16:56:56 +0200 Subject: [PATCH 03/15] new transportation scenario --- .../YouBotSegmentedCollisionGripping.jl | 592 ++++++++++++++++++ 1 file changed, 592 insertions(+) create mode 100644 test/Segmented/YouBotSegmentedCollisionGripping.jl diff --git a/test/Segmented/YouBotSegmentedCollisionGripping.jl b/test/Segmented/YouBotSegmentedCollisionGripping.jl new file mode 100644 index 0000000..836e500 --- /dev/null +++ b/test/Segmented/YouBotSegmentedCollisionGripping.jl @@ -0,0 +1,592 @@ +module YouBotSegmentedCollisionGripping2 + +using Modia3D + +include("$(Modia3D.modelsPath)/Blocks.jl") +include("$(Modia3D.modelsPath)/Electric.jl") +include("$(Modia3D.modelsPath)/Rotational.jl") +include("$(Modia3D.modelsPath)/Translational.jl") + + +# some constants +# use boxes instead of meshes for finger contact + +# visual materials +vmatYellow = VisualMaterial(color=[255,215,0]) # ground +vmatBlue = VisualMaterial(color="DodgerBlue3") # table +vmatGrey = VisualMaterial(color="DarkGrey") # work piece +vmatInvisible = VisualMaterial(transparency=1.0) # contact boxes +tableX = 0.3 +tableY = 0.3 +tableZ = 0.01 +heigthLeg = 0.365 +widthLeg = 0.02 +diameter = 0.05 +diameterLock = 0.03 +rightFingerOffset = 0.03 + +# all needed paths to fileMeshes +base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/base_frame.obj") +plate_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/plate.obj") +front_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-right_wheel.obj") +front_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-left_wheel.obj") +back_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-right_wheel.obj") +back_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-left_wheel.obj") +arm_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_base_frame.obj") +arm_joint_1_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_1.obj") +arm_joint_2_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_2.obj") +arm_joint_3_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_3.obj") +arm_joint_4_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_4.obj") +arm_joint_5_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_5.obj") +gripper_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_base_frame.obj") +gripper_left_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_left_finger.obj") +gripper_right_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_right_finger.obj") + +# Drive train data: motor inertias and gear ratios +nullRot = [0, 0, 0] + +motorInertia1 = 0.0000135 + 0.000000409 +motorInertia2 = 0.0000135 + 0.000000409 +motorInertia3 = 0.0000135 + 0.000000071 +motorInertia4 = 0.00000925 + 0.000000071 +motorInertia5 = 0.0000035 + 0.000000071 +gearRatio1 = 156.0 +gearRatio2 = 156.0 +gearRatio3 = 100.0 +gearRatio4 = 71.0 +gearRatio5 = 71.0 + +m1=1.390 +translation1 =[0.033,0,0] +rotation1 = [180u"°", 0, 0] + +m2=1.318 +translation2=[0.155,0,0] +rotation2 = [90u"°", 0.0, -90u"°"] + +m3=0.821 +translation3=[0,0.135,0] +rotation3 = [0, 0, -90u"°"] + +m4=0.769 +translation4 = [0,0.11316,0] + +m5=0.687 +translation5=[0,0,0.05716] +rotation5 = [-90u"°", 0, 0] + +### ----------------- Servo Model ----------------------- +# parameters for Link +axisLink = 3 +dLink = 1.0 +k1Link = 50.0 +k2Link = 0.1 +T2Link = 1.0 +# parameters for MyGripper +axisGripper = 2 +k1Gripper = 50.0 +k2Gripper = 0.1*100 +T2Gripper = 1.0 +motorInertiaGripper = 0.1 +gearRatioGripper = 1.0 + +initPosition = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +function robotProgram(robotActions) + addReferencePath(robotActions, + names = ["bot1angle1", "bot1angle2", "bot1angle3", "bot1angle4", "bot1angle5", "bot1gripper"], + position = initPosition, + v_max = [2.68512, 2.68512, 4.8879, 5.8997, 5.8997, 2.0], + a_max = [1.5, 1.5, 1.5, 1.5, 1.5, 0.5] + ) + ptpJointSpace(robotActions, [ + 0.0 0.0 0.0 0.0 0.0 0.0; + ]) + # Attach sphere to plate + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0) + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 0.0 0.0 diameter+0.01; # start top of ball + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # go to plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0) + + # gripping via collision handling + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + ]) + + # Attach sphere to gripper + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", waitingPeriod=0.0) + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0) + + # free bouncing sphere + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter+0.01; # grip + top + ]) + EventAfterPeriod(robotActions, 0.6) + + # Attach sphere to plate + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0) + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 0.0 0.0 diameter+0.01; # grip + plate + pi pi/4 pi/4 1.1 0.0 diameter+0.01; # grip + plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0) + + # gripping via collision handling + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.1 0.0 diameter-0.002; # grip + plate + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + # pi pi/4 pi/4 1.04 0.0 diameter+0.01; # release + plate + # pi 0.0 pi/2 0.0 0.0 diameter+0.01; # open + top + # pi pi/4 pi/4 1.04 0.0 diameter+0.01; # open + plate + # pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + # pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + ]) + + + return nothing +end + +# Controller Model +Controller = Model( + # Interface + refLoadAngle = input, + actualMotorAngle = input, + actualMotorSpeed = input, + refTorque = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadAngle, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorAngle, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refTorque)] +) + +Drive = Model( + # Interface + refTorque = input, + actualSpeed = output, + actualAngle = output, + flange = Flange, + # Components + torque = UnitlessTorque, + speedSensor = UnitlessSpeedSensor, + angleSensor = UnitlessAngleSensor, + motorInertia = Inertia, + idealGear = IdealGear, + + connect = :[ + (refTorque, torque.tau) + (torque.flange, speedSensor.flange, angleSensor.flange, motorInertia.flange_a) + (motorInertia.flange_b, idealGear.flange_a) + (idealGear.flange_b, flange) + (speedSensor.w, actualSpeed) + (angleSensor.phi, actualAngle) + ] +) + +Servo = Model( + refLoadAngle = input, + flange = Flange, + ppi = Controller, + drive = Drive, + + connect = :[ + # inputs of ppi + (refLoadAngle, ppi.refLoadAngle) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorAngle, drive.actualAngle) + # output of ppi --> input of motor + (ppi.refTorque, drive.refTorque) + # Flange of drive + (drive.flange, flange) + ] +) + +# Controller Model +ControllerTrans = Model( + # Interface + refLoadPos = input, + actualMotorPos = input, + actualMotorSpeed = input, + refForce = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadPos, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorPos, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refForce)] +) + +DriveTrans = Model( + # Interface + refForce = input, + actualSpeed = output, + actualPos = output, + flange = TranslationalFlange, + # Components + force = UnitlessForce, + speedSensor = UnitlessVelocitySensor, + posSensor = UnitlessPositionSensor, + motorMass = TranslationalMass, + + connect = :[ + (refForce, force.f) + (force.flange, speedSensor.flange, posSensor.flange, motorMass.flange_a) + (motorMass.flange_b, flange) + (speedSensor.v, actualSpeed) + (posSensor.s, actualPos) + ] +) + +ServoTrans = Model( + refLoadPos = input, + flange = TranslationalFlange, + ppi = ControllerTrans, + drive = DriveTrans, + + connect = :[ + # inputs of ppi + (refLoadPos, ppi.refLoadPos) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorPos, drive.actualPos) + # output of ppi --> input of motor + (ppi.refForce, drive.refForce) + # Flange of drive + (drive.flange, flange) + ] +) + +Ground = Model( + ground = Object3D(parent=:world, + translation=[0.0, 0.0, -0.005], feature = Solid(shape =Box(lengthX=2.5, lengthY=2.5, lengthZ=0.01), solidMaterial="DryWood", visualMaterial=vmatYellow)) +) + + +Base = Model( + position = [0.0, 0.0, 0.0], + orientation = [0.0, 0.0, 0.0], + base_frame = Object3D(parent=:world, translation=:position, rotation=:orientation, + feature=Solid(shape=FileMesh(filename = base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=19.803))), + plate = Object3D(parent=:base_frame, translation=[-0.159, 0, 0.046], + feature=Solid(shape=FileMesh(filename = plate_obj), massProperties=MassPropertiesFromShapeAndMass(mass=2.397), collision=false)), + plateLock = Object3D(parent=:base_frame, + lockable=true, + translation=[-0.19,0.0,0.0702], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + plate_box = Object3D(parent=:base_frame, translation=[-0.16, 0, 0.0702],# 0.06514], + feature=Solid(shape=Box(lengthX=0.22, lengthY=0.22, lengthZ=0.01), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, + solidMaterial="DryWood", collision=true)), + front_right_wheel = Object3D(parent=:base_frame, translation=[0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + front_left_wheel = Object3D(parent=:base_frame, translation=[0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_right_wheel = Object3D(parent=:base_frame, translation=[-0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_left_wheel = Object3D(parent=:base_frame, translation=[-0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))) +) + +servoParameters1 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio1), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia1), + idealGear = Map(ratio=gearRatio1) + ) + ) + +servoParameters2 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio2), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia2), + idealGear = Map(ratio=gearRatio2) + ) + ) + +servoParameters3 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio3), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia3), + idealGear = Map(ratio=gearRatio3) + ) + ) + +servoParameters4 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio4), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia4), + idealGear = Map(ratio=gearRatio4) + ) + ) + +servoParameters5 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio5), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia5), + idealGear = Map(ratio=gearRatio5) + ) + ) + +servoParameters6 = Map( + ppi = Map( + gainOuter = Map(k=gearRatioGripper), + gainInner = Map(k=k1Gripper), + PI = Map(k=k2Gripper, T=T2Gripper) + ), + drive = Map( + motorMass = Map(m=motorInertiaGripper) + ) + ) + +featureBody1 = Solid(shape = FileMesh(filename = arm_joint_1_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m1)) +featureBody2 = Solid(shape = FileMesh(filename = arm_joint_2_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m2)) +featureBody3 = Solid(shape = FileMesh(filename = arm_joint_3_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m3)) +featureBody4 = Solid(shape = FileMesh(filename = arm_joint_4_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m4)) +featureBody5 = Solid(shape = FileMesh(filename = arm_joint_5_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m5)) + +linkParameters1 = Map( + parent1 = Par(value = :(armBase_b)), + featureBody = featureBody1, + initRefPos = initPosition[1], + trans = translation1, + rota = Par(value = :(rotation1)) +) + +linkParameters2 = Map( + parent1 = Par(value = :(link1.obj2)), + featureBody = featureBody2, + m = m2, + initRefPos = initPosition[2], + trans = translation2, + rota = Par(value = :(rotation2)) +) + +linkParameters3 = Map( + parent1 = Par(value = :(link2.obj2)), + featureBody = featureBody3, + m = m3, + initRefPos = initPosition[3], + trans = translation3, + rota = Par(value = :(rotation3)) +) + +linkParameters4 = Map( + parent1 = Par(value = :(link3.obj2)), + featureBody = featureBody4, + m = m4, + initRefPos = initPosition[4], + trans = translation4, + rota = Par(value = :(nullRot)) +) + +linkParameters5 = Map( + parent1 = Par(value = :(link4.obj2)), + featureBody = featureBody5, + m = m5, + initRefPos = initPosition[5], + trans = translation5, + rota = Par(value = :(rotation5)) +) + +Link = Model( + parent1 = Par(value = :(nothing)), + featureBody = featureBody1, + trans = [0,0,0], + rota = Par(value = :(nullRot)), + + obj1 = Object3D(parent=:parent1, rotation=:rota), + body = Object3D(feature = :featureBody), + obj2 = Object3D(parent =:body, translation = :trans), +) + +MyGripper = Model( + obj1 = Object3D(parent=:(link5.obj2), rotation=[0, 0, -180u"°"]), + gripper_base_frame = Object3D(parent=:obj1, feature=Solid(shape= + FileMesh(filename = gripper_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.199))), + gripper_left_finger_a = Object3D(), + gripper_left_finger = Object3D(parent=:gripper_left_finger_a, translation=[0, 0.0082, 0], + feature=Solid(shape= FileMesh(filename = gripper_left_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), collision=false)), + gripper_left_finger_box = Object3D(parent=:gripper_left_finger_a, translation=[0.0, 0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripper_right_finger_a = Object3D(parent=:gripper_base_frame, + translation=[0,-rightFingerOffset,0]), + gripper_right_finger = Object3D(parent=:gripper_right_finger_a, translation=[0, -0.0082, 0], + feature = Solid(shape = FileMesh(filename = gripper_right_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), collision=false)), + gripper_right_finger_box = Object3D(parent=:gripper_right_finger_a, translation=[0.0, -0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripperLock = Object3D(parent=:gripper_right_finger_a, + lockable=true, + translation=[0.012/2,0.01/2,0.045/2], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), +) + +YouBot(worldName;pathIndexOffset) = Model( + pathIndexOffset2 = pathIndexOffset, + base = Base, + worldName = Par(worldName), + arm_base_frame = Object3D(parent=:(base.base_frame), + translation=[0.143, 0.0, 0.046], + feature = Solid(shape = FileMesh(filename = arm_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.961))), + armBase_b = Object3D(parent=:arm_base_frame, translation=[0.024, 0, 0.115]), + + link1 = Link | linkParameters1, + link2 = Link | linkParameters2, + link3 = Link | linkParameters3, + link4 = Link | linkParameters4, + link5 = Link | linkParameters5, + gripper = MyGripper, + + rev1 = RevoluteWithFlange(obj1 = :(link1.obj1), obj2 = :(link1.body), + axis=axisLink, phi = Var(init = initPosition[1+pathIndexOffset])), + rev2 = RevoluteWithFlange(obj1 = :(link2.obj1), obj2 = :(link2.body), + axis=axisLink, phi = Var(init = initPosition[2+pathIndexOffset])), + rev3 = RevoluteWithFlange(obj1 = :(link3.obj1), obj2 = :(link3.body), + axis=axisLink, phi = Var(init = initPosition[3+pathIndexOffset])), + rev4 = RevoluteWithFlange(obj1 = :(link4.obj1), obj2 = :(link4.body), + axis=axisLink, phi = Var(init = initPosition[4+pathIndexOffset])), + rev5 = RevoluteWithFlange(obj1 = :(link5.obj1), obj2 = :(link5.body), + axis=axisLink, phi = Var(init = initPosition[5+pathIndexOffset])), + + prism = PrismaticWithFlange(obj1 = :(gripper.gripper_right_finger_a), obj2 = :(gripper.gripper_left_finger_a), + axis=axisGripper, s = Var(init = initPosition[6+pathIndexOffset]) ), + + servo1 = Servo, + servo2 = Servo, + servo3 = Servo, + servo4 = Servo, + servo5 = Servo, + servo6 = ServoTrans, + + modelActions = ModelActions(world=:world, actions=robotProgram), + currentAction = Var(hideResult=true), + + equations=:[ + currentAction = executeActions(modelActions), + servo1.refLoadAngle = getRefPathPosition(currentAction, 1+pathIndexOffset2), + servo2.refLoadAngle = getRefPathPosition(currentAction, 2+pathIndexOffset2), + servo3.refLoadAngle = getRefPathPosition(currentAction, 3+pathIndexOffset2), + servo4.refLoadAngle = getRefPathPosition(currentAction, 4+pathIndexOffset2), + servo5.refLoadAngle = getRefPathPosition(currentAction, 5+pathIndexOffset2), + servo6.refLoadPos = getRefPathPosition(currentAction, 6+pathIndexOffset2) + ], + + connect = :[ + (servo1.flange, rev1.flange) + (servo2.flange, rev2.flange) + (servo3.flange, rev3.flange) + (servo4.flange, rev4.flange) + (servo5.flange, rev5.flange) + (servo6.flange, prism.flange) + ] +) + +Scenario = Model3D( + gravField = UniformGravityField(g=9.81, n=[0,0,-1]), + world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, + enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), #, animationFile="YouBotDynamicState.json")), +# worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.2))), + + + ground = Ground, + + sphere = Object3D(parent=:world, fixedToParent=false, + assemblyRoot=true, + translation=[-0.78, 0.0, 0.1845], #[-0.78, 0.0, 0.1792], + feature=Solid(shape=Sphere(diameter=diameter), + visualMaterial=vmatGrey, + solidMaterial="DryWood", + collision=true)), + sphereLock = Object3D(parent=:sphere, + lockable=true, + translation=[0.0,diameter/2,0.0], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + youbot1 = YouBot("world", pathIndexOffset=0) +) + +modelParameters = Map( + youbot1 = Map( + base = Map( + orientation = [0.0, 0.0, 0.0], + position = [-0.569, 0.0, 0.084], + ), + servo1 = servoParameters1, + servo2 = servoParameters2, + servo3 = servoParameters3, + servo4 = servoParameters4, + servo5 = servoParameters5, + servo6 = servoParameters6 + ) +) + + +youbotModel = Scenario | modelParameters + +youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) + +stopTime = 13.1 +tolerance = 1e-7 +# use boxes instead of FileMesh for better collision performance +requiredFinalStates = [3.1415926313235216, 2.227704607451224e-8, -2.8441240591542057e-7, 2.844479623947795e-7, -6.300618493694368e-7, 6.301438745162919e-7, -3.8367060940761664e-7, 3.8373222528068616e-7, 2.9773366458646674e-9, -2.9778176373587e-9, -0.00017011927750574103, 0.2394616012878047, -0.003881746258204947, -0.002274156117355221, 1.0356446209926906e-5, -8.985047184676908e-11, 0.047999999998151396, 1.8748871907289576e-12, -0.7616251395986522, 0.00025213516122149976, 0.18419327722973222, 1.2698578611388089e-9, 2.2444158136065587e-12, -1.772988910724902e-11, 3.141474457755385, 1.0187988505971024, 3.141041134037263, -8.150376270957884e-11, 4.6108853365504784e-8, 8.826370248608988e-22] + +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) + +# showInfo(youbot) + +@usingModiaPlot +plot(youbot, ["sphere.translation", + ("youbot1.rev1.phi", + "youbot1.rev2.phi", + "youbot1.rev3.phi", + "youbot1.rev4.phi", + "youbot1.rev5.phi")], figure=1) + +plot(youbot, [ "sphere.r_abs[1]", + "sphere.r_abs[2]", + "sphere.r_abs[3]"], figure=2) + +end From 5d0ff3eb493a4011713fb12d087db9e383da1f06 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Tue, 6 Jun 2023 14:43:11 +0200 Subject: [PATCH 04/15] little changes --- src/Composition/dynamics.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index 8d9faad..f38f7dd 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -547,7 +547,7 @@ function computeGeneralizedForces!(mbs::MultibodyData{F,TimeType}, qdd_hidden::V # objects can have interactionManner (need to rename updateVisuElements) if scene.options.useOptimizedStructure objIndices = mbs.objIndices - for (i,obj) in enumerate(scene.updateVisuElements) + for (i,obj) in enumerate(scene.allVisuElements) parent = obj.parent obj.r_abs = obj.r_rel ≡ Modia3D.ZeroVector3D(F) ? parent.r_abs : parent.r_abs + parent.R_abs'*obj.r_rel obj.R_abs = obj.R_rel ≡ Modia3D.NullRotation(F) ? parent.R_abs : obj.R_rel*parent.R_abs From bbe705084439ecaeb81f77ed4c32e697eb27f8b3 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Tue, 6 Jun 2023 17:13:12 +0200 Subject: [PATCH 05/15] enable and dissable collision handling --- src/Composition/dynamics.jl | 14 ++++++++ src/GrippingDetection/grippingPair.jl | 1 + src/PathPlanning/referencePathInternal.jl | 40 ++++++++++++++++------- src/PathPlanning/referencePathUser.jl | 22 ++++++------- 4 files changed, 55 insertions(+), 22 deletions(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index f38f7dd..db65caa 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -149,6 +149,9 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode if isdefined(scene, :gripPair) if Modia3D.checkGrippingFeatures(scene, scene.gripPair) Modia3D.changeParentOfMovableUnit!(scene, world, scene.gripPair) + if isdefined(scene.gripPair, :enableContactDetection) + scene.collide = scene.gripPair.enableContactDetection + end else @error("Andrea: print warning für gripping features") # printWarnGrip(robotOrDepot, movableObj, waitingPeriod) @@ -168,6 +171,17 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode # rebuild MKS if scene.options.useOptimizedStructure rebuild_superObjs!(scene, world) + + if isdefined(scene, :gripPair) + if Modia3D.checkGrippingFeatures(scene, scene.gripPair) + if isdefined(scene.gripPair, :enableContactDetection) + if scene.options.enableContactDetection && scene.collide + scene.collide = scene.gripPair.enableContactDetection + end + end + end + end + else error("Full restart is possible only if useOptimizedStructure is enabled in SceneOptions.") end diff --git a/src/GrippingDetection/grippingPair.jl b/src/GrippingDetection/grippingPair.jl index 5564e54..1af4175 100644 --- a/src/GrippingDetection/grippingPair.jl +++ b/src/GrippingDetection/grippingPair.jl @@ -5,6 +5,7 @@ mutable struct GrippingPair{F <: Modia3D.VarFloatType} movableObj::Composition.Object3D{F} # must be part of a movable unit # is set for Attach and ReleaseAndAttach robotOrDepot::Composition.Object3D{F} # must be part of a gripper unit or part of a depot (like bottom or other movable unit) + enableContactDetection::Bool GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType} = new(gripStatus, movableObj) GrippingPair{F}(gripStatus::GripStatus, movableObj::Composition.Object3D{F}, robotOrDepot::Composition.Object3D{F}) where {F <: Modia3D.VarFloatType} = new(gripStatus, movableObj, robotOrDepot) end diff --git a/src/PathPlanning/referencePathInternal.jl b/src/PathPlanning/referencePathInternal.jl index e597439..fed2ef8 100644 --- a/src/PathPlanning/referencePathInternal.jl +++ b/src/PathPlanning/referencePathInternal.jl @@ -4,6 +4,7 @@ struct ArbitraryMotion inputArg1 # e.g. positions, waitingPeriod, robotOrDepot inputArg2 # e.g. nothing, movablePart inputArg3 # e.g. nothing, waitingPeriod + inputArg4 # e.g. nothing, waitingPeriod end @@ -86,7 +87,7 @@ end #-------------- Functions that can be executed at an event instant ------- ### ------------------ ptpJointSpace! ------------------------------------- -function ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Matrix{Float64}, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Matrix{Float64}, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} pos1 = size(transpose(ref.referencePath.position)) pos2 = size(positions) if pos1[2] != pos2[2] @@ -101,61 +102,78 @@ function ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Matrix{Float64 return nothing end -ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Vector{Float64}, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} = - ptpJointSpace!(ref, reshape(positions, :, 1), dummy1, dummy2) +ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Vector{Float64}, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} = + ptpJointSpace!(ref, reshape(positions, :, 1), dummy1, dummy2, dummy3) -ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Float64, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} = - ptpJointSpace!(ref, reshape([positions], :, 1), dummy1, dummy2) +ptpJointSpace!(ref::ModelActions{F,TimeType}, positions::Float64, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} = + ptpJointSpace!(ref, reshape([positions], :, 1), dummy1, dummy2, dummy3) ###------------------------- ActionWait! ---------------------------------- -function ActionWait!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionWait!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} @assert(waitingPeriod >= 0.0) setAttributesReferencePath!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod) return nothing end -function robotEventAfterPeriod!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function robotEventAfterPeriod!(ref::ModelActions{F,TimeType}, waitingPeriod::Float64, dummy1, dummy2, dummy3) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} @assert(waitingPeriod >= 0.0) setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod) return nothing end ###--------------------- ActionReleaseAndAttach! ------------------------- -function ActionReleaseAndAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionReleaseAndAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool, Nothing}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} @assert(waitingPeriod >= 0.0) # waitingPeriod must be positive ref.scene.gripPair = Modia3D.GrippingPair{F}( Modia3D.ReleaseAndSetDown, getComponent(ref, movableObj), getComponent(ref, robotOrDepot)) + if !isnothing(enableContactDetection) + ref.scene.gripPair.enableContactDetection = enableContactDetection + end + setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod) return nothing end ###------------------------- ActionAttach! ---------------------------------- -function ActionAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionAttach!(ref::ModelActions{F,TimeType}, robotOrDepot::String, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} @assert(waitingPeriod >= 0.0) # waitingPeriod must be bigger than 0.1 sec ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Grip, getComponent(ref, movableObj), getComponent(ref, robotOrDepot)) + + if !isnothing(enableContactDetection) + ref.scene.gripPair.enableContactDetection = enableContactDetection + end + setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod) return nothing end ###------------------------- ActionRelease! ------------------------------- -function ActionRelease!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionRelease!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} @assert(waitingPeriod >= 0.0) # waitingPeriod must be positive ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Release, getComponent(ref, movableObj)) + if !isnothing(enableContactDetection) + ref.scene.gripPair.enableContactDetection = enableContactDetection + end + setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod) return nothing end ###------------------------- ActionDelete! ------------------------------- -function ActionDelete!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionDelete!(ref::ModelActions{F,TimeType}, movableObj::String, waitingPeriod::Float64, enableContactDetection::Union{Bool,Nothing}, nothing) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} @assert(waitingPeriod >= 0.0) # waitingPeriod must be positive ref.scene.gripPair = Modia3D.GrippingPair{F}(Modia3D.Delete, getComponent(ref, movableObj)) + if !isnothing(enableContactDetection) + ref.scene.gripPair.enableContactDetection = enableContactDetection + end + setAttributesReferencePathRestart!(ref, Modia.getTime(ref.instantiatedModel) + waitingPeriod) return nothing end diff --git a/src/PathPlanning/referencePathUser.jl b/src/PathPlanning/referencePathUser.jl index 2518677..6d3268d 100644 --- a/src/PathPlanning/referencePathUser.jl +++ b/src/PathPlanning/referencePathUser.jl @@ -3,7 +3,7 @@ function ptpJointSpace(ref::ModelActions{F,TimeType}, positions::Union{Matrix{Float64}, Vector{Float64}, Float64}, iargs...; kwargs...) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorPtpJointSpace(iargs, kwargs) - push!(ref.refMotion, ArbitraryMotion(ptpJointSpace!, positions, nothing, nothing)) + push!(ref.refMotion, ArbitraryMotion(ptpJointSpace!, positions, nothing, nothing, nothing)) return nothing end ptpJointSpace(;referencePath, positions) = @@ -39,7 +39,7 @@ function executeActions(modelActions::ModelActions{F,TimeType}) where {F <: Modi # Execute next command in referenceMotion if !isempty(modelActions.refMotion) item = popfirst!(modelActions.refMotion) - item.func(modelActions, item.inputArg1, item.inputArg2, item.inputArg3) + item.func(modelActions, item.inputArg1, item.inputArg2, item.inputArg3, item.inputArg4) end; end; end # only if a ptp path is defined getPosition! can be executed @@ -57,7 +57,7 @@ end ### ------------------------- ActionWait ------------------------------ function ActionWait(ref::ModelActions{F,TimeType}, waitingPeriod::Float64=0.0, iargs...; kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorExplicitRobotFunc("ActionWait", "waitingPeriod", iargs, kwargs, refPathExists=true) - push!(ref.refMotion, ArbitraryMotion(ActionWait!, waitingPeriod, nothing, nothing)) + push!(ref.refMotion, ArbitraryMotion(ActionWait!, waitingPeriod, nothing, nothing, nothing)) return nothing end ActionWait(iargs...; kwargs...)::Nothing = @@ -65,9 +65,9 @@ ActionWait(iargs...; kwargs...)::Nothing = ### --------------------- ActionReleaseAndAttach ------------------------- -function ActionReleaseAndAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionReleaseAndAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, enableContactDetection::Bool=true, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorExplicitRobotFunc("ActionReleaseAndAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true) - push!(ref.refMotion, ArbitraryMotion(ActionReleaseAndAttach!, robotOrDepot, movablePart, waitingPeriod)) + push!(ref.refMotion, ArbitraryMotion(ActionReleaseAndAttach!, robotOrDepot, movablePart, waitingPeriod, enableContactDetection)) return nothing end ActionReleaseAndAttach(iargs...; kwargs...)::Nothing = @@ -75,7 +75,7 @@ ActionReleaseAndAttach(iargs...; kwargs...)::Nothing = function EventAfterPeriod(ref::ModelActions{F,TimeType}, waitingPeriod::Float64=0.0, iargs...; kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorExplicitRobotFunc("EventAfterPeriod", "waitingPeriod", iargs, kwargs, refPathExists=true) - push!(ref.refMotion, ArbitraryMotion(robotEventAfterPeriod!, waitingPeriod, nothing, nothing)) + push!(ref.refMotion, ArbitraryMotion(robotEventAfterPeriod!, waitingPeriod, nothing, nothing, nothing)) return nothing end EventAfterPeriod(iargs...; kwargs...)::Nothing = @@ -83,9 +83,9 @@ EventAfterPeriod(iargs...; kwargs...)::Nothing = ### --------------------- ActionAttach ------------------------------------ -function ActionAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionAttach(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, enableContactDetection::Bool=true, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorExplicitRobotFunc("ActionAttach", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true) - push!(ref.refMotion, ArbitraryMotion(ActionAttach!, robotOrDepot, movablePart, waitingPeriod)) + push!(ref.refMotion, ArbitraryMotion(ActionAttach!, robotOrDepot, movablePart, waitingPeriod, enableContactDetection)) return nothing end ActionAttach(iargs...; kwargs...)::Nothing = @@ -93,9 +93,9 @@ ActionAttach(iargs...; kwargs...)::Nothing = ### --------------------- ActionRelease ----------------------------------- -function ActionRelease(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} +function ActionRelease(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, enableContactDetection::Bool=true, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorExplicitRobotFunc("ActionRelease", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true) - push!(ref.refMotion, ArbitraryMotion(ActionRelease!, movablePart, waitingPeriod, nothing)) + push!(ref.refMotion, ArbitraryMotion(ActionRelease!, movablePart, waitingPeriod, enableContactDetection, nothing)) return nothing end ActionRelease(iargs...; kwargs...)::Nothing = @@ -106,7 +106,7 @@ ActionRelease(iargs...; kwargs...)::Nothing = ### --------------------- ActionRelease ----------------------------------- function ActionDelete(ref::ModelActions{F,TimeType}, movablePart::String="", robotOrDepot::String="", iargs...; waitingPeriod::Float64=0.0, kwargs...)::Nothing where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} checkErrorExplicitRobotFunc("ActionDelete", "robotOrDepot, movablePart, and waitingPeriod", iargs, kwargs, refPathExists=true) - push!(ref.refMotion, ArbitraryMotion(ActionDelete!, movablePart, waitingPeriod, nothing)) + push!(ref.refMotion, ArbitraryMotion(ActionDelete!, movablePart, waitingPeriod, nothing, nothing)) return nothing end ActionDelete(iargs...; kwargs...)::Nothing = From 261226ab45ec81ed739161bad26e3b1373458428 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Tue, 6 Jun 2023 17:28:07 +0200 Subject: [PATCH 06/15] some test models --- test/Robot/ScenarioCollisionOnly.jl | 562 +++++++++++++++++ .../ScenarioSegmentedCollisionOff.jl | 590 ++++++++++++++++++ .../Segmented/ScenarioSegmentedCollisionOn.jl | 590 ++++++++++++++++++ test/Segmented/ScenarioSegmentedOnly.jl | 586 +++++++++++++++++ 4 files changed, 2328 insertions(+) create mode 100644 test/Robot/ScenarioCollisionOnly.jl create mode 100644 test/Segmented/ScenarioSegmentedCollisionOff.jl create mode 100644 test/Segmented/ScenarioSegmentedCollisionOn.jl create mode 100644 test/Segmented/ScenarioSegmentedOnly.jl diff --git a/test/Robot/ScenarioCollisionOnly.jl b/test/Robot/ScenarioCollisionOnly.jl new file mode 100644 index 0000000..bf52cc1 --- /dev/null +++ b/test/Robot/ScenarioCollisionOnly.jl @@ -0,0 +1,562 @@ +module ScenarioCollisionOnly + +using Modia3D + +include("$(Modia3D.modelsPath)/Blocks.jl") +include("$(Modia3D.modelsPath)/Electric.jl") +include("$(Modia3D.modelsPath)/Rotational.jl") +include("$(Modia3D.modelsPath)/Translational.jl") + + +# some constants +# use boxes instead of meshes for finger contact + +# visual materials +vmatYellow = VisualMaterial(color=[255,215,0]) # ground +vmatBlue = VisualMaterial(color="DodgerBlue3") # table +vmatGrey = VisualMaterial(color="DarkGrey") # work piece +vmatInvisible = VisualMaterial(transparency=1.0) # contact boxes +tableX = 0.3 +tableY = 0.3 +tableZ = 0.01 +heigthLeg = 0.365 +widthLeg = 0.02 +diameter = 0.05 +diameterLock = 0.03 +rightFingerOffset = 0.03 + +# all needed paths to fileMeshes +base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/base_frame.obj") +plate_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/plate.obj") +front_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-right_wheel.obj") +front_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-left_wheel.obj") +back_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-right_wheel.obj") +back_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-left_wheel.obj") +arm_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_base_frame.obj") +arm_joint_1_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_1.obj") +arm_joint_2_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_2.obj") +arm_joint_3_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_3.obj") +arm_joint_4_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_4.obj") +arm_joint_5_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_5.obj") +gripper_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_base_frame.obj") +gripper_left_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_left_finger.obj") +gripper_right_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_right_finger.obj") + +# Drive train data: motor inertias and gear ratios +nullRot = [0, 0, 0] + +motorInertia1 = 0.0000135 + 0.000000409 +motorInertia2 = 0.0000135 + 0.000000409 +motorInertia3 = 0.0000135 + 0.000000071 +motorInertia4 = 0.00000925 + 0.000000071 +motorInertia5 = 0.0000035 + 0.000000071 +gearRatio1 = 156.0 +gearRatio2 = 156.0 +gearRatio3 = 100.0 +gearRatio4 = 71.0 +gearRatio5 = 71.0 + +m1=1.390 +translation1 =[0.033,0,0] +rotation1 = [180u"°", 0, 0] + +m2=1.318 +translation2=[0.155,0,0] +rotation2 = [90u"°", 0.0, -90u"°"] + +m3=0.821 +translation3=[0,0.135,0] +rotation3 = [0, 0, -90u"°"] + +m4=0.769 +translation4 = [0,0.11316,0] + +m5=0.687 +translation5=[0,0,0.05716] +rotation5 = [-90u"°", 0, 0] + +### ----------------- Servo Model ----------------------- +# parameters for Link +axisLink = 3 +dLink = 1.0 +k1Link = 50.0 +k2Link = 0.1 +T2Link = 1.0 +# parameters for MyGripper +axisGripper = 2 +k1Gripper = 50.0 +k2Gripper = 0.1*100 +T2Gripper = 1.0 +motorInertiaGripper = 0.1 +gearRatioGripper = 1.0 + +initPosition = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +function robotProgram(robotActions) + addReferencePath(robotActions, + names = ["bot1angle1", "bot1angle2", "bot1angle3", "bot1angle4", "bot1angle5", "bot1gripper"], + position = initPosition, + v_max = [2.68512, 2.68512, 4.8879, 5.8997, 5.8997, 2.0], + a_max = [1.5, 1.5, 1.5, 1.5, 1.5, 0.5]) + + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 0.0 0.0 diameter+0.01; # start top of ball + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # go to plate + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter-0.002; # release + plate + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter+0.01; # release + plate + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # release + plate + pi 0.0 pi/2 0.0 0.0 diameter+0.01; # open + top + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # open + plate + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + ]) + return nothing +end + +# Controller Model +Controller = Model( + # Interface + refLoadAngle = input, + actualMotorAngle = input, + actualMotorSpeed = input, + refTorque = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadAngle, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorAngle, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refTorque)] +) + +Drive = Model( + # Interface + refTorque = input, + actualSpeed = output, + actualAngle = output, + flange = Flange, + # Components + torque = UnitlessTorque, + speedSensor = UnitlessSpeedSensor, + angleSensor = UnitlessAngleSensor, + motorInertia = Inertia, + idealGear = IdealGear, + + connect = :[ + (refTorque, torque.tau) + (torque.flange, speedSensor.flange, angleSensor.flange, motorInertia.flange_a) + (motorInertia.flange_b, idealGear.flange_a) + (idealGear.flange_b, flange) + (speedSensor.w, actualSpeed) + (angleSensor.phi, actualAngle) + ] +) + +Servo = Model( + refLoadAngle = input, + flange = Flange, + ppi = Controller, + drive = Drive, + + connect = :[ + # inputs of ppi + (refLoadAngle, ppi.refLoadAngle) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorAngle, drive.actualAngle) + # output of ppi --> input of motor + (ppi.refTorque, drive.refTorque) + # Flange of drive + (drive.flange, flange) + ] +) + +# Controller Model +ControllerTrans = Model( + # Interface + refLoadPos = input, + actualMotorPos = input, + actualMotorSpeed = input, + refForce = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadPos, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorPos, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refForce)] +) + +DriveTrans = Model( + # Interface + refForce = input, + actualSpeed = output, + actualPos = output, + flange = TranslationalFlange, + # Components + force = UnitlessForce, + speedSensor = UnitlessVelocitySensor, + posSensor = UnitlessPositionSensor, + motorMass = TranslationalMass, + + connect = :[ + (refForce, force.f) + (force.flange, speedSensor.flange, posSensor.flange, motorMass.flange_a) + (motorMass.flange_b, flange) + (speedSensor.v, actualSpeed) + (posSensor.s, actualPos) + ] +) + +ServoTrans = Model( + refLoadPos = input, + flange = TranslationalFlange, + ppi = ControllerTrans, + drive = DriveTrans, + + connect = :[ + # inputs of ppi + (refLoadPos, ppi.refLoadPos) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorPos, drive.actualPos) + # output of ppi --> input of motor + (ppi.refForce, drive.refForce) + # Flange of drive + (drive.flange, flange) + ] +) + +Ground = Model( + ground = Object3D(parent=:world, + translation=[0.0, 0.0, -0.005], feature = Solid(shape =Box(lengthX=2.5, lengthY=2.5, lengthZ=0.01), solidMaterial="DryWood", visualMaterial=vmatYellow)) +) + +Table = Model( + plate = Object3D(parent=:world, translation=[0.0, 0.0, heigthLeg], + feature=Solid(shape=Box(lengthX=tableX, lengthY=tableY, lengthZ=tableZ), + collisionSmoothingRadius=0.001, solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg1 = Object3D(parent=:plate, translation=[ tableX/2-widthLeg/2, tableY/2-widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), + solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg2 = Object3D(parent=:plate, translation=[-tableX/2+widthLeg/2, tableY/2-widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg3 = Object3D(parent=:plate, translation=[ tableX/2-widthLeg/2, -tableY/2+widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg4 = Object3D(parent=:plate, translation=[-tableX/2+widthLeg/2, -tableY/2+widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), solidMaterial="DryWood", visualMaterial=vmatBlue)) +) + +Base = Model( + position = [0.0, 0.0, 0.0], + orientation = [0.0, 0.0, 0.0], + base_frame = Object3D(parent=:world, translation=:position, rotation=:orientation, + feature=Solid(shape=FileMesh(filename = base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=19.803))), + plate = Object3D(parent=:base_frame, translation=[-0.159, 0, 0.046], + feature=Solid(shape=FileMesh(filename = plate_obj), massProperties=MassPropertiesFromShapeAndMass(mass=2.397), solidMaterial="DryWood")), + plate_box = Object3D(parent=:base_frame, translation=[-0.16, 0, 0.0702],# 0.06514], + feature=Solid(shape=Box(lengthX=0.22, lengthY=0.22, lengthZ=0.01), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + front_right_wheel = Object3D(parent=:base_frame, translation=[0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + front_left_wheel = Object3D(parent=:base_frame, translation=[0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_right_wheel = Object3D(parent=:base_frame, translation=[-0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_left_wheel = Object3D(parent=:base_frame, translation=[-0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))) +) + +servoParameters1 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio1), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia1), + idealGear = Map(ratio=gearRatio1) + ) + ) + +servoParameters2 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio2), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia2), + idealGear = Map(ratio=gearRatio2) + ) + ) + +servoParameters3 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio3), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia3), + idealGear = Map(ratio=gearRatio3) + ) + ) + +servoParameters4 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio4), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia4), + idealGear = Map(ratio=gearRatio4) + ) + ) + +servoParameters5 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio5), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia5), + idealGear = Map(ratio=gearRatio5) + ) + ) + +servoParameters6 = Map( + ppi = Map( + gainOuter = Map(k=gearRatioGripper), + gainInner = Map(k=k1Gripper), + PI = Map(k=k2Gripper, T=T2Gripper) + ), + drive = Map( + motorMass = Map(m=motorInertiaGripper) + ) + ) + +featureBody1 = Solid(shape = FileMesh(filename = arm_joint_1_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m1)) +featureBody2 = Solid(shape = FileMesh(filename = arm_joint_2_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m2)) +featureBody3 = Solid(shape = FileMesh(filename = arm_joint_3_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m3)) +featureBody4 = Solid(shape = FileMesh(filename = arm_joint_4_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m4)) +featureBody5 = Solid(shape = FileMesh(filename = arm_joint_5_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m5)) + +linkParameters1 = Map( + parent1 = Par(value = :(armBase_b)), + featureBody = featureBody1, + initRefPos = initPosition[1], + trans = translation1, + rota = Par(value = :(rotation1)) +) + +linkParameters2 = Map( + parent1 = Par(value = :(link1.obj2)), + featureBody = featureBody2, + m = m2, + initRefPos = initPosition[2], + trans = translation2, + rota = Par(value = :(rotation2)) +) + +linkParameters3 = Map( + parent1 = Par(value = :(link2.obj2)), + featureBody = featureBody3, + m = m3, + initRefPos = initPosition[3], + trans = translation3, + rota = Par(value = :(rotation3)) +) + +linkParameters4 = Map( + parent1 = Par(value = :(link3.obj2)), + featureBody = featureBody4, + m = m4, + initRefPos = initPosition[4], + trans = translation4, + rota = Par(value = :(nullRot)) +) + +linkParameters5 = Map( + parent1 = Par(value = :(link4.obj2)), + featureBody = featureBody5, + m = m5, + initRefPos = initPosition[5], + trans = translation5, + rota = Par(value = :(rotation5)) +) + +Link = Model( + parent1 = Par(value = :(nothing)), + featureBody = featureBody1, + trans = [0,0,0], + rota = Par(value = :(nullRot)), + + obj1 = Object3D(parent=:parent1, rotation=:rota), + body = Object3D(feature = :featureBody), + obj2 = Object3D(parent =:body, translation = :trans), +) + +MyGripper = Model( + obj1 = Object3D(parent=:(link5.obj2), rotation=[0, 0, -180u"°"]), + gripper_base_frame = Object3D(parent=:obj1, feature=Solid(shape= + FileMesh(filename = gripper_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.199))), + gripper_left_finger_a = Object3D(), + gripper_left_finger = Object3D(parent=:gripper_left_finger_a, translation=[0, 0.0082, 0], + feature=Solid(shape= FileMesh(filename = gripper_left_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), solidMaterial="DryWood")), + gripper_left_finger_box = Object3D(parent=:gripper_left_finger_a, translation=[0.0, 0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripper_right_finger_a = Object3D(parent=:gripper_base_frame, + translation=[0,-rightFingerOffset,0]), + gripper_right_finger = Object3D(parent=:gripper_right_finger_a, translation=[0, -0.0082, 0], + feature = Solid(shape = FileMesh(filename = gripper_right_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), solidMaterial="DryWood")), + gripper_right_finger_box = Object3D(parent=:gripper_right_finger_a, translation=[0.0, -0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), +) + +YouBot(worldName;pathIndexOffset) = Model( + pathIndexOffset2 = pathIndexOffset, + base = Base, + worldName = Par(worldName), + arm_base_frame = Object3D(parent=:(base.base_frame), + translation=[0.143, 0.0, 0.046], + feature = Solid(shape = FileMesh(filename = arm_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.961))), + armBase_b = Object3D(parent=:arm_base_frame, translation=[0.024, 0, 0.115]), + + link1 = Link | linkParameters1, + link2 = Link | linkParameters2, + link3 = Link | linkParameters3, + link4 = Link | linkParameters4, + link5 = Link | linkParameters5, + gripper = MyGripper, + + rev1 = RevoluteWithFlange(obj1 = :(link1.obj1), obj2 = :(link1.body), + axis=axisLink, phi = Var(init = initPosition[1+pathIndexOffset])), + rev2 = RevoluteWithFlange(obj1 = :(link2.obj1), obj2 = :(link2.body), + axis=axisLink, phi = Var(init = initPosition[2+pathIndexOffset])), + rev3 = RevoluteWithFlange(obj1 = :(link3.obj1), obj2 = :(link3.body), + axis=axisLink, phi = Var(init = initPosition[3+pathIndexOffset])), + rev4 = RevoluteWithFlange(obj1 = :(link4.obj1), obj2 = :(link4.body), + axis=axisLink, phi = Var(init = initPosition[4+pathIndexOffset])), + rev5 = RevoluteWithFlange(obj1 = :(link5.obj1), obj2 = :(link5.body), + axis=axisLink, phi = Var(init = initPosition[5+pathIndexOffset])), + + prism = PrismaticWithFlange(obj1 = :(gripper.gripper_right_finger_a), obj2 = :(gripper.gripper_left_finger_a), + axis=axisGripper, s = Var(init = initPosition[6+pathIndexOffset]) ), + + servo1 = Servo, + servo2 = Servo, + servo3 = Servo, + servo4 = Servo, + servo5 = Servo, + servo6 = ServoTrans, + + modelActions = ModelActions(world=:world, actions=robotProgram), + currentAction = Var(hideResult=true), + + equations=:[ + currentAction = executeActions(modelActions), + servo1.refLoadAngle = getRefPathPosition(currentAction, 1+pathIndexOffset2), + servo2.refLoadAngle = getRefPathPosition(currentAction, 2+pathIndexOffset2), + servo3.refLoadAngle = getRefPathPosition(currentAction, 3+pathIndexOffset2), + servo4.refLoadAngle = getRefPathPosition(currentAction, 4+pathIndexOffset2), + servo5.refLoadAngle = getRefPathPosition(currentAction, 5+pathIndexOffset2), + servo6.refLoadPos = getRefPathPosition(currentAction, 6+pathIndexOffset2) + ], + + connect = :[ + (servo1.flange, rev1.flange) + (servo2.flange, rev2.flange) + (servo3.flange, rev3.flange) + (servo4.flange, rev4.flange) + (servo5.flange, rev5.flange) + (servo6.flange, prism.flange) + ] +) + +Scenario = Model3D( + gravField = UniformGravityField(g=9.81, n=[0,0,-1]), + world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, + enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), + + # table = Table, + ground = Ground, + + sphere = Object3D(parent=:world, fixedToParent=false, + # assemblyRoot=true, + translation=[-0.78, 0.0, 0.1845], #[-0.78, 0.0, 0.1792], + feature=Solid(shape=Sphere(diameter=diameter), + visualMaterial=VisualMaterial(color = "Green"), + solidMaterial="DryWood", + collision=true)), + youbot1 = YouBot("world", pathIndexOffset=0) +) + +modelParameters = Map( + youbot1 = Map( + base = Map( + orientation = [0.0, 0.0, 0.0], + position = [-0.569, 0.0, 0.084], + ), + servo1 = servoParameters1, + servo2 = servoParameters2, + servo3 = servoParameters3, + servo4 = servoParameters4, + servo5 = servoParameters5, + servo6 = servoParameters6 + ) +) + + +youbotModel = Scenario | modelParameters + +youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) + +stopTime = 13.5 +tolerance = 1e-7 +# use boxes instead of FileMesh for better performance +if Sys.iswindows() + requiredFinalStates = [3.1415947353156977, 2.4404618792441014e-6, 0.06413101847025342, -0.38051837261409877, 1.5066656031419667, 0.38051766101104056, 0.08493817930077076, -0.5038810237109801, -4.6660513554873775e-7, 3.752739122664999e-7, 0.02640050697959681, -0.05767990246020697, -0.38561153991690283, -0.13530700791021252, 0.003352929911771347, -0.1332188913735816, 0.04978725220079851, -6.996407090545839e-6, -0.7775608475001248, 0.005107163857175746, 0.38015816381775924, 0.04903299043458591, 4.3039218890990144e-6, 0.10223649086921456, -0.23251977356265563, 0.5671780375371024, 6.409469280850812, 5.535728438671193e-7, 0.5044004851046952, -2.473504004121177e-6] +elseif Sys.isapple() + requiredFinalStates = missing +else + requiredFinalStates = [3.141594735312716, 2.440457544603476e-6, 0.06413101847181349, -0.380518372597451, 1.5066656031498458, 0.3805176609510743, 0.08493817930222113, -0.5038810238174551, -4.666083661539158e-7, 3.7522126012581574e-7, 0.026400488779754015, -0.057679898615238055, -0.3856115354698501, -0.1353070539098794, 0.0033529207102979517, -0.13321886490666132, 0.049787252111476124, -6.996240368087093e-6, -0.7775614019840636, 0.005107163901554179, 0.38015912423821296, 0.04903348978501739, 4.303738169808239e-6, 0.1022367696490673, -0.2408237298542479, 0.6183371828465237, 6.424368692503729, 5.538288842857016e-7, 0.5044004851850659, -2.4739225351517005e-6] +end + + +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) + +@usingModiaPlot +plot(youbot, ["sphere.translation", + ("youbot1.rev1.phi", + "youbot1.rev2.phi", + "youbot1.rev3.phi", + "youbot1.rev4.phi", + "youbot1.rev5.phi")], figure=1) + +plot(youbot, [ "sphere.r_abs[1]", + "sphere.r_abs[2]", + "sphere.r_abs[3]"], figure=2) + +end diff --git a/test/Segmented/ScenarioSegmentedCollisionOff.jl b/test/Segmented/ScenarioSegmentedCollisionOff.jl new file mode 100644 index 0000000..bf743b5 --- /dev/null +++ b/test/Segmented/ScenarioSegmentedCollisionOff.jl @@ -0,0 +1,590 @@ +module ScenarioSegmentedCollisionOff + +using Modia3D + +include("$(Modia3D.modelsPath)/Blocks.jl") +include("$(Modia3D.modelsPath)/Electric.jl") +include("$(Modia3D.modelsPath)/Rotational.jl") +include("$(Modia3D.modelsPath)/Translational.jl") + + +# some constants +# use boxes instead of meshes for finger contact + +# visual materials +vmatYellow = VisualMaterial(color=[255,215,0]) # ground +vmatBlue = VisualMaterial(color="DodgerBlue3") # table +vmatGrey = VisualMaterial(color="DarkGrey") # work piece +vmatInvisible = VisualMaterial(transparency=1.0) # contact boxes +tableX = 0.3 +tableY = 0.3 +tableZ = 0.01 +heigthLeg = 0.365 +widthLeg = 0.02 +diameter = 0.05 +diameterLock = 0.03 +rightFingerOffset = 0.03 + +# all needed paths to fileMeshes +base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/base_frame.obj") +plate_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/plate.obj") +front_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-right_wheel.obj") +front_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-left_wheel.obj") +back_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-right_wheel.obj") +back_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-left_wheel.obj") +arm_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_base_frame.obj") +arm_joint_1_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_1.obj") +arm_joint_2_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_2.obj") +arm_joint_3_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_3.obj") +arm_joint_4_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_4.obj") +arm_joint_5_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_5.obj") +gripper_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_base_frame.obj") +gripper_left_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_left_finger.obj") +gripper_right_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_right_finger.obj") + +# Drive train data: motor inertias and gear ratios +nullRot = [0, 0, 0] + +motorInertia1 = 0.0000135 + 0.000000409 +motorInertia2 = 0.0000135 + 0.000000409 +motorInertia3 = 0.0000135 + 0.000000071 +motorInertia4 = 0.00000925 + 0.000000071 +motorInertia5 = 0.0000035 + 0.000000071 +gearRatio1 = 156.0 +gearRatio2 = 156.0 +gearRatio3 = 100.0 +gearRatio4 = 71.0 +gearRatio5 = 71.0 + +m1=1.390 +translation1 =[0.033,0,0] +rotation1 = [180u"°", 0, 0] + +m2=1.318 +translation2=[0.155,0,0] +rotation2 = [90u"°", 0.0, -90u"°"] + +m3=0.821 +translation3=[0,0.135,0] +rotation3 = [0, 0, -90u"°"] + +m4=0.769 +translation4 = [0,0.11316,0] + +m5=0.687 +translation5=[0,0,0.05716] +rotation5 = [-90u"°", 0, 0] + +### ----------------- Servo Model ----------------------- +# parameters for Link +axisLink = 3 +dLink = 1.0 +k1Link = 50.0 +k2Link = 0.1 +T2Link = 1.0 +# parameters for MyGripper +axisGripper = 2 +k1Gripper = 50.0 +k2Gripper = 0.1*100 +T2Gripper = 1.0 +motorInertiaGripper = 0.1 +gearRatioGripper = 1.0 + +initPosition = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +function robotProgram(robotActions) + addReferencePath(robotActions, + names = ["bot1angle1", "bot1angle2", "bot1angle3", "bot1angle4", "bot1angle5", "bot1gripper"], + position = initPosition, + v_max = [2.68512, 2.68512, 4.8879, 5.8997, 5.8997, 2.0], + a_max = [1.5, 1.5, 1.5, 1.5, 1.5, 0.5] + ) + # Attach sphere to plate + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0, enableContactDetection=false) + EventAfterPeriod(robotActions, 1e-10) + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 0.0 0.0 diameter+0.01; # start top of ball + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # go to plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0, enableContactDetection=true) + + # gripping via collision handling + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + ]) + + # Attach sphere to gripper + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", enableContactDetection=false) + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter-0.002; # release + plate + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter+0.01; # release + plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0, enableContactDetection=true) + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # release + plate + ]) + + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0, enableContactDetection=false) + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter+0.01; # open + top + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # open + plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0, enableContactDetection=true) + + # gripping via collision handling + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + ]) + + # Attach sphere to gripper + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", waitingPeriod=0.0, enableContactDetection=false) + + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + ]) + + + return nothing +end + +# Controller Model +Controller = Model( + # Interface + refLoadAngle = input, + actualMotorAngle = input, + actualMotorSpeed = input, + refTorque = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadAngle, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorAngle, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refTorque)] +) + +Drive = Model( + # Interface + refTorque = input, + actualSpeed = output, + actualAngle = output, + flange = Flange, + # Components + torque = UnitlessTorque, + speedSensor = UnitlessSpeedSensor, + angleSensor = UnitlessAngleSensor, + motorInertia = Inertia, + idealGear = IdealGear, + + connect = :[ + (refTorque, torque.tau) + (torque.flange, speedSensor.flange, angleSensor.flange, motorInertia.flange_a) + (motorInertia.flange_b, idealGear.flange_a) + (idealGear.flange_b, flange) + (speedSensor.w, actualSpeed) + (angleSensor.phi, actualAngle) + ] +) + +Servo = Model( + refLoadAngle = input, + flange = Flange, + ppi = Controller, + drive = Drive, + + connect = :[ + # inputs of ppi + (refLoadAngle, ppi.refLoadAngle) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorAngle, drive.actualAngle) + # output of ppi --> input of motor + (ppi.refTorque, drive.refTorque) + # Flange of drive + (drive.flange, flange) + ] +) + +# Controller Model +ControllerTrans = Model( + # Interface + refLoadPos = input, + actualMotorPos = input, + actualMotorSpeed = input, + refForce = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadPos, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorPos, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refForce)] +) + +DriveTrans = Model( + # Interface + refForce = input, + actualSpeed = output, + actualPos = output, + flange = TranslationalFlange, + # Components + force = UnitlessForce, + speedSensor = UnitlessVelocitySensor, + posSensor = UnitlessPositionSensor, + motorMass = TranslationalMass, + + connect = :[ + (refForce, force.f) + (force.flange, speedSensor.flange, posSensor.flange, motorMass.flange_a) + (motorMass.flange_b, flange) + (speedSensor.v, actualSpeed) + (posSensor.s, actualPos) + ] +) + +ServoTrans = Model( + refLoadPos = input, + flange = TranslationalFlange, + ppi = ControllerTrans, + drive = DriveTrans, + + connect = :[ + # inputs of ppi + (refLoadPos, ppi.refLoadPos) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorPos, drive.actualPos) + # output of ppi --> input of motor + (ppi.refForce, drive.refForce) + # Flange of drive + (drive.flange, flange) + ] +) + +Ground = Model( + ground = Object3D(parent=:world, + translation=[0.0, 0.0, -0.005], feature = Solid(shape =Box(lengthX=2.5, lengthY=2.5, lengthZ=0.01), solidMaterial="DryWood", visualMaterial=vmatYellow)) +) + + +Base = Model( + position = [0.0, 0.0, 0.0], + orientation = [0.0, 0.0, 0.0], + base_frame = Object3D(parent=:world, translation=:position, rotation=:orientation, + feature=Solid(shape=FileMesh(filename = base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=19.803))), + plate = Object3D(parent=:base_frame, translation=[-0.159, 0, 0.046], + feature=Solid(shape=FileMesh(filename = plate_obj), massProperties=MassPropertiesFromShapeAndMass(mass=2.397), collision=false)), + plateLock = Object3D(parent=:base_frame, + lockable=true, + translation=[-0.19,0.0,0.0702], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + plate_box = Object3D(parent=:base_frame, translation=[-0.16, 0, 0.0702],# 0.06514], + feature=Solid(shape=Box(lengthX=0.22, lengthY=0.22, lengthZ=0.01), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, + solidMaterial="DryWood", collision=true)), + front_right_wheel = Object3D(parent=:base_frame, translation=[0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + front_left_wheel = Object3D(parent=:base_frame, translation=[0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_right_wheel = Object3D(parent=:base_frame, translation=[-0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_left_wheel = Object3D(parent=:base_frame, translation=[-0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))) +) + +servoParameters1 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio1), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia1), + idealGear = Map(ratio=gearRatio1) + ) + ) + +servoParameters2 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio2), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia2), + idealGear = Map(ratio=gearRatio2) + ) + ) + +servoParameters3 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio3), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia3), + idealGear = Map(ratio=gearRatio3) + ) + ) + +servoParameters4 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio4), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia4), + idealGear = Map(ratio=gearRatio4) + ) + ) + +servoParameters5 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio5), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia5), + idealGear = Map(ratio=gearRatio5) + ) + ) + +servoParameters6 = Map( + ppi = Map( + gainOuter = Map(k=gearRatioGripper), + gainInner = Map(k=k1Gripper), + PI = Map(k=k2Gripper, T=T2Gripper) + ), + drive = Map( + motorMass = Map(m=motorInertiaGripper) + ) + ) + +featureBody1 = Solid(shape = FileMesh(filename = arm_joint_1_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m1)) +featureBody2 = Solid(shape = FileMesh(filename = arm_joint_2_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m2)) +featureBody3 = Solid(shape = FileMesh(filename = arm_joint_3_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m3)) +featureBody4 = Solid(shape = FileMesh(filename = arm_joint_4_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m4)) +featureBody5 = Solid(shape = FileMesh(filename = arm_joint_5_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m5)) + +linkParameters1 = Map( + parent1 = Par(value = :(armBase_b)), + featureBody = featureBody1, + initRefPos = initPosition[1], + trans = translation1, + rota = Par(value = :(rotation1)) +) + +linkParameters2 = Map( + parent1 = Par(value = :(link1.obj2)), + featureBody = featureBody2, + m = m2, + initRefPos = initPosition[2], + trans = translation2, + rota = Par(value = :(rotation2)) +) + +linkParameters3 = Map( + parent1 = Par(value = :(link2.obj2)), + featureBody = featureBody3, + m = m3, + initRefPos = initPosition[3], + trans = translation3, + rota = Par(value = :(rotation3)) +) + +linkParameters4 = Map( + parent1 = Par(value = :(link3.obj2)), + featureBody = featureBody4, + m = m4, + initRefPos = initPosition[4], + trans = translation4, + rota = Par(value = :(nullRot)) +) + +linkParameters5 = Map( + parent1 = Par(value = :(link4.obj2)), + featureBody = featureBody5, + m = m5, + initRefPos = initPosition[5], + trans = translation5, + rota = Par(value = :(rotation5)) +) + +Link = Model( + parent1 = Par(value = :(nothing)), + featureBody = featureBody1, + trans = [0,0,0], + rota = Par(value = :(nullRot)), + + obj1 = Object3D(parent=:parent1, rotation=:rota), + body = Object3D(feature = :featureBody), + obj2 = Object3D(parent =:body, translation = :trans), +) + +MyGripper = Model( + obj1 = Object3D(parent=:(link5.obj2), rotation=[0, 0, -180u"°"]), + gripper_base_frame = Object3D(parent=:obj1, feature=Solid(shape= + FileMesh(filename = gripper_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.199))), + gripper_left_finger_a = Object3D(), + gripper_left_finger = Object3D(parent=:gripper_left_finger_a, translation=[0, 0.0082, 0], + feature=Solid(shape= FileMesh(filename = gripper_left_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), collision=false)), + gripper_left_finger_box = Object3D(parent=:gripper_left_finger_a, translation=[0.0, 0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripper_right_finger_a = Object3D(parent=:gripper_base_frame, + translation=[0,-rightFingerOffset,0]), + gripper_right_finger = Object3D(parent=:gripper_right_finger_a, translation=[0, -0.0082, 0], + feature = Solid(shape = FileMesh(filename = gripper_right_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), collision=false)), + gripper_right_finger_box = Object3D(parent=:gripper_right_finger_a, translation=[0.0, -0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripperLock = Object3D(parent=:gripper_right_finger_a, + lockable=true, + translation=[0.012/2,0.01/2,0.045/2], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), +) + +YouBot(worldName;pathIndexOffset) = Model( + pathIndexOffset2 = pathIndexOffset, + base = Base, + worldName = Par(worldName), + arm_base_frame = Object3D(parent=:(base.base_frame), + translation=[0.143, 0.0, 0.046], + feature = Solid(shape = FileMesh(filename = arm_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.961))), + armBase_b = Object3D(parent=:arm_base_frame, translation=[0.024, 0, 0.115]), + + link1 = Link | linkParameters1, + link2 = Link | linkParameters2, + link3 = Link | linkParameters3, + link4 = Link | linkParameters4, + link5 = Link | linkParameters5, + gripper = MyGripper, + + rev1 = RevoluteWithFlange(obj1 = :(link1.obj1), obj2 = :(link1.body), + axis=axisLink, phi = Var(init = initPosition[1+pathIndexOffset])), + rev2 = RevoluteWithFlange(obj1 = :(link2.obj1), obj2 = :(link2.body), + axis=axisLink, phi = Var(init = initPosition[2+pathIndexOffset])), + rev3 = RevoluteWithFlange(obj1 = :(link3.obj1), obj2 = :(link3.body), + axis=axisLink, phi = Var(init = initPosition[3+pathIndexOffset])), + rev4 = RevoluteWithFlange(obj1 = :(link4.obj1), obj2 = :(link4.body), + axis=axisLink, phi = Var(init = initPosition[4+pathIndexOffset])), + rev5 = RevoluteWithFlange(obj1 = :(link5.obj1), obj2 = :(link5.body), + axis=axisLink, phi = Var(init = initPosition[5+pathIndexOffset])), + + prism = PrismaticWithFlange(obj1 = :(gripper.gripper_right_finger_a), obj2 = :(gripper.gripper_left_finger_a), + axis=axisGripper, s = Var(init = initPosition[6+pathIndexOffset]) ), + + servo1 = Servo, + servo2 = Servo, + servo3 = Servo, + servo4 = Servo, + servo5 = Servo, + servo6 = ServoTrans, + + modelActions = ModelActions(world=:world, actions=robotProgram), + currentAction = Var(hideResult=true), + + equations=:[ + currentAction = executeActions(modelActions), + servo1.refLoadAngle = getRefPathPosition(currentAction, 1+pathIndexOffset2), + servo2.refLoadAngle = getRefPathPosition(currentAction, 2+pathIndexOffset2), + servo3.refLoadAngle = getRefPathPosition(currentAction, 3+pathIndexOffset2), + servo4.refLoadAngle = getRefPathPosition(currentAction, 4+pathIndexOffset2), + servo5.refLoadAngle = getRefPathPosition(currentAction, 5+pathIndexOffset2), + servo6.refLoadPos = getRefPathPosition(currentAction, 6+pathIndexOffset2) + ], + + connect = :[ + (servo1.flange, rev1.flange) + (servo2.flange, rev2.flange) + (servo3.flange, rev3.flange) + (servo4.flange, rev4.flange) + (servo5.flange, rev5.flange) + (servo6.flange, prism.flange) + ] +) + +Scenario = Model3D( + gravField = UniformGravityField(g=9.81, n=[0,0,-1]), + world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, + enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), #, animationFile="YouBotDynamicState.json")), +# worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.2))), + + + ground = Ground, + + sphere = Object3D(parent=:world, fixedToParent=false, + assemblyRoot=true, + translation=[-0.78, 0.0, 0.1845], #[-0.78, 0.0, 0.1792], + feature=Solid(shape=Sphere(diameter=diameter), + visualMaterial=vmatGrey, + solidMaterial="DryWood", + collision=true)), + sphereLock = Object3D(parent=:sphere, + lockable=true, + translation=[0.0,diameter/2,0.0], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + youbot1 = YouBot("world", pathIndexOffset=0) +) + +modelParameters = Map( + youbot1 = Map( + base = Map( + orientation = [0.0, 0.0, 0.0], + position = [-0.569, 0.0, 0.084], + ), + servo1 = servoParameters1, + servo2 = servoParameters2, + servo3 = servoParameters3, + servo4 = servoParameters4, + servo5 = servoParameters5, + servo6 = servoParameters6 + ) +) + + +youbotModel = Scenario | modelParameters + +youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) + +stopTime = 13.5 +tolerance = 1e-7 +# use boxes instead of FileMesh for better collision performance +requiredFinalStates = [3.1415926313235216, 2.227704607451224e-8, -2.8441240591542057e-7, 2.844479623947795e-7, -6.300618493694368e-7, 6.301438745162919e-7, -3.8367060940761664e-7, 3.8373222528068616e-7, 2.9773366458646674e-9, -2.9778176373587e-9, -0.00017011927750574103, 0.2394616012878047, -0.003881746258204947, -0.002274156117355221, 1.0356446209926906e-5, -8.985047184676908e-11, 0.047999999998151396, 1.8748871907289576e-12, -0.7616251395986522, 0.00025213516122149976, 0.18419327722973222, 1.2698578611388089e-9, 2.2444158136065587e-12, -1.772988910724902e-11, 3.141474457755385, 1.0187988505971024, 3.141041134037263, -8.150376270957884e-11, 4.6108853365504784e-8, 8.826370248608988e-22] + +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) + +# showInfo(youbot) + +@usingModiaPlot +plot(youbot, ["sphere.translation", + ("youbot1.rev1.phi", + "youbot1.rev2.phi", + "youbot1.rev3.phi", + "youbot1.rev4.phi", + "youbot1.rev5.phi")], figure=1) + +plot(youbot, [ "sphere.r_abs[1]", + "sphere.r_abs[2]", + "sphere.r_abs[3]"], figure=2) + +end diff --git a/test/Segmented/ScenarioSegmentedCollisionOn.jl b/test/Segmented/ScenarioSegmentedCollisionOn.jl new file mode 100644 index 0000000..2fc5379 --- /dev/null +++ b/test/Segmented/ScenarioSegmentedCollisionOn.jl @@ -0,0 +1,590 @@ +module ScenarioSegmentedCollisionOn + +using Modia3D + +include("$(Modia3D.modelsPath)/Blocks.jl") +include("$(Modia3D.modelsPath)/Electric.jl") +include("$(Modia3D.modelsPath)/Rotational.jl") +include("$(Modia3D.modelsPath)/Translational.jl") + + +# some constants +# use boxes instead of meshes for finger contact + +# visual materials +vmatYellow = VisualMaterial(color=[255,215,0]) # ground +vmatBlue = VisualMaterial(color="DodgerBlue3") # table +vmatGrey = VisualMaterial(color="DarkGrey") # work piece +vmatInvisible = VisualMaterial(transparency=1.0) # contact boxes +tableX = 0.3 +tableY = 0.3 +tableZ = 0.01 +heigthLeg = 0.365 +widthLeg = 0.02 +diameter = 0.05 +diameterLock = 0.03 +rightFingerOffset = 0.03 + +# all needed paths to fileMeshes +base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/base_frame.obj") +plate_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/plate.obj") +front_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-right_wheel.obj") +front_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-left_wheel.obj") +back_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-right_wheel.obj") +back_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-left_wheel.obj") +arm_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_base_frame.obj") +arm_joint_1_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_1.obj") +arm_joint_2_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_2.obj") +arm_joint_3_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_3.obj") +arm_joint_4_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_4.obj") +arm_joint_5_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_5.obj") +gripper_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_base_frame.obj") +gripper_left_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_left_finger.obj") +gripper_right_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_right_finger.obj") + +# Drive train data: motor inertias and gear ratios +nullRot = [0, 0, 0] + +motorInertia1 = 0.0000135 + 0.000000409 +motorInertia2 = 0.0000135 + 0.000000409 +motorInertia3 = 0.0000135 + 0.000000071 +motorInertia4 = 0.00000925 + 0.000000071 +motorInertia5 = 0.0000035 + 0.000000071 +gearRatio1 = 156.0 +gearRatio2 = 156.0 +gearRatio3 = 100.0 +gearRatio4 = 71.0 +gearRatio5 = 71.0 + +m1=1.390 +translation1 =[0.033,0,0] +rotation1 = [180u"°", 0, 0] + +m2=1.318 +translation2=[0.155,0,0] +rotation2 = [90u"°", 0.0, -90u"°"] + +m3=0.821 +translation3=[0,0.135,0] +rotation3 = [0, 0, -90u"°"] + +m4=0.769 +translation4 = [0,0.11316,0] + +m5=0.687 +translation5=[0,0,0.05716] +rotation5 = [-90u"°", 0, 0] + +### ----------------- Servo Model ----------------------- +# parameters for Link +axisLink = 3 +dLink = 1.0 +k1Link = 50.0 +k2Link = 0.1 +T2Link = 1.0 +# parameters for MyGripper +axisGripper = 2 +k1Gripper = 50.0 +k2Gripper = 0.1*100 +T2Gripper = 1.0 +motorInertiaGripper = 0.1 +gearRatioGripper = 1.0 + +initPosition = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +function robotProgram(robotActions) + addReferencePath(robotActions, + names = ["bot1angle1", "bot1angle2", "bot1angle3", "bot1angle4", "bot1angle5", "bot1gripper"], + position = initPosition, + v_max = [2.68512, 2.68512, 4.8879, 5.8997, 5.8997, 2.0], + a_max = [1.5, 1.5, 1.5, 1.5, 1.5, 0.5] + ) + # Attach sphere to plate + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0) + EventAfterPeriod(robotActions, 1e-10) + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 0.0 0.0 diameter+0.01; # start top of ball + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # go to plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0) + + # gripping via collision handling + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + ]) + + # Attach sphere to gripper + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", waitingPeriod=0.0) + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter-0.002; # release + plate + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter+0.01; # release + plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0) + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # release + plate + ]) + + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0) + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter+0.01; # open + top + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # open + plate + ]) + ActionRelease(robotActions, "sphereLock", waitingPeriod=0.0) + + # gripping via collision handling + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + ]) + + # Attach sphere to gripper + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", waitingPeriod=0.0) + + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + ]) + + + return nothing +end + +# Controller Model +Controller = Model( + # Interface + refLoadAngle = input, + actualMotorAngle = input, + actualMotorSpeed = input, + refTorque = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadAngle, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorAngle, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refTorque)] +) + +Drive = Model( + # Interface + refTorque = input, + actualSpeed = output, + actualAngle = output, + flange = Flange, + # Components + torque = UnitlessTorque, + speedSensor = UnitlessSpeedSensor, + angleSensor = UnitlessAngleSensor, + motorInertia = Inertia, + idealGear = IdealGear, + + connect = :[ + (refTorque, torque.tau) + (torque.flange, speedSensor.flange, angleSensor.flange, motorInertia.flange_a) + (motorInertia.flange_b, idealGear.flange_a) + (idealGear.flange_b, flange) + (speedSensor.w, actualSpeed) + (angleSensor.phi, actualAngle) + ] +) + +Servo = Model( + refLoadAngle = input, + flange = Flange, + ppi = Controller, + drive = Drive, + + connect = :[ + # inputs of ppi + (refLoadAngle, ppi.refLoadAngle) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorAngle, drive.actualAngle) + # output of ppi --> input of motor + (ppi.refTorque, drive.refTorque) + # Flange of drive + (drive.flange, flange) + ] +) + +# Controller Model +ControllerTrans = Model( + # Interface + refLoadPos = input, + actualMotorPos = input, + actualMotorSpeed = input, + refForce = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadPos, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorPos, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refForce)] +) + +DriveTrans = Model( + # Interface + refForce = input, + actualSpeed = output, + actualPos = output, + flange = TranslationalFlange, + # Components + force = UnitlessForce, + speedSensor = UnitlessVelocitySensor, + posSensor = UnitlessPositionSensor, + motorMass = TranslationalMass, + + connect = :[ + (refForce, force.f) + (force.flange, speedSensor.flange, posSensor.flange, motorMass.flange_a) + (motorMass.flange_b, flange) + (speedSensor.v, actualSpeed) + (posSensor.s, actualPos) + ] +) + +ServoTrans = Model( + refLoadPos = input, + flange = TranslationalFlange, + ppi = ControllerTrans, + drive = DriveTrans, + + connect = :[ + # inputs of ppi + (refLoadPos, ppi.refLoadPos) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorPos, drive.actualPos) + # output of ppi --> input of motor + (ppi.refForce, drive.refForce) + # Flange of drive + (drive.flange, flange) + ] +) + +Ground = Model( + ground = Object3D(parent=:world, + translation=[0.0, 0.0, -0.005], feature = Solid(shape =Box(lengthX=2.5, lengthY=2.5, lengthZ=0.01), solidMaterial="DryWood", visualMaterial=vmatYellow)) +) + + +Base = Model( + position = [0.0, 0.0, 0.0], + orientation = [0.0, 0.0, 0.0], + base_frame = Object3D(parent=:world, translation=:position, rotation=:orientation, + feature=Solid(shape=FileMesh(filename = base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=19.803))), + plate = Object3D(parent=:base_frame, translation=[-0.159, 0, 0.046], + feature=Solid(shape=FileMesh(filename = plate_obj), massProperties=MassPropertiesFromShapeAndMass(mass=2.397), collision=false)), + plateLock = Object3D(parent=:base_frame, + lockable=true, + translation=[-0.19,0.0,0.0702], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + plate_box = Object3D(parent=:base_frame, translation=[-0.16, 0, 0.0702],# 0.06514], + feature=Solid(shape=Box(lengthX=0.22, lengthY=0.22, lengthZ=0.01), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, + solidMaterial="DryWood", collision=true)), + front_right_wheel = Object3D(parent=:base_frame, translation=[0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + front_left_wheel = Object3D(parent=:base_frame, translation=[0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_right_wheel = Object3D(parent=:base_frame, translation=[-0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_left_wheel = Object3D(parent=:base_frame, translation=[-0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))) +) + +servoParameters1 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio1), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia1), + idealGear = Map(ratio=gearRatio1) + ) + ) + +servoParameters2 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio2), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia2), + idealGear = Map(ratio=gearRatio2) + ) + ) + +servoParameters3 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio3), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia3), + idealGear = Map(ratio=gearRatio3) + ) + ) + +servoParameters4 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio4), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia4), + idealGear = Map(ratio=gearRatio4) + ) + ) + +servoParameters5 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio5), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia5), + idealGear = Map(ratio=gearRatio5) + ) + ) + +servoParameters6 = Map( + ppi = Map( + gainOuter = Map(k=gearRatioGripper), + gainInner = Map(k=k1Gripper), + PI = Map(k=k2Gripper, T=T2Gripper) + ), + drive = Map( + motorMass = Map(m=motorInertiaGripper) + ) + ) + +featureBody1 = Solid(shape = FileMesh(filename = arm_joint_1_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m1)) +featureBody2 = Solid(shape = FileMesh(filename = arm_joint_2_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m2)) +featureBody3 = Solid(shape = FileMesh(filename = arm_joint_3_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m3)) +featureBody4 = Solid(shape = FileMesh(filename = arm_joint_4_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m4)) +featureBody5 = Solid(shape = FileMesh(filename = arm_joint_5_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m5)) + +linkParameters1 = Map( + parent1 = Par(value = :(armBase_b)), + featureBody = featureBody1, + initRefPos = initPosition[1], + trans = translation1, + rota = Par(value = :(rotation1)) +) + +linkParameters2 = Map( + parent1 = Par(value = :(link1.obj2)), + featureBody = featureBody2, + m = m2, + initRefPos = initPosition[2], + trans = translation2, + rota = Par(value = :(rotation2)) +) + +linkParameters3 = Map( + parent1 = Par(value = :(link2.obj2)), + featureBody = featureBody3, + m = m3, + initRefPos = initPosition[3], + trans = translation3, + rota = Par(value = :(rotation3)) +) + +linkParameters4 = Map( + parent1 = Par(value = :(link3.obj2)), + featureBody = featureBody4, + m = m4, + initRefPos = initPosition[4], + trans = translation4, + rota = Par(value = :(nullRot)) +) + +linkParameters5 = Map( + parent1 = Par(value = :(link4.obj2)), + featureBody = featureBody5, + m = m5, + initRefPos = initPosition[5], + trans = translation5, + rota = Par(value = :(rotation5)) +) + +Link = Model( + parent1 = Par(value = :(nothing)), + featureBody = featureBody1, + trans = [0,0,0], + rota = Par(value = :(nullRot)), + + obj1 = Object3D(parent=:parent1, rotation=:rota), + body = Object3D(feature = :featureBody), + obj2 = Object3D(parent =:body, translation = :trans), +) + +MyGripper = Model( + obj1 = Object3D(parent=:(link5.obj2), rotation=[0, 0, -180u"°"]), + gripper_base_frame = Object3D(parent=:obj1, feature=Solid(shape= + FileMesh(filename = gripper_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.199))), + gripper_left_finger_a = Object3D(), + gripper_left_finger = Object3D(parent=:gripper_left_finger_a, translation=[0, 0.0082, 0], + feature=Solid(shape= FileMesh(filename = gripper_left_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), collision=false)), + gripper_left_finger_box = Object3D(parent=:gripper_left_finger_a, translation=[0.0, 0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripper_right_finger_a = Object3D(parent=:gripper_base_frame, + translation=[0,-rightFingerOffset,0]), + gripper_right_finger = Object3D(parent=:gripper_right_finger_a, translation=[0, -0.0082, 0], + feature = Solid(shape = FileMesh(filename = gripper_right_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), collision=false)), + gripper_right_finger_box = Object3D(parent=:gripper_right_finger_a, translation=[0.0, -0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, solidMaterial="DryWood", collision=true)), + gripperLock = Object3D(parent=:gripper_right_finger_a, + lockable=true, + translation=[0.012/2,0.01/2,0.045/2], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), +) + +YouBot(worldName;pathIndexOffset) = Model( + pathIndexOffset2 = pathIndexOffset, + base = Base, + worldName = Par(worldName), + arm_base_frame = Object3D(parent=:(base.base_frame), + translation=[0.143, 0.0, 0.046], + feature = Solid(shape = FileMesh(filename = arm_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.961))), + armBase_b = Object3D(parent=:arm_base_frame, translation=[0.024, 0, 0.115]), + + link1 = Link | linkParameters1, + link2 = Link | linkParameters2, + link3 = Link | linkParameters3, + link4 = Link | linkParameters4, + link5 = Link | linkParameters5, + gripper = MyGripper, + + rev1 = RevoluteWithFlange(obj1 = :(link1.obj1), obj2 = :(link1.body), + axis=axisLink, phi = Var(init = initPosition[1+pathIndexOffset])), + rev2 = RevoluteWithFlange(obj1 = :(link2.obj1), obj2 = :(link2.body), + axis=axisLink, phi = Var(init = initPosition[2+pathIndexOffset])), + rev3 = RevoluteWithFlange(obj1 = :(link3.obj1), obj2 = :(link3.body), + axis=axisLink, phi = Var(init = initPosition[3+pathIndexOffset])), + rev4 = RevoluteWithFlange(obj1 = :(link4.obj1), obj2 = :(link4.body), + axis=axisLink, phi = Var(init = initPosition[4+pathIndexOffset])), + rev5 = RevoluteWithFlange(obj1 = :(link5.obj1), obj2 = :(link5.body), + axis=axisLink, phi = Var(init = initPosition[5+pathIndexOffset])), + + prism = PrismaticWithFlange(obj1 = :(gripper.gripper_right_finger_a), obj2 = :(gripper.gripper_left_finger_a), + axis=axisGripper, s = Var(init = initPosition[6+pathIndexOffset]) ), + + servo1 = Servo, + servo2 = Servo, + servo3 = Servo, + servo4 = Servo, + servo5 = Servo, + servo6 = ServoTrans, + + modelActions = ModelActions(world=:world, actions=robotProgram), + currentAction = Var(hideResult=true), + + equations=:[ + currentAction = executeActions(modelActions), + servo1.refLoadAngle = getRefPathPosition(currentAction, 1+pathIndexOffset2), + servo2.refLoadAngle = getRefPathPosition(currentAction, 2+pathIndexOffset2), + servo3.refLoadAngle = getRefPathPosition(currentAction, 3+pathIndexOffset2), + servo4.refLoadAngle = getRefPathPosition(currentAction, 4+pathIndexOffset2), + servo5.refLoadAngle = getRefPathPosition(currentAction, 5+pathIndexOffset2), + servo6.refLoadPos = getRefPathPosition(currentAction, 6+pathIndexOffset2) + ], + + connect = :[ + (servo1.flange, rev1.flange) + (servo2.flange, rev2.flange) + (servo3.flange, rev3.flange) + (servo4.flange, rev4.flange) + (servo5.flange, rev5.flange) + (servo6.flange, prism.flange) + ] +) + +Scenario = Model3D( + gravField = UniformGravityField(g=9.81, n=[0,0,-1]), + world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, + enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), #, animationFile="YouBotDynamicState.json")), +# worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.2))), + + + ground = Ground, + + sphere = Object3D(parent=:world, fixedToParent=false, + assemblyRoot=true, + translation=[-0.78, 0.0, 0.1845], #[-0.78, 0.0, 0.1792], + feature=Solid(shape=Sphere(diameter=diameter), + visualMaterial=vmatGrey, + solidMaterial="DryWood", + collision=true)), + sphereLock = Object3D(parent=:sphere, + lockable=true, + translation=[0.0,diameter/2,0.0], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + youbot1 = YouBot("world", pathIndexOffset=0) +) + +modelParameters = Map( + youbot1 = Map( + base = Map( + orientation = [0.0, 0.0, 0.0], + position = [-0.569, 0.0, 0.084], + ), + servo1 = servoParameters1, + servo2 = servoParameters2, + servo3 = servoParameters3, + servo4 = servoParameters4, + servo5 = servoParameters5, + servo6 = servoParameters6 + ) +) + + +youbotModel = Scenario | modelParameters + +youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) + +stopTime = 13.5 +tolerance = 1e-7 +# use boxes instead of FileMesh for better collision performance +requiredFinalStates = [3.1415926313235216, 2.227704607451224e-8, -2.8441240591542057e-7, 2.844479623947795e-7, -6.300618493694368e-7, 6.301438745162919e-7, -3.8367060940761664e-7, 3.8373222528068616e-7, 2.9773366458646674e-9, -2.9778176373587e-9, -0.00017011927750574103, 0.2394616012878047, -0.003881746258204947, -0.002274156117355221, 1.0356446209926906e-5, -8.985047184676908e-11, 0.047999999998151396, 1.8748871907289576e-12, -0.7616251395986522, 0.00025213516122149976, 0.18419327722973222, 1.2698578611388089e-9, 2.2444158136065587e-12, -1.772988910724902e-11, 3.141474457755385, 1.0187988505971024, 3.141041134037263, -8.150376270957884e-11, 4.6108853365504784e-8, 8.826370248608988e-22] + +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) + +# showInfo(youbot) + +@usingModiaPlot +plot(youbot, ["sphere.translation", + ("youbot1.rev1.phi", + "youbot1.rev2.phi", + "youbot1.rev3.phi", + "youbot1.rev4.phi", + "youbot1.rev5.phi")], figure=1) + +plot(youbot, [ "sphere.r_abs[1]", + "sphere.r_abs[2]", + "sphere.r_abs[3]"], figure=2) + +end diff --git a/test/Segmented/ScenarioSegmentedOnly.jl b/test/Segmented/ScenarioSegmentedOnly.jl new file mode 100644 index 0000000..bb6773c --- /dev/null +++ b/test/Segmented/ScenarioSegmentedOnly.jl @@ -0,0 +1,586 @@ +module ScenarioSegmentedOnly + +using Modia3D + +include("$(Modia3D.modelsPath)/Blocks.jl") +include("$(Modia3D.modelsPath)/Electric.jl") +include("$(Modia3D.modelsPath)/Rotational.jl") +include("$(Modia3D.modelsPath)/Translational.jl") + + +# some constants +# use boxes instead of meshes for finger contact + +# visual materials +vmatYellow = VisualMaterial(color=[255,215,0]) # ground +vmatBlue = VisualMaterial(color="DodgerBlue3") # table +vmatGrey = VisualMaterial(color="DarkGrey") # work piece +vmatInvisible = VisualMaterial(transparency=1.0) # contact boxes +tableX = 0.3 +tableY = 0.3 +tableZ = 0.01 +heigthLeg = 0.365 +widthLeg = 0.02 +diameter = 0.05 +diameterLock = 0.03 +rightFingerOffset = 0.03 + +# all needed paths to fileMeshes +base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/base_frame.obj") +plate_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/plate.obj") +front_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-right_wheel.obj") +front_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/front-left_wheel.obj") +back_right_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-right_wheel.obj") +back_left_wheel_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/back-left_wheel.obj") +arm_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_base_frame.obj") +arm_joint_1_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_1.obj") +arm_joint_2_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_2.obj") +arm_joint_3_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_3.obj") +arm_joint_4_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_4.obj") +arm_joint_5_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/arm_joint_5.obj") +gripper_base_frame_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_base_frame.obj") +gripper_left_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_left_finger.obj") +gripper_right_finger_obj = joinpath(Modia3D.path, "objects/robot_KUKA_YouBot/gripper_right_finger.obj") + +# Drive train data: motor inertias and gear ratios +nullRot = [0, 0, 0] + +motorInertia1 = 0.0000135 + 0.000000409 +motorInertia2 = 0.0000135 + 0.000000409 +motorInertia3 = 0.0000135 + 0.000000071 +motorInertia4 = 0.00000925 + 0.000000071 +motorInertia5 = 0.0000035 + 0.000000071 +gearRatio1 = 156.0 +gearRatio2 = 156.0 +gearRatio3 = 100.0 +gearRatio4 = 71.0 +gearRatio5 = 71.0 + +m1=1.390 +translation1 =[0.033,0,0] +rotation1 = [180u"°", 0, 0] + +m2=1.318 +translation2=[0.155,0,0] +rotation2 = [90u"°", 0.0, -90u"°"] + +m3=0.821 +translation3=[0,0.135,0] +rotation3 = [0, 0, -90u"°"] + +m4=0.769 +translation4 = [0,0.11316,0] + +m5=0.687 +translation5=[0,0,0.05716] +rotation5 = [-90u"°", 0, 0] + +### ----------------- Servo Model ----------------------- +# parameters for Link +axisLink = 3 +dLink = 1.0 +k1Link = 50.0 +k2Link = 0.1 +T2Link = 1.0 +# parameters for MyGripper +axisGripper = 2 +k1Gripper = 50.0 +k2Gripper = 0.1*100 +T2Gripper = 1.0 +motorInertiaGripper = 0.1 +gearRatioGripper = 1.0 + +initPosition = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +function robotProgram(robotActions) + addReferencePath(robotActions, + names = ["bot1angle1", "bot1angle2", "bot1angle3", "bot1angle4", "bot1angle5", "bot1gripper"], + position = initPosition, + v_max = [2.68512, 2.68512, 4.8879, 5.8997, 5.8997, 2.0], + a_max = [1.5, 1.5, 1.5, 1.5, 1.5, 0.5]) + + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock",waitingPeriod=0.0) + + EventAfterPeriod(robotActions, 1e-10) + + ptpJointSpace(robotActions, [ + pi pi/4 pi/4 0.0 0.0 diameter+0.01; # start top of ball + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # go to plate + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + ]) + + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", waitingPeriod=0.0) + + ptpJointSpace(robotActions, [ + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter-0.002; # release + plate + pi pi/4 - 0.015 pi/4 1.04 0.0 diameter+0.01; # release + plate + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # release + plate + ]) + + ActionReleaseAndAttach(robotActions, "sphereLock", "youbot1.base.plateLock", waitingPeriod=0.0) + + ptpJointSpace(robotActions, [ + pi 0.0 pi/2 0.0 0.0 diameter+0.01; # open + top + pi pi/4 pi/4 1.04 0.0 diameter+0.01; # open + plate + pi pi/4 pi/4 1.04 0.0 diameter-0.002; # grip + ]) + + ActionAttach(robotActions, "sphereLock", "youbot1.gripper.gripperLock", waitingPeriod=0.0) + + ptpJointSpace(robotActions, [ + pi pi/4 - 0.1 pi/4 1.04 0.0 diameter-0.002; # grip + transport a bit + pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top + ]) + return nothing +end + +# Controller Model +Controller = Model( + # Interface + refLoadAngle = input, + actualMotorAngle = input, + actualMotorSpeed = input, + refTorque = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadAngle, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorAngle, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refTorque)] +) + +Drive = Model( + # Interface + refTorque = input, + actualSpeed = output, + actualAngle = output, + flange = Flange, + # Components + torque = UnitlessTorque, + speedSensor = UnitlessSpeedSensor, + angleSensor = UnitlessAngleSensor, + motorInertia = Inertia, + idealGear = IdealGear, + + connect = :[ + (refTorque, torque.tau) + (torque.flange, speedSensor.flange, angleSensor.flange, motorInertia.flange_a) + (motorInertia.flange_b, idealGear.flange_a) + (idealGear.flange_b, flange) + (speedSensor.w, actualSpeed) + (angleSensor.phi, actualAngle) + ] +) + +Servo = Model( + refLoadAngle = input, + flange = Flange, + ppi = Controller, + drive = Drive, + + connect = :[ + # inputs of ppi + (refLoadAngle, ppi.refLoadAngle) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorAngle, drive.actualAngle) + # output of ppi --> input of motor + (ppi.refTorque, drive.refTorque) + # Flange of drive + (drive.flange, flange) + ] +) + +# Controller Model +ControllerTrans = Model( + # Interface + refLoadPos = input, + actualMotorPos = input, + actualMotorSpeed = input, + refForce = output, + # Components + gainOuter = Gain, + angleFeedback = Feedback, + gainInner = Gain, + speedFeedback = Feedback, + PI = PI, + + connect = :[ + (refLoadPos, gainOuter.u) + (gainOuter.y, angleFeedback.u1) + (actualMotorPos, angleFeedback.u2) + (angleFeedback.y, gainInner.u) + (gainInner.y, speedFeedback.u1) + (actualMotorSpeed, speedFeedback.u2) + (speedFeedback.y, PI.u) + (PI.y, refForce)] +) + +DriveTrans = Model( + # Interface + refForce = input, + actualSpeed = output, + actualPos = output, + flange = TranslationalFlange, + # Components + force = UnitlessForce, + speedSensor = UnitlessVelocitySensor, + posSensor = UnitlessPositionSensor, + motorMass = TranslationalMass, + + connect = :[ + (refForce, force.f) + (force.flange, speedSensor.flange, posSensor.flange, motorMass.flange_a) + (motorMass.flange_b, flange) + (speedSensor.v, actualSpeed) + (posSensor.s, actualPos) + ] +) + +ServoTrans = Model( + refLoadPos = input, + flange = TranslationalFlange, + ppi = ControllerTrans, + drive = DriveTrans, + + connect = :[ + # inputs of ppi + (refLoadPos, ppi.refLoadPos) + (ppi.actualMotorSpeed, drive.actualSpeed) + (ppi.actualMotorPos, drive.actualPos) + # output of ppi --> input of motor + (ppi.refForce, drive.refForce) + # Flange of drive + (drive.flange, flange) + ] +) + +Ground = Model( + ground = Object3D(parent=:world, + translation=[0.0, 0.0, -0.005], feature = Solid(shape =Box(lengthX=2.5, lengthY=2.5, lengthZ=0.01), solidMaterial="DryWood", visualMaterial=vmatYellow)) +) + +Table = Model( + plate = Object3D(parent=:world, translation=[0.0, 0.0, heigthLeg], + feature=Solid(shape=Box(lengthX=tableX, lengthY=tableY, lengthZ=tableZ), + collisionSmoothingRadius=0.001, solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg1 = Object3D(parent=:plate, translation=[ tableX/2-widthLeg/2, tableY/2-widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), + solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg2 = Object3D(parent=:plate, translation=[-tableX/2+widthLeg/2, tableY/2-widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg3 = Object3D(parent=:plate, translation=[ tableX/2-widthLeg/2, -tableY/2+widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), solidMaterial="DryWood", visualMaterial=vmatBlue)), + leg4 = Object3D(parent=:plate, translation=[-tableX/2+widthLeg/2, -tableY/2+widthLeg/2, -heigthLeg/2], + feature=Solid(shape=Box(lengthX=widthLeg, lengthY=widthLeg, lengthZ=heigthLeg), solidMaterial="DryWood", visualMaterial=vmatBlue)) +) + +Base = Model( + position = [0.0, 0.0, 0.0], + orientation = [0.0, 0.0, 0.0], + base_frame = Object3D(parent=:world, translation=:position, rotation=:orientation, + feature=Solid(shape=FileMesh(filename = base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=19.803))), + plate = Object3D(parent=:base_frame, translation=[-0.159, 0, 0.046], + feature=Solid(shape=FileMesh(filename = plate_obj), massProperties=MassPropertiesFromShapeAndMass(mass=2.397), contactMaterial="DryWood")), + plateLock = Object3D(parent=:base_frame, + lockable=true, + translation=[-0.19,0.0,0.0702], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + plate_box = Object3D(parent=:base_frame, translation=[-0.16, 0, 0.0702],# 0.06514], + feature=Solid(shape=Box(lengthX=0.22, lengthY=0.22, lengthZ=0.01), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, contactMaterial="DryWood", collision=false)), + front_right_wheel = Object3D(parent=:base_frame, translation=[0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + front_left_wheel = Object3D(parent=:base_frame, translation=[0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = front_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_right_wheel = Object3D(parent=:base_frame, translation=[-0.228, -0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_right_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))), + back_left_wheel = Object3D(parent=:base_frame, translation=[-0.228, 0.158, -0.034], + feature=Solid(shape=FileMesh(filename = back_left_wheel_obj), massProperties=MassPropertiesFromShapeAndMass(mass=1.4))) +) + +servoParameters1 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio1), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia1), + idealGear = Map(ratio=gearRatio1) + ) + ) + +servoParameters2 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio2), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia2), + idealGear = Map(ratio=gearRatio2) + ) + ) + +servoParameters3 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio3), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia3), + idealGear = Map(ratio=gearRatio3) + ) + ) + +servoParameters4 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio4), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia4), + idealGear = Map(ratio=gearRatio4) + ) + ) + +servoParameters5 = Map( + ppi = Map( + gainOuter = Map(k=gearRatio5), + gainInner = Map(k=k1Link), + PI = Map(k=k2Link, T=T2Link) + ), + drive = Map( + motorInertia = Map(J=motorInertia5), + idealGear = Map(ratio=gearRatio5) + ) + ) + +servoParameters6 = Map( + ppi = Map( + gainOuter = Map(k=gearRatioGripper), + gainInner = Map(k=k1Gripper), + PI = Map(k=k2Gripper, T=T2Gripper) + ), + drive = Map( + motorMass = Map(m=motorInertiaGripper) + ) + ) + +featureBody1 = Solid(shape = FileMesh(filename = arm_joint_1_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m1)) +featureBody2 = Solid(shape = FileMesh(filename = arm_joint_2_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m2)) +featureBody3 = Solid(shape = FileMesh(filename = arm_joint_3_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m3)) +featureBody4 = Solid(shape = FileMesh(filename = arm_joint_4_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m4)) +featureBody5 = Solid(shape = FileMesh(filename = arm_joint_5_obj), massProperties = MassPropertiesFromShapeAndMass(mass=m5)) + +linkParameters1 = Map( + parent1 = Par(value = :(armBase_b)), + featureBody = featureBody1, + initRefPos = initPosition[1], + trans = translation1, + rota = Par(value = :(rotation1)) +) + +linkParameters2 = Map( + parent1 = Par(value = :(link1.obj2)), + featureBody = featureBody2, + m = m2, + initRefPos = initPosition[2], + trans = translation2, + rota = Par(value = :(rotation2)) +) + +linkParameters3 = Map( + parent1 = Par(value = :(link2.obj2)), + featureBody = featureBody3, + m = m3, + initRefPos = initPosition[3], + trans = translation3, + rota = Par(value = :(rotation3)) +) + +linkParameters4 = Map( + parent1 = Par(value = :(link3.obj2)), + featureBody = featureBody4, + m = m4, + initRefPos = initPosition[4], + trans = translation4, + rota = Par(value = :(nullRot)) +) + +linkParameters5 = Map( + parent1 = Par(value = :(link4.obj2)), + featureBody = featureBody5, + m = m5, + initRefPos = initPosition[5], + trans = translation5, + rota = Par(value = :(rotation5)) +) + +Link = Model( + parent1 = Par(value = :(nothing)), + featureBody = featureBody1, + trans = [0,0,0], + rota = Par(value = :(nullRot)), + + obj1 = Object3D(parent=:parent1, rotation=:rota), + body = Object3D(feature = :featureBody), + obj2 = Object3D(parent =:body, translation = :trans), +) + +MyGripper = Model( + obj1 = Object3D(parent=:(link5.obj2), rotation=[0, 0, -180u"°"]), + gripper_base_frame = Object3D(parent=:obj1, feature=Solid(shape= + FileMesh(filename = gripper_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.199))), + gripper_left_finger_a = Object3D(), + gripper_left_finger = Object3D(parent=:gripper_left_finger_a, translation=[0, 0.0082, 0], + feature=Solid(shape= FileMesh(filename = gripper_left_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), contactMaterial="DryWood")), + gripper_left_finger_box = Object3D(parent=:gripper_left_finger_a, translation=[0.0, 0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, contactMaterial="DryWood", collision=false)), + gripper_right_finger_a = Object3D(parent=:gripper_base_frame, + translation=[0,-rightFingerOffset,0]), + gripper_right_finger = Object3D(parent=:gripper_right_finger_a, translation=[0, -0.0082, 0], + feature = Solid(shape = FileMesh(filename = gripper_right_finger_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.010), contactMaterial="DryWood")), + gripper_right_finger_box = Object3D(parent=:gripper_right_finger_a, translation=[0.0, -0.0051, 0.0135], + feature = Solid(shape=Box(lengthX=0.012, lengthY=0.01, lengthZ=0.045), massProperties=MassPropertiesFromShapeAndMass(mass=1.0e-9), visualMaterial=vmatInvisible, contactMaterial="DryWood")), + gripperLock = Object3D(parent=:gripper_right_finger_a, + lockable=true, + translation=[0.012/2,0.01/2,0.045/2], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), +) + +YouBot(worldName;pathIndexOffset) = Model( + pathIndexOffset2 = pathIndexOffset, + base = Base, + worldName = Par(worldName), + arm_base_frame = Object3D(parent=:(base.base_frame), + translation=[0.143, 0.0, 0.046], + feature = Solid(shape = FileMesh(filename = arm_base_frame_obj), massProperties=MassPropertiesFromShapeAndMass(mass=0.961))), + armBase_b = Object3D(parent=:arm_base_frame, translation=[0.024, 0, 0.115]), + + link1 = Link | linkParameters1, + link2 = Link | linkParameters2, + link3 = Link | linkParameters3, + link4 = Link | linkParameters4, + link5 = Link | linkParameters5, + gripper = MyGripper, + + rev1 = RevoluteWithFlange(obj1 = :(link1.obj1), obj2 = :(link1.body), + axis=axisLink, phi = Var(init = initPosition[1+pathIndexOffset])), + rev2 = RevoluteWithFlange(obj1 = :(link2.obj1), obj2 = :(link2.body), + axis=axisLink, phi = Var(init = initPosition[2+pathIndexOffset])), + rev3 = RevoluteWithFlange(obj1 = :(link3.obj1), obj2 = :(link3.body), + axis=axisLink, phi = Var(init = initPosition[3+pathIndexOffset])), + rev4 = RevoluteWithFlange(obj1 = :(link4.obj1), obj2 = :(link4.body), + axis=axisLink, phi = Var(init = initPosition[4+pathIndexOffset])), + rev5 = RevoluteWithFlange(obj1 = :(link5.obj1), obj2 = :(link5.body), + axis=axisLink, phi = Var(init = initPosition[5+pathIndexOffset])), + + prism = PrismaticWithFlange(obj1 = :(gripper.gripper_right_finger_a), obj2 = :(gripper.gripper_left_finger_a), + axis=axisGripper, s = Var(init = initPosition[6+pathIndexOffset]) ), + + servo1 = Servo, + servo2 = Servo, + servo3 = Servo, + servo4 = Servo, + servo5 = Servo, + servo6 = ServoTrans, + + modelActions = ModelActions(world=:world, actions=robotProgram), + currentAction = Var(hideResult=true), + + equations=:[ + currentAction = executeActions(modelActions), + servo1.refLoadAngle = getRefPathPosition(currentAction, 1+pathIndexOffset2), + servo2.refLoadAngle = getRefPathPosition(currentAction, 2+pathIndexOffset2), + servo3.refLoadAngle = getRefPathPosition(currentAction, 3+pathIndexOffset2), + servo4.refLoadAngle = getRefPathPosition(currentAction, 4+pathIndexOffset2), + servo5.refLoadAngle = getRefPathPosition(currentAction, 5+pathIndexOffset2), + servo6.refLoadPos = getRefPathPosition(currentAction, 6+pathIndexOffset2) + ], + + connect = :[ + (servo1.flange, rev1.flange) + (servo2.flange, rev2.flange) + (servo3.flange, rev3.flange) + (servo4.flange, rev4.flange) + (servo5.flange, rev5.flange) + (servo6.flange, prism.flange) + ] +) + +Scenario = Model3D( + gravField = UniformGravityField(g=9.81, n=[0,0,-1]), + world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, + enableContactDetection=false, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), + + # table = Table, + ground = Ground, + + sphere = Object3D(parent=:world, fixedToParent=false, + assemblyRoot=true, + translation=[-0.78, 0.0, 0.1845], #[-0.78, 0.0, 0.1792], + feature=Solid(shape=Sphere(diameter=diameter), + visualMaterial=VisualMaterial(color = "Green"), + solidMaterial="DryWood", + collision=false)), + sphereLock = Object3D(parent=:sphere, + lockable=true, + translation=[0.0,diameter/2,0.0], + feature=Visual(shape=Sphere(diameter=diameterLock)) ), + youbot1 = YouBot("world", pathIndexOffset=0) +) + +modelParameters = Map( + youbot1 = Map( + base = Map( + orientation = [0.0, 0.0, 0.0], + position = [-0.569, 0.0, 0.084], + ), + servo1 = servoParameters1, + servo2 = servoParameters2, + servo3 = servoParameters3, + servo4 = servoParameters4, + servo5 = servoParameters5, + servo6 = servoParameters6 + ) +) + + +youbotModel = Scenario | modelParameters + +youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) + +stopTime = 13.5 +tolerance = 1e-7 +# use boxes instead of FileMesh for better performance +requiredFinalStates = [3.1415949391160716, 2.336082293991764e-6, 0.06413133541403294, -0.38051894214017756, 1.5066658708779075, 0.3805177476718944, 0.08493921383177153, -0.5038822885155388, -6.703045591592847e-8, 4.3316969172557847e-8, 0.023983617410734844, -0.05999191143397139, -0.3892762572912395, -0.1403675210062752, 0.001062006988854881, 2.9199925186852378e-5, 0.048000595687271566, -5.954980731236771e-7] + +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) + + +# @usingModiaPlot +# plot(youbot, ["sphere.translation", +# ("youbot1.rev1.phi", +# "youbot1.rev2.phi", +# "youbot1.rev3.phi", +# "youbot1.rev4.phi", +# "youbot1.rev5.phi")], figure=1) + +# plot(youbot, [ "sphere.r_abs[1]", +# "sphere.r_abs[2]", +# "sphere.r_abs[3]"], figure=2) +end From 173d7bc285f75c529dd9e854492111a1fca87d82 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Wed, 21 Jun 2023 09:44:13 +0200 Subject: [PATCH 07/15] modified files --- test/Robot/ScenarioCollisionOnly.jl | 27 +++++++++++++------ .../ScenarioSegmentedCollisionOff.jl | 18 ++++++++----- .../Segmented/ScenarioSegmentedCollisionOn.jl | 20 +++++++------- test/Segmented/ScenarioSegmentedOnly.jl | 18 ++++++++----- test/Segmented/YouBotFixSphere.jl | 2 +- 5 files changed, 53 insertions(+), 32 deletions(-) diff --git a/test/Robot/ScenarioCollisionOnly.jl b/test/Robot/ScenarioCollisionOnly.jl index bf52cc1..333dea7 100644 --- a/test/Robot/ScenarioCollisionOnly.jl +++ b/test/Robot/ScenarioCollisionOnly.jl @@ -498,7 +498,10 @@ YouBot(worldName;pathIndexOffset) = Model( Scenario = Model3D( gravField = UniformGravityField(g=9.81, n=[0,0,-1]), world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, - enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), + enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), + + worldFrame = Object3D(parent=:world, + feature=Visual(shape=CoordinateSystem(length=1.0))), # table = Table, ground = Ground, @@ -548,15 +551,23 @@ end simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) @usingModiaPlot -plot(youbot, ["sphere.translation", - ("youbot1.rev1.phi", - "youbot1.rev2.phi", - "youbot1.rev3.phi", - "youbot1.rev4.phi", - "youbot1.rev5.phi")], figure=1) +plot(youbot, [ "sphere.translation[1]", +"sphere.translation[2]", +"sphere.translation[3]"], reuse=true, prefix="S1: ", figure=1) plot(youbot, [ "sphere.r_abs[1]", "sphere.r_abs[2]", - "sphere.r_abs[3]"], figure=2) + "sphere.r_abs[3]"], reuse=true, prefix="S1: ", figure=2) + + +# plot(youbot, ["sphere.translation", +# # ("youbot1.rev1.phi", +# # "youbot1.rev2.phi", +# # "youbot1.rev3.phi", +# # "youbot1.rev4.phi", +# # "youbot1.rev5.phi") +# ], figure=1) + + end diff --git a/test/Segmented/ScenarioSegmentedCollisionOff.jl b/test/Segmented/ScenarioSegmentedCollisionOff.jl index bf743b5..f6d85cb 100644 --- a/test/Segmented/ScenarioSegmentedCollisionOff.jl +++ b/test/Segmented/ScenarioSegmentedCollisionOff.jl @@ -576,15 +576,19 @@ simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_at # showInfo(youbot) @usingModiaPlot -plot(youbot, ["sphere.translation", - ("youbot1.rev1.phi", - "youbot1.rev2.phi", - "youbot1.rev3.phi", - "youbot1.rev4.phi", - "youbot1.rev5.phi")], figure=1) +# plot(youbot, ["sphere.translation", +# ("youbot1.rev1.phi", +# "youbot1.rev2.phi", +# "youbot1.rev3.phi", +# "youbot1.rev4.phi", +# "youbot1.rev5.phi")], figure=1) + +plot(youbot, [ "sphere.translation[1]", +"sphere.translation[2]", +"sphere.translation[3]"], reuse=true, prefix="S3: ", figure=1) plot(youbot, [ "sphere.r_abs[1]", "sphere.r_abs[2]", - "sphere.r_abs[3]"], figure=2) + "sphere.r_abs[3]"], reuse=true, prefix="S3: ", figure=2) end diff --git a/test/Segmented/ScenarioSegmentedCollisionOn.jl b/test/Segmented/ScenarioSegmentedCollisionOn.jl index 2fc5379..37a22f4 100644 --- a/test/Segmented/ScenarioSegmentedCollisionOn.jl +++ b/test/Segmented/ScenarioSegmentedCollisionOn.jl @@ -145,8 +145,6 @@ function robotProgram(robotActions) ptpJointSpace(robotActions, [ pi 0.0 pi/2 0.0 0.0 diameter-0.002; # grip + top ]) - - return nothing end @@ -576,15 +574,19 @@ simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_at # showInfo(youbot) @usingModiaPlot -plot(youbot, ["sphere.translation", - ("youbot1.rev1.phi", - "youbot1.rev2.phi", - "youbot1.rev3.phi", - "youbot1.rev4.phi", - "youbot1.rev5.phi")], figure=1) +# plot(youbot, ["sphere.translation", +# ("youbot1.rev1.phi", +# "youbot1.rev2.phi", +# "youbot1.rev3.phi", +# "youbot1.rev4.phi", +# "youbot1.rev5.phi")], figure=1) + +plot(youbot, [ "sphere.translation[1]", +"sphere.translation[2]", +"sphere.translation[3]"], reuse=true, prefix="S2: ", figure=1) plot(youbot, [ "sphere.r_abs[1]", "sphere.r_abs[2]", - "sphere.r_abs[3]"], figure=2) + "sphere.r_abs[3]"], reuse=true, prefix="S2: ", figure=2) end diff --git a/test/Segmented/ScenarioSegmentedOnly.jl b/test/Segmented/ScenarioSegmentedOnly.jl index bb6773c..15d8e34 100644 --- a/test/Segmented/ScenarioSegmentedOnly.jl +++ b/test/Segmented/ScenarioSegmentedOnly.jl @@ -99,10 +99,9 @@ function robotProgram(robotActions) v_max = [2.68512, 2.68512, 4.8879, 5.8997, 5.8997, 2.0], a_max = [1.5, 1.5, 1.5, 1.5, 1.5, 0.5]) - ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock",waitingPeriod=0.0) + ActionAttach(robotActions, "sphereLock", "youbot1.base.plateLock",waitingPeriod=0.0) EventAfterPeriod(robotActions, 1e-10) - ptpJointSpace(robotActions, [ pi pi/4 pi/4 0.0 0.0 diameter+0.01; # start top of ball pi pi/4 pi/4 1.04 0.0 diameter+0.01; # go to plate @@ -572,15 +571,20 @@ requiredFinalStates = [3.1415949391160716, 2.336082293991764e-6, 0.0641313354140 simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) -# @usingModiaPlot +@usingModiaPlot # plot(youbot, ["sphere.translation", # ("youbot1.rev1.phi", # "youbot1.rev2.phi", # "youbot1.rev3.phi", # "youbot1.rev4.phi", -# "youbot1.rev5.phi")], figure=1) +# "youbot1.rev5.phi") +# ], figure=1) + +plot(youbot, [ "sphere.translation[1]", +"sphere.translation[2]", +"sphere.translation[3]"], reuse=true, prefix="S4: ", figure=1) -# plot(youbot, [ "sphere.r_abs[1]", -# "sphere.r_abs[2]", -# "sphere.r_abs[3]"], figure=2) +plot(youbot, [ "sphere.r_abs[1]", + "sphere.r_abs[2]", + "sphere.r_abs[3]"], reuse=true, prefix="S4: ", figure=2) end diff --git a/test/Segmented/YouBotFixSphere.jl b/test/Segmented/YouBotFixSphere.jl index 74c508b..c0a6a31 100644 --- a/test/Segmented/YouBotFixSphere.jl +++ b/test/Segmented/YouBotFixSphere.jl @@ -522,7 +522,7 @@ YouBot(worldName;pathIndexOffset) = Model( Scenario = Model3D( gravField = UniformGravityField(g=9.81, n=[0,0,-1]), world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, - enableContactDetection=false, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=false)), + enableContactDetection=false, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), table = Table, ground = Ground, From f833dae87725a7075209b203cad86811ecfc0a17 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Fri, 3 Nov 2023 14:35:39 +0100 Subject: [PATCH 08/15] use updateVisuElements again --- src/Composition/dynamics.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index db65caa..2186e7d 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -205,8 +205,8 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode # objIndices[i,1]: Index of r_abs of Object3D i # [i,2]: Index of R_abs of Object3D i - objIndices = Matrix{Int}(undef, length(scene.allVisuElements), 2) - for (i,obj) in enumerate(scene.allVisuElements) + objIndices = Matrix{Int}(undef, length(scene.updateVisuElements), 2) + for (i,obj) in enumerate(scene.updateVisuElements) if typeof(obj.feature) == Modia3D.Composition.EmptyObject3DFeature objIndices[i,1] = 0 objIndices[i,2] = 0 @@ -561,7 +561,7 @@ function computeGeneralizedForces!(mbs::MultibodyData{F,TimeType}, qdd_hidden::V # objects can have interactionManner (need to rename updateVisuElements) if scene.options.useOptimizedStructure objIndices = mbs.objIndices - for (i,obj) in enumerate(scene.allVisuElements) + for (i,obj) in enumerate(scene.updateVisuElements) parent = obj.parent obj.r_abs = obj.r_rel ≡ Modia3D.ZeroVector3D(F) ? parent.r_abs : parent.r_abs + parent.R_abs'*obj.r_rel obj.R_abs = obj.R_rel ≡ Modia3D.NullRotation(F) ? parent.R_abs : obj.R_rel*parent.R_abs From ceed5971e0db233928e4b9ea9ba91bb24382999e Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Fri, 3 Nov 2023 14:36:34 +0100 Subject: [PATCH 09/15] finalize test models --- test/Robot/ScenarioCollisionOnly.jl | 15 +++++++------- .../ScenarioSegmentedCollisionOff.jl | 20 +++++++++---------- .../Segmented/ScenarioSegmentedCollisionOn.jl | 18 ++++++++--------- test/Segmented/ScenarioSegmentedOnly.jl | 4 ++-- test/includeTests.jl | 4 ++++ 5 files changed, 33 insertions(+), 28 deletions(-) diff --git a/test/Robot/ScenarioCollisionOnly.jl b/test/Robot/ScenarioCollisionOnly.jl index 333dea7..e5994d1 100644 --- a/test/Robot/ScenarioCollisionOnly.jl +++ b/test/Robot/ScenarioCollisionOnly.jl @@ -500,8 +500,8 @@ Scenario = Model3D( world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), - worldFrame = Object3D(parent=:world, - feature=Visual(shape=CoordinateSystem(length=1.0))), + # worldFrame = Object3D(parent=:world, + # feature=Visual(shape=CoordinateSystem(length=1.0))), # table = Table, ground = Ground, @@ -540,7 +540,8 @@ stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better performance if Sys.iswindows() - requiredFinalStates = [3.1415947353156977, 2.4404618792441014e-6, 0.06413101847025342, -0.38051837261409877, 1.5066656031419667, 0.38051766101104056, 0.08493817930077076, -0.5038810237109801, -4.6660513554873775e-7, 3.752739122664999e-7, 0.02640050697959681, -0.05767990246020697, -0.38561153991690283, -0.13530700791021252, 0.003352929911771347, -0.1332188913735816, 0.04978725220079851, -6.996407090545839e-6, -0.7775608475001248, 0.005107163857175746, 0.38015816381775924, 0.04903299043458591, 4.3039218890990144e-6, 0.10223649086921456, -0.23251977356265563, 0.5671780375371024, 6.409469280850812, 5.535728438671193e-7, 0.5044004851046952, -2.473504004121177e-6] + requiredFinalStates = [3.1415918452296103, 3.0247029956502424e-7, 0.6805918807359593, -0.09565685952807047, 0.7909091454335595, 0.10951995771372612, 1.0327017770452882, -0.14501110594130628, -3.613403077522777e-7, 1.7722424221528134e-7, 0.02892581810809196, -0.07918395248553552, -0.36125810000007835, -0.10904804871112947, 0.001897921781670574, -0.06875145688237284, 0.049788789706814565, -4.140738295574155e-6, -0.7842801128047748, + 0.005105300046456846, 0.21862283801441873, -0.009658480717735566, 2.1757108400615252e-6, 0.021108086393876627, -0.1984193860002232, 0.11865862428464358, 0.02379631583255704, -3.096054388354263e-7, 0.1304890475949812, -4.4594322363919364e-7] elseif Sys.isapple() requiredFinalStates = missing else @@ -548,16 +549,16 @@ else end -simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) @usingModiaPlot plot(youbot, [ "sphere.translation[1]", "sphere.translation[2]", "sphere.translation[3]"], reuse=true, prefix="S1: ", figure=1) -plot(youbot, [ "sphere.r_abs[1]", - "sphere.r_abs[2]", - "sphere.r_abs[3]"], reuse=true, prefix="S1: ", figure=2) +# plot(youbot, [ "sphere.r_abs[1]", +# "sphere.r_abs[2]", +# "sphere.r_abs[3]"], reuse=true, prefix="S1: ", figure=2) # plot(youbot, ["sphere.translation", diff --git a/test/Segmented/ScenarioSegmentedCollisionOff.jl b/test/Segmented/ScenarioSegmentedCollisionOff.jl index f6d85cb..e8d088c 100644 --- a/test/Segmented/ScenarioSegmentedCollisionOff.jl +++ b/test/Segmented/ScenarioSegmentedCollisionOff.jl @@ -527,7 +527,7 @@ Scenario = Model3D( gravField = UniformGravityField(g=9.81, n=[0,0,-1]), world = Object3D(feature=Scene(gravityField=:gravField, visualizeFrames=false, nominalLength=tableX, gap=0.2, enableContactDetection=true, maximumContactDamping=1000, elasticContactReductionFactor=1e-3, enableVisualization=true)), #, animationFile="YouBotDynamicState.json")), -# worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.2))), + # worldFrame = Object3D(parent=:world, feature=Visual(shape=CoordinateSystem(length=0.2))), ground = Ground, @@ -569,13 +569,13 @@ youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better collision performance -requiredFinalStates = [3.1415926313235216, 2.227704607451224e-8, -2.8441240591542057e-7, 2.844479623947795e-7, -6.300618493694368e-7, 6.301438745162919e-7, -3.8367060940761664e-7, 3.8373222528068616e-7, 2.9773366458646674e-9, -2.9778176373587e-9, -0.00017011927750574103, 0.2394616012878047, -0.003881746258204947, -0.002274156117355221, 1.0356446209926906e-5, -8.985047184676908e-11, 0.047999999998151396, 1.8748871907289576e-12, -0.7616251395986522, 0.00025213516122149976, 0.18419327722973222, 1.2698578611388089e-9, 2.2444158136065587e-12, -1.772988910724902e-11, 3.141474457755385, 1.0187988505971024, 3.141041134037263, -8.150376270957884e-11, 4.6108853365504784e-8, 8.826370248608988e-22] +requiredFinalStates = [3.1415921741564143, 1.1955106283153446e-7, 0.6805919652838498, -0.09565658114367705, 0.7909092176133838, 0.10951940810027615, 1.0327020647791874, -0.145010847265314, 2.259247708251684e-7, -1.9741430416860724e-7, 0.028546003581824412, -0.07925467651812873, -0.3613840032918982, -0.10919851158919243, 0.0016270711594961662, -0.04583765473394904, 0.047072862725141544, 0.0014855277166491237] -simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) # showInfo(youbot) -@usingModiaPlot +# @usingModiaPlot # plot(youbot, ["sphere.translation", # ("youbot1.rev1.phi", # "youbot1.rev2.phi", @@ -583,12 +583,12 @@ simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_at # "youbot1.rev4.phi", # "youbot1.rev5.phi")], figure=1) -plot(youbot, [ "sphere.translation[1]", -"sphere.translation[2]", -"sphere.translation[3]"], reuse=true, prefix="S3: ", figure=1) +# plot(youbot, [ "sphere.translation[1]", +# "sphere.translation[2]", +# "sphere.translation[3]"], reuse=true, prefix="S3: ", figure=1) -plot(youbot, [ "sphere.r_abs[1]", - "sphere.r_abs[2]", - "sphere.r_abs[3]"], reuse=true, prefix="S3: ", figure=2) +# plot(youbot, [ "sphere.r_abs[1]", +# "sphere.r_abs[2]", +# "sphere.r_abs[3]"], reuse=true, prefix="S3: ", figure=2) end diff --git a/test/Segmented/ScenarioSegmentedCollisionOn.jl b/test/Segmented/ScenarioSegmentedCollisionOn.jl index 37a22f4..91e89d7 100644 --- a/test/Segmented/ScenarioSegmentedCollisionOn.jl +++ b/test/Segmented/ScenarioSegmentedCollisionOn.jl @@ -567,13 +567,13 @@ youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better collision performance -requiredFinalStates = [3.1415926313235216, 2.227704607451224e-8, -2.8441240591542057e-7, 2.844479623947795e-7, -6.300618493694368e-7, 6.301438745162919e-7, -3.8367060940761664e-7, 3.8373222528068616e-7, 2.9773366458646674e-9, -2.9778176373587e-9, -0.00017011927750574103, 0.2394616012878047, -0.003881746258204947, -0.002274156117355221, 1.0356446209926906e-5, -8.985047184676908e-11, 0.047999999998151396, 1.8748871907289576e-12, -0.7616251395986522, 0.00025213516122149976, 0.18419327722973222, 1.2698578611388089e-9, 2.2444158136065587e-12, -1.772988910724902e-11, 3.141474457755385, 1.0187988505971024, 3.141041134037263, -8.150376270957884e-11, 4.6108853365504784e-8, 8.826370248608988e-22] +requiredFinalStates = [3.141591818383938, 1.0335725977173708e-6, 0.6805919657593686, -0.09565660198844163, 0.7909092191050598, 0.10951941774642412, 1.0327020645468628, -0.14501085658793922, 2.186064148392944e-7, -1.879592613709152e-7, 0.02883998021248762, -0.07925163267233755, -0.36137151574752563, -0.1091942688913675, 0.0016238609193932954, -0.06877780254544075, 0.04978954142146488, -2.2444357924892364e-6] -simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) # showInfo(youbot) -@usingModiaPlot +# @usingModiaPlot # plot(youbot, ["sphere.translation", # ("youbot1.rev1.phi", # "youbot1.rev2.phi", @@ -581,12 +581,12 @@ simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_at # "youbot1.rev4.phi", # "youbot1.rev5.phi")], figure=1) -plot(youbot, [ "sphere.translation[1]", -"sphere.translation[2]", -"sphere.translation[3]"], reuse=true, prefix="S2: ", figure=1) +# plot(youbot, [ "sphere.translation[1]", +# "sphere.translation[2]", +# "sphere.translation[3]"], reuse=true, prefix="S2: ", figure=1) -plot(youbot, [ "sphere.r_abs[1]", - "sphere.r_abs[2]", - "sphere.r_abs[3]"], reuse=true, prefix="S2: ", figure=2) +# plot(youbot, [ "sphere.r_abs[1]", +# "sphere.r_abs[2]", +# "sphere.r_abs[3]"], reuse=true, prefix="S2: ", figure=2) end diff --git a/test/Segmented/ScenarioSegmentedOnly.jl b/test/Segmented/ScenarioSegmentedOnly.jl index 15d8e34..ebaa772 100644 --- a/test/Segmented/ScenarioSegmentedOnly.jl +++ b/test/Segmented/ScenarioSegmentedOnly.jl @@ -566,9 +566,9 @@ youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better performance -requiredFinalStates = [3.1415949391160716, 2.336082293991764e-6, 0.06413133541403294, -0.38051894214017756, 1.5066658708779075, 0.3805177476718944, 0.08493921383177153, -0.5038822885155388, -6.703045591592847e-8, 4.3316969172557847e-8, 0.023983617410734844, -0.05999191143397139, -0.3892762572912395, -0.1403675210062752, 0.001062006988854881, 2.9199925186852378e-5, 0.048000595687271566, -5.954980731236771e-7] +requiredFinalStates = [3.1415920364553855, 2.7428136813418614e-7, 0.6805920818714688, -0.09565666285888169, 0.790909491205814, 0.10951932389708785, 1.0327026276867144, -0.14501108875418495, 2.273931612905906e-8, -6.523005228871711e-8, 0.027854397659264934, -0.0802102064050292, -0.3629121280113591, -0.11128538667241196, 0.0008842091487311585, 6.0035452726848865e-5, 0.04800122496867457, -1.227967289312977e-6] -simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=missing, logEvents=false) +simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) @usingModiaPlot diff --git a/test/includeTests.jl b/test/includeTests.jl index b463b9f..d0840fe 100644 --- a/test/includeTests.jl +++ b/test/includeTests.jl @@ -61,6 +61,7 @@ Test.@testset "Robot" begin Test.@test_broken include(joinpath("Robot", "YouBotWithSphere.jl")) # LinearAlgebra.SingularException include(joinpath("Robot", "YouBotGripping.jl")) include(joinpath("Robot", "YouBotSphereTransport.jl")) + include(joinpath("Robot", "ScenarioCollisionOnly.jl")) end if testsExtend == completeTests include(joinpath("Robot", "YouBotPingPong.jl")) # long computation time @@ -105,6 +106,9 @@ Test.@testset "Segmented" begin include(joinpath("Segmented", "YouBotDynamicState.jl")) include(joinpath("Segmented", "YouBotFixBox.jl")) include(joinpath("Segmented", "YouBotFixSphere.jl")) + include(joinpath("Segmented", "ScenarioSegmentedCollisionOff.jl")) + include(joinpath("Segmented", "ScenarioSegmentedCollisionOn.jl")) + include(joinpath("Segmented", "ScenarioSegmentedOnly.jl")) end end From 864ffeb74a1f6c88d6b2a857aab8a3a08dd0474f Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Fri, 3 Nov 2023 14:54:36 +0100 Subject: [PATCH 10/15] little changes --- src/Composition/dynamics.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index 2186e7d..02d676a 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -153,7 +153,7 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode scene.collide = scene.gripPair.enableContactDetection end else - @error("Andrea: print warning für gripping features") + @error("Print warning für gripping features") # printWarnGrip(robotOrDepot, movableObj, waitingPeriod) end end From b347d19d89d6b04db591d234234df1c48288ef84 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 6 Nov 2023 09:16:54 +0100 Subject: [PATCH 11/15] remove error message --- src/Composition/dynamics.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Composition/dynamics.jl b/src/Composition/dynamics.jl index 02d676a..32dad67 100644 --- a/src/Composition/dynamics.jl +++ b/src/Composition/dynamics.jl @@ -154,7 +154,6 @@ function initSegment_Model3D!(partiallyInstantiatedModel::Modia.InstantiatedMode end else @error("Print warning für gripping features") - # printWarnGrip(robotOrDepot, movableObj, waitingPeriod) end end From 90fb1cfbacceb255713118c56882400c6f251af4 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 6 Nov 2023 14:33:36 +0100 Subject: [PATCH 12/15] update final results --- test/Robot/ScenarioCollisionOnly.jl | 11 +++++------ test/Segmented/ScenarioSegmentedCollisionOff.jl | 2 +- test/Segmented/ScenarioSegmentedCollisionOn.jl | 2 +- test/Segmented/ScenarioSegmentedOnly.jl | 16 ++++++++-------- 4 files changed, 15 insertions(+), 16 deletions(-) diff --git a/test/Robot/ScenarioCollisionOnly.jl b/test/Robot/ScenarioCollisionOnly.jl index e5994d1..72ec92d 100644 --- a/test/Robot/ScenarioCollisionOnly.jl +++ b/test/Robot/ScenarioCollisionOnly.jl @@ -540,8 +540,7 @@ stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better performance if Sys.iswindows() - requiredFinalStates = [3.1415918452296103, 3.0247029956502424e-7, 0.6805918807359593, -0.09565685952807047, 0.7909091454335595, 0.10951995771372612, 1.0327017770452882, -0.14501110594130628, -3.613403077522777e-7, 1.7722424221528134e-7, 0.02892581810809196, -0.07918395248553552, -0.36125810000007835, -0.10904804871112947, 0.001897921781670574, -0.06875145688237284, 0.049788789706814565, -4.140738295574155e-6, -0.7842801128047748, - 0.005105300046456846, 0.21862283801441873, -0.009658480717735566, 2.1757108400615252e-6, 0.021108086393876627, -0.1984193860002232, 0.11865862428464358, 0.02379631583255704, -3.096054388354263e-7, 0.1304890475949812, -4.4594322363919364e-7] + requiredFinalStates = [3.141592656083914, 3.2955210650043794e-9, 0.6805942885295763, -0.09565767441181468, 0.7909095511209971, 0.10951974347893331, 1.032702582672235, -0.14501159330773356, 4.843043035569045e-8, -5.084066106605599e-8, 9.463525418080984e-8, -0.4161958225397787, -0.3634865106392557, -0.11218698258440177, 0.00028351378391616364, -0.0687518903928338, 0.04978878983770011, -4.140566109957699e-6, -0.7842800497816398, 0.005105609550611603, 0.21862295671355367, -0.0096585351122136, 2.0689584764943053e-6, 0.02110845828012153, -0.19785528209242548, 0.1186760937422613, 0.02372051044718875, -6.533396584593705e-8, 0.13049051323039967, 4.26642489125089e-8] elseif Sys.isapple() requiredFinalStates = missing else @@ -551,10 +550,10 @@ end simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) -@usingModiaPlot -plot(youbot, [ "sphere.translation[1]", -"sphere.translation[2]", -"sphere.translation[3]"], reuse=true, prefix="S1: ", figure=1) +# @usingModiaPlot +# plot(youbot, [ "sphere.translation[1]", +# "sphere.translation[2]", +# "sphere.translation[3]"], reuse=true, prefix="S1: ", figure=1) # plot(youbot, [ "sphere.r_abs[1]", # "sphere.r_abs[2]", diff --git a/test/Segmented/ScenarioSegmentedCollisionOff.jl b/test/Segmented/ScenarioSegmentedCollisionOff.jl index e8d088c..e76841f 100644 --- a/test/Segmented/ScenarioSegmentedCollisionOff.jl +++ b/test/Segmented/ScenarioSegmentedCollisionOff.jl @@ -569,7 +569,7 @@ youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better collision performance -requiredFinalStates = [3.1415921741564143, 1.1955106283153446e-7, 0.6805919652838498, -0.09565658114367705, 0.7909092176133838, 0.10951940810027615, 1.0327020647791874, -0.145010847265314, 2.259247708251684e-7, -1.9741430416860724e-7, 0.028546003581824412, -0.07925467651812873, -0.3613840032918982, -0.10919851158919243, 0.0016270711594961662, -0.04583765473394904, 0.047072862725141544, 0.0014855277166491237] +requiredFinalStates = [3.1415926570363713, -1.3013458081419306e-7, 0.6805942873976024, -0.09565764991241311, 0.7909095517856483, 0.10951972995434828, 1.032702591771644, -0.14501158670555986, 5.492377676162606e-8, -5.981922191949395e-8, 7.757643552277316e-6, -0.4162469942033338, -0.3635507982892172, -0.11225051653570566, 0.0002829193229775221, -0.045836985439091864, 0.04707287716612709, 0.0014854402731197453] simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) diff --git a/test/Segmented/ScenarioSegmentedCollisionOn.jl b/test/Segmented/ScenarioSegmentedCollisionOn.jl index 91e89d7..fa69632 100644 --- a/test/Segmented/ScenarioSegmentedCollisionOn.jl +++ b/test/Segmented/ScenarioSegmentedCollisionOn.jl @@ -567,7 +567,7 @@ youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better collision performance -requiredFinalStates = [3.141591818383938, 1.0335725977173708e-6, 0.6805919657593686, -0.09565660198844163, 0.7909092191050598, 0.10951941774642412, 1.0327020645468628, -0.14501085658793922, 2.186064148392944e-7, -1.879592613709152e-7, 0.02883998021248762, -0.07925163267233755, -0.36137151574752563, -0.1091942688913675, 0.0016238609193932954, -0.06877780254544075, 0.04978954142146488, -2.2444357924892364e-6] +requiredFinalStates = [3.1415926561937666, 3.180307603613472e-9, 0.6805942902151595, -0.09565769212815624, 0.7909095547559756, 0.10951974466588697, 1.0327025949882498, -0.14501161390073966, 4.834365439536424e-8, -5.083589168175532e-8, 9.482986225093542e-7, -0.41623125840706904, -0.3635339629899328, -0.11224029745945692, 0.000283221896408894, -0.06877832213561617, 0.04978954930374486, -2.2467894719281828e-6] simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) diff --git a/test/Segmented/ScenarioSegmentedOnly.jl b/test/Segmented/ScenarioSegmentedOnly.jl index ebaa772..018b144 100644 --- a/test/Segmented/ScenarioSegmentedOnly.jl +++ b/test/Segmented/ScenarioSegmentedOnly.jl @@ -566,12 +566,12 @@ youbot = @instantiateModel(youbotModel, unitless=true, logCode=false, log=false) stopTime = 13.5 tolerance = 1e-7 # use boxes instead of FileMesh for better performance -requiredFinalStates = [3.1415920364553855, 2.7428136813418614e-7, 0.6805920818714688, -0.09565666285888169, 0.790909491205814, 0.10951932389708785, 1.0327026276867144, -0.14501108875418495, 2.273931612905906e-8, -6.523005228871711e-8, 0.027854397659264934, -0.0802102064050292, -0.3629121280113591, -0.11128538667241196, 0.0008842091487311585, 6.0035452726848865e-5, 0.04800122496867457, -1.227967289312977e-6] +requiredFinalStates = [3.1415926553482936, 3.940308425576264e-9, 0.6805942761756493, -0.0956576899775176, 0.7909095290954901, 0.10951977177943406, 1.0327025673843655, -0.14501159844620148, 1.9286175287484307e-8, -3.283069277920548e-8, -4.1609409172268536e-6, -0.41631100515552766, -0.36361306819749833, -0.1122726475747107, 0.0003657180323440799, 6.003535500656957e-5, 0.04800122492323228, -1.2243878275181126e-6] simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_atol=0.002, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) -@usingModiaPlot +# @usingModiaPlot # plot(youbot, ["sphere.translation", # ("youbot1.rev1.phi", # "youbot1.rev2.phi", @@ -580,11 +580,11 @@ simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_at # "youbot1.rev5.phi") # ], figure=1) -plot(youbot, [ "sphere.translation[1]", -"sphere.translation[2]", -"sphere.translation[3]"], reuse=true, prefix="S4: ", figure=1) +# plot(youbot, [ "sphere.translation[1]", +# "sphere.translation[2]", +# "sphere.translation[3]"], reuse=true, prefix="S4: ", figure=1) -plot(youbot, [ "sphere.r_abs[1]", - "sphere.r_abs[2]", - "sphere.r_abs[3]"], reuse=true, prefix="S4: ", figure=2) +# plot(youbot, [ "sphere.r_abs[1]", +# "sphere.r_abs[2]", +# "sphere.r_abs[3]"], reuse=true, prefix="S4: ", figure=2) end From aa39469a9490ca7dd2827340e4853799414636de Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 6 Nov 2023 15:05:57 +0100 Subject: [PATCH 13/15] update final states ubuntu --- test/Robot/ScenarioCollisionOnly.jl | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/test/Robot/ScenarioCollisionOnly.jl b/test/Robot/ScenarioCollisionOnly.jl index 72ec92d..dae24d2 100644 --- a/test/Robot/ScenarioCollisionOnly.jl +++ b/test/Robot/ScenarioCollisionOnly.jl @@ -544,8 +544,7 @@ if Sys.iswindows() elseif Sys.isapple() requiredFinalStates = missing else - requiredFinalStates = [3.141594735312716, 2.440457544603476e-6, 0.06413101847181349, -0.380518372597451, 1.5066656031498458, 0.3805176609510743, 0.08493817930222113, -0.5038810238174551, -4.666083661539158e-7, 3.7522126012581574e-7, 0.026400488779754015, -0.057679898615238055, -0.3856115354698501, -0.1353070539098794, 0.0033529207102979517, -0.13321886490666132, 0.049787252111476124, -6.996240368087093e-6, -0.7775614019840636, 0.005107163901554179, 0.38015912423821296, 0.04903348978501739, 4.303738169808239e-6, 0.1022367696490673, -0.2408237298542479, 0.6183371828465237, 6.424368692503729, 5.538288842857016e-7, 0.5044004851850659, -2.4739225351517005e-6] -end + requiredFinalStates = [3.1415926560840615, 3.433146484584177e-9, 0.680594288222518, -0.09565765972236899, 0.7909095512391782, 0.10951973678968051, 1.0327025824769411, -0.14501158461595007, 4.8430503980790146e-8, -5.084105210479976e-8, 9.524808086169992e-8, -0.4161959543409354, -0.3634865967325259, -0.11218706237664271, 0.00028351404040694717, -0.06875187949659926, 0.04978878982947555, -4.130665462558622e-6, -0.784280256433898, 0.005105609558166718, 0.21862297071743714, -0.009658532695902744, 2.0584489985929135e-6, 0.02110848366792953, -0.1983447342409089, 0.11866492473473893, 0.023777554594603637, -5.754371782338561e-8, 0.1304905069106358, 3.6548193401444445e-8] simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) From 6ae8da56b9de3ae7270f9f2285e19305d57da1ef Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 6 Nov 2023 15:40:40 +0100 Subject: [PATCH 14/15] YouBotSphereTransport.jl no longer works --- test/includeTests.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/includeTests.jl b/test/includeTests.jl index d0840fe..b8e0e48 100644 --- a/test/includeTests.jl +++ b/test/includeTests.jl @@ -60,7 +60,7 @@ Test.@testset "Robot" begin if testsExtend >= normalTests Test.@test_broken include(joinpath("Robot", "YouBotWithSphere.jl")) # LinearAlgebra.SingularException include(joinpath("Robot", "YouBotGripping.jl")) - include(joinpath("Robot", "YouBotSphereTransport.jl")) + Test.@test_broken include(joinpath("Robot", "YouBotSphereTransport.jl")) include(joinpath("Robot", "ScenarioCollisionOnly.jl")) end if testsExtend == completeTests From 57faff1749e7845cf6c7f254c320245d90e51289 Mon Sep 17 00:00:00 2001 From: Andrea Neumayr Date: Mon, 6 Nov 2023 16:18:11 +0100 Subject: [PATCH 15/15] little changes --- test/Robot/ScenarioCollisionOnly.jl | 2 +- test/includeTests.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/test/Robot/ScenarioCollisionOnly.jl b/test/Robot/ScenarioCollisionOnly.jl index dae24d2..21c55c7 100644 --- a/test/Robot/ScenarioCollisionOnly.jl +++ b/test/Robot/ScenarioCollisionOnly.jl @@ -545,7 +545,7 @@ elseif Sys.isapple() requiredFinalStates = missing else requiredFinalStates = [3.1415926560840615, 3.433146484584177e-9, 0.680594288222518, -0.09565765972236899, 0.7909095512391782, 0.10951973678968051, 1.0327025824769411, -0.14501158461595007, 4.8430503980790146e-8, -5.084105210479976e-8, 9.524808086169992e-8, -0.4161959543409354, -0.3634865967325259, -0.11218706237664271, 0.00028351404040694717, -0.06875187949659926, 0.04978878982947555, -4.130665462558622e-6, -0.784280256433898, 0.005105609558166718, 0.21862297071743714, -0.009658532695902744, 2.0584489985929135e-6, 0.02110848366792953, -0.1983447342409089, 0.11866492473473893, 0.023777554594603637, -5.754371782338561e-8, 0.1304905069106358, 3.6548193401444445e-8] - +end simulate!(youbot, stopTime=stopTime, tolerance=tolerance, requiredFinalStates_rtol=0.01, requiredFinalStates_atol=0.01, log=true, logStates=false, logParameters=false, requiredFinalStates=requiredFinalStates, logEvents=false) diff --git a/test/includeTests.jl b/test/includeTests.jl index b8e0e48..d0840fe 100644 --- a/test/includeTests.jl +++ b/test/includeTests.jl @@ -60,7 +60,7 @@ Test.@testset "Robot" begin if testsExtend >= normalTests Test.@test_broken include(joinpath("Robot", "YouBotWithSphere.jl")) # LinearAlgebra.SingularException include(joinpath("Robot", "YouBotGripping.jl")) - Test.@test_broken include(joinpath("Robot", "YouBotSphereTransport.jl")) + include(joinpath("Robot", "YouBotSphereTransport.jl")) include(joinpath("Robot", "ScenarioCollisionOnly.jl")) end if testsExtend == completeTests