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):