diff --git a/home/hairygael/InMoov2_IK_leftArm.py b/home/hairygael/InMoov2_IK_leftArm.py new file mode 100644 index 00000000..f6eb54bc --- /dev/null +++ b/home/hairygael/InMoov2_IK_leftArm.py @@ -0,0 +1,178 @@ +########################################## +# Example python script to demonstrate +# InverseKinematics for the InMoovArm +# +########################################## +from time import sleep +import org.myrobotlab.framework.Platform as Platform + + +###################### +# work around until the race conditions in MRL are fixed. +###################### +def resetArm(): + #global leftArm + #leftArm.detach() + #sleep(2) + #leftArm.attach(leftPort) + #sleep(2) + #leftArm.bicep.attach() + #leftArm.rotate.attach() + #leftArm.shoulder.attach() + #leftArm.omoplate.attach() + pass + +############################################################ +# MAIN Entry Point +############################################################ +Platform.setVirtual(True) +# Create the IK Service +ik = Runtime.createAndStart("ik", "InverseKinematics3D") +# Create the left arm of the inmoov +i01 = Runtime.start('i01', 'InMoov2') +i01.setVirtual(True) +i01_leftArm = Runtime.start("i01.leftArm", "InMoov2Arm") +i01_leftArm_shoulder = Runtime.start('i01.leftArm.shoulder', 'Servo') +i01_leftArm_rotate = Runtime.start('i01.leftArm.rotate', 'Servo') +i01_leftArm_bicep = Runtime.start('i01.leftArm.bicep', 'Servo') +i01_leftArm_omoplate = Runtime.start('i01.leftArm.omoplate', 'Servo') +# Create the arduino +i01_left = Runtime.start('i01.left', 'Arduino') + +# Servo Config : i01_leftArm_rotate +i01_leftArm_rotate.setPosition(90) +i01_leftArm_rotate.map(40.0,180.0,64.0,132.0) +i01_leftArm_rotate.setInverted(False) +i01_leftArm_rotate.setSpeed(20.0) +i01_leftArm_rotate.setRest(90.0) +i01_leftArm_rotate.setPin(9) +i01_leftArm_rotate.setAutoDisable(True) + +# Servo Config : i01_leftArm_bicep +i01_leftArm_bicep.setPosition(0) +i01_leftArm_bicep.map(0.0,90.0,46.0,96.0) +i01_leftArm_bicep.setInverted(False) +i01_leftArm_bicep.setSpeed(20.0) +i01_leftArm_bicep.setRest(0.0) +i01_leftArm_bicep.setPin(8) +i01_leftArm_bicep.setAutoDisable(True) + +# Servo Config : i01_leftArm_omoplate +i01_leftArm_omoplate.setPosition(10) +i01_leftArm_omoplate.map(10.0,80.0,42.0,80.0) +i01_leftArm_omoplate.setInverted(False) +i01_leftArm_omoplate.setSpeed(20.0) +i01_leftArm_omoplate.setRest(10.0) +i01_leftArm_omoplate.setPin(11) +i01_leftArm_omoplate.setAutoDisable(True) + +# Servo Config : i01_leftArm_shoulder +i01_leftArm_shoulder.setPosition(30) +i01_leftArm_shoulder.map(0.0,180.0,40.0,147.0) +i01_leftArm_shoulder.setInverted(False) +i01_leftArm_shoulder.setSpeed(20.0) +i01_leftArm_shoulder.setRest(30.0) +i01_leftArm_shoulder.setPin(10) +i01_leftArm_shoulder.setAutoDisable(True) + + +# Arduino Config : i01_right +i01_left.setVirtual(False) +# we have the following ports : [COM3.UART, COM4.UART, COM4, COM3] +i01_left.connect("COM7") +i01_left.setBoardMega() +# make sure the pins are set before attaching +i01_leftArm_shoulder.setPin("10") +i01_leftArm_bicep.setPin("8") +i01_leftArm_omoplate.setPin("11") +i01_leftArm_rotate.setPin("9") +i01_left.attach("i01.leftArm.shoulder") +i01_left.attach("i01.leftArm.bicep") +i01_left.attach("i01.leftArm.omoplate") +i01_left.attach("i01.leftArm.rotate") + +jme = i01.startSimulator() + +# update the IK service with the DH parameter description of the inmoov arm. +# For info, D is a length, r is a length. theta and alpha are angles. +ik.setCurrentArm("i01_leftArm", i01_leftArm.getDHRobotArm("i01", "left")) +# wire in the callbacks between IK and the InMoovArm +ik.addListener("publishJointAngles", i01_leftArm.getName(), "onJointAngles") + +# start up a joystick service. +useJoystick = False +if useJoystick: + joystick = Runtime.start("joystick", "Joystick"); + joystick.setController(8); + joystick.addInputListener(ik); + joystick.startPolling() + # TODO: start polling. + +# start the web gui up. +# webgui = Runtime.createAndStart("webgui", "WebGui") +# work around to re-attach the arduino and servos +# sometimes they don't attach on first try due to some race conditions. + +#resetArm() +#x = 0.1 +#y = 0.1 +#z = 100.0 +#ik.moveTo(x,y,z) +# print(ik.currentPosition("i01_leftArm")) + +# starting point +# x , y , z +ik.centerAllJoints("i01_leftArm") + + +x1 = ik.currentPosition("i01_leftArm").x +y1 = ik.currentPosition("i01_leftArm").y +z1 = ik.currentPosition("i01_leftArm").z + + +x = 0 +y = -50 +z = -200 + +# ending point +# x , y , z +x2 = x1 +x +y2 = y1 +y +z2 = z1 +z + + +startPoint = [ x1, y1, z1 ] +# move along the x in a straight line from 100 to 500 +endPoint = [ x2 , y2 , z2 ] + +# how many steps? +numSteps = 100 + +# delay between steps (in seconds) +delay = 0.1 + +# lets compute how long the path is. +dx = 1.0*(x2 - x1)/numSteps +dy = 1.0*(y2 - y1)/numSteps +dz = 1.0*(z2 - z1)/numSteps + +# our current xyz +curX = startPoint[0] +curY = startPoint[1] +curZ = startPoint[2] + +ik.centerAllJoints("i01_leftArm") +sleep(1.0) + +ik.moveTo("i01_leftArm", curX,curY,curZ) + +print ik.currentPosition("i01_leftArm") + +for i in range(0 , 100): + curX+=dx + curY+=dy + curZ+=dz + ik.moveTo("i01_leftArm", curX, curY, curZ) + sleep(delay) + +print ik.currentPosition("i01_leftArm")