def QuadrupedCtrl():
global quadruped
rate = rospy.Rate(robParm.freq)
setJSMsg = JointState()
while not rospy.is_shutdown():
get_orientation = []
get_euler = []
start = pybulletDebug.cam_and_robotstates(SIM, quadruped, robParm)
#start = True
if start:
pose_orn = p.getBasePositionAndOrientation(quadruped)
for i in range(4):
get_orientation.append(pose_orn[1][i])
get_euler = p.getEulerFromQuaternion(get_orientation)
# robParm.robotRoll = -get_euler[0] * 1.5
# robParm.robotPitch = -get_euler[1] * 0.5
print(get_euler[0], get_euler[1], get_euler[2])
bodytoFeet = trot.loop(robParm)
position = []
robParm.robotX = pidX(get_euler[1])
robParm.robotY = pidY(get_euler[0])
for i in range(4):
for j in range(3):
position.append(bodytoFeet[i][j].tolist())
FR_angles, FL_angles, HR_angles, HL_angles, _ = robotKinematics.solve(
bodytoFeet, robParm)
for i in range(0, robParm.footFR_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
FR_angles[i - robParm.footFR_index]*robParm.compensateSim[i]) # , force = 20)
for i in range(robParm.footFR_index + 1, robParm.footFL_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
FL_angles[i - robParm.footFL_index]*robParm.compensateSim[i-robParm.footFR_index - 1]) # , force = 20)
for i in range(robParm.footFL_index + 1, robParm.footHR_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
HR_angles[i - robParm.footHR_index]*robParm.compensateSim[i-robParm.footFL_index - 1]) # , force = 20)
for i in range(robParm.footHR_index + 1, robParm.footHL_index):
p.setJointMotorControl2(quadruped, i, p.POSITION_CONTROL,
HL_angles[i - robParm.footHL_index]*robParm.compensateSim[i-robParm.footHR_index - 1]) # , force = 20)
angle = FL_angles.tolist()
angle.extend(HL_angles.tolist())
angle.extend(FR_angles.tolist())
angle.extend(HR_angles.tolist())
setJSMsg.header.stamp = rospy.Time.now()
setJSMsg.name = ["abduct_fl", "thigh_fl", "knee_fl", "abduct_hl", "thigh_hl", "knee_hl",
"abduct_fr", "thigh_fr", "knee_fr", "abduct_hr", "thigh_hr", "knee_hr"]
for i in range(12):
angle[i] = angle[i] * robParm.compensateReal[i]
# print(angle)
setJSMsg.position = angle
setJsPub.publish(setJSMsg)
rate.sleep()
orientation = np.array([0., 0., 0.])
deviation = np.array([0., 0., 0.])
lastTime = 0.
pidX = PID(-0.0005, 0.0, 0.000001, setpoint=0.)
pidY = PID(0.0005, 0., 0., setpoint=0.)
pidX.sample_time = 0.02 # update every 0.01 seconds
pidY.sample_time = 0.02
T = 0.5 #period of time (in seconds) of every step
offset = np.array([
0.5, 0., 0., 0.5
]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
while (True):
startTime = time.time()
pos, orn, L, angle, Lrot, T = pybulletDebug.cam_and_robotstates(boxId)
L, angle, Lrot, T = joystick.read()
#calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
bodytoFeet, s = trot.loop(L, angle, Lrot, T, offset, bodytoFeet0)
arduinoLoopTime, realRoll, realPitch = arduino.serialRecive(
) #recive serial message
pos[0] = 0.015 + pidX(realPitch)
pos[1] = pidY(realRoll)
#####################################################################################
##### kinematics Model: Input body orientation, deviation and foot position ####
##### and get the angles, neccesary to reach that position, for every joint ####
lastTime = 0.
pidX = PID(-0.0005, 0.0, 0.000001, setpoint=0.)
pidY = PID(0.0005, 0., 0., setpoint=0.)
pidX.sample_time = 0.02 # update every 0.02 seconds
pidY.sample_time = 0.02
T = 0.4 #period of time (in seconds) of every step
offset = np.array([
0.5, 0., 0., 0.5
]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
p.setRealTimeSimulation(1)
p.setTimeStep(0.002)
while (True):
loopTime = time.time()
_, _, _, _, _, _ = pybulletDebug.cam_and_robotstates(boxId)
commandCoM, V, angle, Wrot, T, compliantMode, yaw, pitch = joystick.read()
#calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
bodytoFeet = trot.loop(V, angle, Wrot, T, offset, bodytoFeet0)
arduinoLoopTime, Xacc, Yacc, realRoll, realPitch = arduino.serialRecive(
) #recive serial message
pos[0] = pidX(realPitch)
pos[1] = pidY(realRoll)
#####################################################################################
##### kinematics Model: Input body orientation, deviation and foot position ####
##### and get the angles, neccesary to reach that position, for every joint ####
footFR_index = 3
footFL_index = 7
footBR_index = 11
footBL_index = 15
pybulletDebug = pybulletDebug()
robotKinematics = robotKinematics()
p.setRealTimeSimulation(1)
while(True):
lastTime = time.time()
POS , ROT , SPEED = pybulletDebug.cam_and_robotstates(boxId)
FR_angles , BL_angles, BR_angles, FL_angles = robotKinematics.doik(POS , ROT ,SPEED)
FR_angles[0] = FR_angles[0]- 0.785398
BL_angles[0] = BL_angles[0]+ 0.785398
BR_angles[0] = BR_angles[0]+ 2.356
FL_angles[0] = FL_angles[0]- 2.356
for i in range(0, footFR_index):
p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, FR_angles[i - footFR_index])
for i in range(footFR_index + 1, footFL_index):
p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, FL_angles[i - footFL_index])
for i in range(footFL_index + 1, footBR_index):
p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, BR_angles[i - footBR_index])
for i in range(footBR_index + 1, footBL_index):
p.setJointMotorControl2(boxId, i, p.POSITION_CONTROL, BL_angles[i - footBL_index])
#bodytoFL0 = np.array([ L/2, W/2 , 0])
#bodytoBR0 = np.array([-L/2, -W/2 , 0])
#bodytoBL0 = np.array([-L/2, W/2 , 0])
#body frame to foot frame vector
bodytoFeet = np.matrix([[Xdist / 2, -Ydist / 2, -height],
[Xdist / 2, Ydist / 2, -height],
[-Xdist / 2, -Ydist / 2, -height],
[-Xdist / 2, Ydist / 2, -height]])
orientation = np.array([0., 0., 0.])
deviation = np.array([0., 0., 0.])
while (True):
timeNow = time.time()
deviation, orientation = pybulletDebug.cam_and_robotstates(boxId)
#####################################################################################
##### kinematics Model: Input body orientation, deviation and foot position ####
##### and get the angles, neccesary to reach that position, for every joint ####
FR_angles, FL_angles, BR_angles, BL_angles = robotKinematics.solve(
orientation, deviation, bodytoFeet)
# 2020-04-03 cyan adapt
FR_angles = FR_angles.tolist()[0]
FL_angles = FL_angles.tolist()[0]
BR_angles = BR_angles.tolist()[0]
BL_angles = BL_angles.tolist()[0]
#move movable joints
for i in range(0, footFR_index):
