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()
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 ####
FR_angles, FL_angles, BR_angles, BL_angles, transformedBodytoFeet = robotKinematics.solve(
orn, pos, bodytoFeet)
pulsesCommand = angleToPulse.convert(FR_angles, FL_angles, BR_angles,
BL_angles)
arduino.serialSend(pulsesCommand) #send serial command to arduino
#move movable joints
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])
from kinematic_model import robotKinematics import numpy as np robotKinematics = robotKinematics() POS = np.array([0, 0, 0]) ROT = np.array([0, 0, 0]) FR_angles = robotKinematics.solve(POS, ROT) print(FR_angles)
[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):
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):
