Having link orientation be in relation to world frame instead of base frame

[jgarc248]

Hello, I am currently trying to get a single-legged hopper to move in the x-z plane by calculating the proper landing and takeoff angle. From that angle, I calculate the quaternion points that will be put into the setJointMotorControlMultiDof in order to properly orient the landing pole.
However, the issue I am having is that when I orient the pole of the hopper, the orientation is with respect to the coordinate system of the base rather than the world frame.

I've been trying to find a way to change this but so far have been unsuccessful.
I would appreciate any advice or guidance!
Thank you for your time and have a nice day!

`# -- coding: utf-8 --

import pybullet as p
import time
import pybullet_data
import math
import matplotlib.pyplot as plt

physicsClient = p.connect(p.GUI)

p.resetSimulation()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
p.resetDebugVisualizerCamera(cameraDistance=4.5, cameraYaw=44, cameraPitch=-37.80,
cameraTargetPosition=[0.0,0.0,0.42])
p.setGravity(0,0,-10)

disconnect0 = p.addUserDebugParameter('Close Simulation', 1,0,0)
kForce0 = p.addUserDebugParameter('K Constant (N/m)', 0, 25000, 10000)

p.changeDynamics(planeId,-1,restitution=1.0)

sRadius = .1
testPlate = p.createCollisionShape(p.GEOM_BOX, halfExtents = [.75,.75,.01])
testSphere = p.createCollisionShape(p.GEOM_SPHERE, sRadius)
testCylinder = p.createCollisionShape(p.GEOM_CYLINDER, radius = .05, height = 1)

link_Masses = [50]
linkCollisionShapeIndices = [testCylinder]
linkVisualShapeIndices = [-1]
linkPositions = [[0, 0, -0.55]]
linkOrientations = [[0, 0, 0, 1]]
linkInertialFramePositions = [[0, 0, 0]]
linkInertialFrameOrientations = [[0, 0, 0, 1]]
indices = [0]
jointTypes = [p.JOINT_SPHERICAL]
axis = [[0, 1, 0]]

mass = .01
pos0 = [ 0, 0, 2.0]
pos1 = [ 0, 0, 1]
ori = [0,0,0,1]
pogo = p.createMultiBody(3, testPlate, -1, pos0, ori, linkMasses = link_Masses,
linkCollisionShapeIndices = linkCollisionShapeIndices,
linkVisualShapeIndices= linkVisualShapeIndices,
linkPositions = linkPositions, linkOrientations = linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis, flags = p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT)
ball = p.createMultiBody(1, testSphere, -1, [0,0,.45], [0,0,0,1])
p.changeDynamics(ball, -1, lateralFriction = 1.0)

maxForceJoint = 0
mode = p.VELOCITY_CONTROL
p.setJointMotorControl2(pogo, 0, controlMode=mode, force=maxForceJoint)
p.setJointMotorControl2(pogo, 0, controlMode=p.POSITION_CONTROL , force=maxForceJoint)

cid = p.createConstraint(pogo, 0, ball, -1 ,p.JOINT_FIXED,[0,0,1],[0,0,0],[0,0,1])
pris = p.createConstraint(pogo, 0, ball, -1, p.JOINT_PRISMATIC, [0,0,1],[0,0,0],[0,0,1])

kConstant = 10000 #N/m
initialDis = .40

p.resetBaseVelocity(pogo, [1,0,0])

springTD = 0
forceList = []
velY = []
stepsX = []
step = 0

TDangleList = []
TOangleList= []

print('--------------------------------------------------------------')

while 1:
#time.sleep(1/12)
#------------------------------------------------
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
p.stepSimulation()

disconnect1 = p.readUserDebugParameter(disconnect0)
kForce1 = p.readUserDebugParameter(kForce0)

#------------------------------------------------

Ts = math.pi * math.sqrt(53.01/kForce1)
xdot = 1
Vdes = xdot

launchAngle = math.acos(xdot * Ts/(2*initialDis)+ .1*(xdot-Vdes))
angleTD = [0, -launchAngle, 0]
angleTO = [0, launchAngle, 0]

quatTD = p.getQuaternionFromEuler(angleTD)
quatTO = p.getQuaternionFromEuler(angleTO)

print("Touchdown Angle: " + str(-launchAngle))
TDangleList.append(quatTD)
print("Take-Off Angle: " + str(launchAngle))
TOangleList.append(quatTO)
p.setJointMotorControl2(pogo, 0, controlMode=p.POSITION_CONTROL, targetPosition = -launchAngle, force=300)

planeBallContact = p.getContactPoints(planeId, ball, linkIndexB =-1 )
if planeBallContact:
  p.setJointMotorControl2(pogo, 0, controlMode=p.POSITION_CONTROL, targetPosition = launchAngle, force=300)

#-------------------------------------------------

basePos = p.getBasePositionAndOrientation(pogo)[0]
zBasePos = basePos[2]

if (disconnect1 == 1):
    plt.plot(stepsX, forceList)
    #plt.plot(stepsX, zLinearBaseVel)
    plt.xlabel('Steps')
    plt.ylabel("Spring Force")
    plt.title("Spring Force vs Steps")
    plt.show()
    
    p.disconnect()
    disconnect1 = 0

#---------------------------------------------------

#-----------------------------------------------------

# p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)    
# p.stepSimulation()
pogoBallPoints = p.getClosestPoints(pogo, ball, distance = 100, linkIndexA = 0, linkIndexB = -1)
pogoBallPointsList = pogoBallPoints[0]
pogoBallDis = pogoBallPointsList[8]
compressDis = initialDis - pogoBallDis
force = compressDis * kForce1
p.changeConstraint(cid, maxForce = force)
    
print("X Distance: " + str(compressDis))
print("Spring Force: " + str(force))
forceList.append(force) 


baseVel = p.getBaseVelocity(pogo)
linearBaseVel = baseVel[0]
zLinearBaseVel = linearBaseVel[2]

#print(" Base Velocity: " + str(zLinearBaseVel))
velY.append(abs(zLinearBaseVel))
step = step + 1
stepsX.append(step)

while abs(zLinearBaseVel) < .10:
    p.changeConstraint(cid, maxForce = 6000)
    print("Base Velocity approx. Zero")
    
    baseVel = p.getBaseVelocity(pogo)
    linearBaseVel = baseVel[0]
    zLinearBaseVel = linearBaseVel[2]
    
    p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)    
    p.stepSimulation()
    
    disconnect1 = p.readUserDebugParameter(disconnect0)
    if (disconnect1 == 1):
        p.disconnect()
        disconnect1 = 0

print('--------------------------------------------------------------')

`