├── Bittle_moving.gif ├── LICENSE ├── Nybble_moving.gif ├── README.md ├── kinematics_bittle.py ├── kinematics_nybble.py └── requirements.txt /Bittle_moving.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ger01d/kinematic-model-opencat/3c40e21850c63028e7f674493c30e3230086b4be/Bittle_moving.gif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 ger01d 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /Nybble_moving.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ger01d/kinematic-model-opencat/3c40e21850c63028e7f674493c30e3230086b4be/Nybble_moving.gif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Inverse Kinematic Model OpenCat 2 | 3 | This is an inverse kinematic model and gait generator for the OpenCat project based on the IKPY library (http://phylliade.github.io/ikpy). 4 | 5 | ## Nybble-Version 6 | ![Nybble](Nybble_moving.gif) 7 | 8 | ## Bittle-Version 9 | ![Bittle](Bittle_moving.gif) 10 | 11 | ## Usage and Tweaks 12 | Simply run the script with python kinematics_bittle.py or kinematics_nybble.py. At first you will see a plot of the vertical and horizontal movement. After closing this window, the script will generate the joint angles, that can be directly used for the instincts.h in OpenCat. 13 | 14 | 15 | Please make sure that you install the required packages in requirements.txt. 16 | 17 | 18 | -------------------------------------------------------------------------------- /kinematics_bittle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: utf-8 3 | 4 | import ikpy 5 | from ikpy.chain import Chain 6 | from ikpy.link import OriginLink, URDFLink 7 | import numpy as np 8 | from numpy import sin, cos, pi 9 | import matplotlib.pyplot as plt 10 | import mpl_toolkits.mplot3d.axes3d as p3 11 | 12 | 13 | deg2rad = pi/180 14 | rad2deg = 180/pi 15 | 16 | # Dimensions Bittle (should not be changed - all values in cm) 17 | upperArmLength = 4.5 18 | lowerArmLength = 5.0 19 | bodyLength = 10.5 20 | bodyWidth = 9.7 21 | 22 | # Variable parameters 23 | distanceFloor = 6.0 # Distance torso to floor 24 | stepLength = 3.5 # Longitudinal movement of each paw within one period 25 | swingHeight = 0.5 # Amplitude for swing 26 | swingFactorLegs = 5 # Increase swing amplitude for legs 27 | leanForward = 0.0 # Shifting torso in x-direction 28 | stanceFront = 0.0 # Distance of front paws from shoulder in x-direction 29 | stanceBack = 3 # Distance of back paws from hip in negative x-direction 30 | 31 | # Number of Frames 32 | frames = 57 33 | 34 | # Kinematic chain of Bittle 35 | left_arm = Chain(name='left_arm', links=[ 36 | URDFLink( 37 | name="center", 38 | origin_translation=[leanForward, 0, distanceFloor], 39 | origin_orientation=[0, 0, 0], 40 | rotation=[0, 0, 0], 41 | ), 42 | URDFLink( 43 | name="shoulder", 44 | origin_translation=[0, bodyWidth/2, 0], 45 | origin_orientation=[0, 0, 0], 46 | rotation=[0, 1, 0], 47 | bounds=(0.1*deg2rad, 179.9*deg2rad), 48 | ), 49 | URDFLink( 50 | name="upperArm", 51 | origin_translation=[upperArmLength, 0, 0], 52 | origin_orientation=[0, 0, 0], 53 | rotation=[0, 1, 0], 54 | bounds=(-179.9*deg2rad, -0.1*deg2rad), 55 | ), 56 | URDFLink( 57 | name="lowerArm", 58 | origin_translation=[lowerArmLength, 0, 0], 59 | origin_orientation=[0, 0, 0], 60 | rotation=[0, 0, 0], 61 | ) 62 | ]) 63 | 64 | right_arm = Chain(name='right_arm', links=[ 65 | URDFLink( 66 | name="center", 67 | origin_translation=[leanForward, 0, distanceFloor], 68 | origin_orientation=[0, 0, 0], 69 | rotation=[0, 0, 0], 70 | ), 71 | URDFLink( 72 | name="shoulder", 73 | origin_translation=[0, -bodyWidth/2, 0], 74 | origin_orientation=[0, 0, 0], 75 | rotation=[0, 1, 0], 76 | bounds=(0.1*deg2rad, 179.9*deg2rad), 77 | ), 78 | URDFLink( 79 | name="upperArm", 80 | origin_translation=[upperArmLength, 0, 0], 81 | origin_orientation=[0,0,0], 82 | rotation=[0,1,0], 83 | bounds=(-179.9*deg2rad, -0.1*deg2rad), 84 | ), 85 | URDFLink( 86 | name="lowerArm", 87 | origin_translation=[lowerArmLength, 0, 0], 88 | origin_orientation=[0,0,0], 89 | rotation=[0, 1, 0], 90 | ) 91 | ]) 92 | 93 | left_leg = Chain(name='left_leg', links=[ 94 | URDFLink( 95 | name="center", 96 | origin_translation=[leanForward, 0, distanceFloor], 97 | origin_orientation=[0, 0, 0], 98 | rotation=[0, 0, 0], 99 | ), 100 | URDFLink( 101 | name="butt", 102 | origin_translation=[-bodyLength, 0, 0], 103 | origin_orientation=[0, 0, 0], 104 | rotation=[0, 0, 0], 105 | ), 106 | URDFLink( 107 | name="hip", 108 | origin_translation=[0, bodyWidth/2, 0], 109 | origin_orientation=[0, 0, 0], 110 | rotation=[0, 1, 0], 111 | bounds=(0.1*deg2rad, 200*deg2rad), 112 | ), 113 | URDFLink( 114 | name="upperLeg", 115 | origin_translation=[upperArmLength, 0, 0], 116 | origin_orientation=[0, 0, 0], 117 | rotation=[0, 1, 0], 118 | bounds=(-200*deg2rad, -0.1*deg2rad), 119 | ), 120 | URDFLink( 121 | name="lowerLeg", 122 | origin_translation=[lowerArmLength, 0, 0], 123 | origin_orientation=[0, 0, 0], 124 | rotation=[0, 1, 0], 125 | ) 126 | ]) 127 | 128 | right_leg = Chain(name='right_leg', links=[ 129 | URDFLink( 130 | name="center", 131 | origin_translation=[leanForward, 0, distanceFloor], 132 | origin_orientation=[0, 0, 0], 133 | rotation=[0, 0, 0], 134 | ), 135 | URDFLink( 136 | name="butt", 137 | origin_translation=[-bodyLength, 0, 0], 138 | origin_orientation=[0, 0, 0], 139 | rotation=[0, 0, 0], 140 | ), 141 | URDFLink( 142 | name="hip", 143 | origin_translation=[ 0, -bodyWidth/2, 0], 144 | origin_orientation=[0, 0, 0], 145 | rotation=[0, 1, 0], 146 | bounds=(0.1*deg2rad, 200*deg2rad), 147 | ), 148 | URDFLink( 149 | name="upperLeg", 150 | origin_translation=[upperArmLength, 0, 0], 151 | origin_orientation=[0, 0, 0], 152 | rotation=[0, 1, 0], 153 | bounds=(-200*deg2rad, -0.1*deg2rad), 154 | ), 155 | URDFLink( 156 | name="lowerLeg", 157 | origin_translation=[lowerArmLength, 0, 0], 158 | origin_orientation=[0, 0, 0], 159 | rotation=[0, 1, 0], 160 | ) 161 | ]) 162 | 163 | def buildGait(frames): 164 | frame = np.arange(0,frames) 165 | swingEnd = pi/2 166 | 167 | # longitudinal Movement 168 | swing = -cos(2*(frame*2*pi/frames)) 169 | stance = +cos(2/3*(frame*2*pi/frames-swingEnd)) 170 | swingSlice = np.less_equal(frame,swingEnd/(2*pi/frames)) 171 | stanceSlice = np.invert(swingSlice) 172 | longitudinalMovement = np.concatenate((swing[swingSlice], stance[stanceSlice])) 173 | longitudinalMovement = np.concatenate((longitudinalMovement,longitudinalMovement, longitudinalMovement, longitudinalMovement)) 174 | 175 | # vertical Movement 176 | lift = sin(2*(frame*2*pi/frames)) #np.ones(frames) 177 | liftSlice = swingSlice 178 | verticalMovement = np.concatenate((lift[liftSlice], np.zeros(np.count_nonzero(stanceSlice)))) 179 | verticalMovement = np.concatenate((verticalMovement, verticalMovement, verticalMovement, verticalMovement)) 180 | return longitudinalMovement, verticalMovement 181 | 182 | 183 | longitudinalMovement, verticalMovement = buildGait(frames) 184 | plt.plot(longitudinalMovement) 185 | plt.plot(verticalMovement) 186 | plt.show() 187 | plt.close() 188 | 189 | shiftFrame = np.round(np.array([0, pi/2, pi, 3*pi/2])/2/pi*frames) 190 | #shiftFrame = np.round(np.array([0, pi, pi, 0])/2/pi*frames) 191 | #shiftFrame = np.round(np.array([0, 0, pi, pi])/2/pi*frames) 192 | shiftFrame = shiftFrame.astype(int) 193 | 194 | for frame in range(0, frames): 195 | right_arm_angles_raw = right_arm.inverse_kinematics(target_position = np.array([stanceFront+stepLength*longitudinalMovement[frame], -bodyWidth/2 ,swingHeight*verticalMovement[frame]]), optimization_method='SLSQP') 196 | right_arm_correction = np.array([0, -pi/2, pi/2, 0]) 197 | right_arm_angles = np.round(rad2deg*(right_arm_angles_raw+right_arm_correction)) 198 | right_arm_angles = np.delete(right_arm_angles, np.s_[0,3], axis=0) 199 | right_arm_angles = right_arm_angles.astype(int) 200 | 201 | right_leg_angles_raw = right_leg.inverse_kinematics(target_position = np.array([-stanceBack-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[1]], -bodyWidth/2 , swingFactorLegs*swingHeight*verticalMovement[frame+shiftFrame[1]]]), optimization_method='SLSQP') 202 | right_leg_correction = np.array([0, 0, -pi/2, pi/2, 0]) 203 | right_leg_angles = np.round(rad2deg*(right_leg_angles_raw+right_leg_correction)) 204 | right_leg_angles = np.delete(right_leg_angles, np.s_[0,1,4], axis=0) 205 | right_leg_angles = right_leg_angles.astype(int) 206 | 207 | left_arm_angles_raw = left_arm.inverse_kinematics(target_position = np.array([stanceFront+stepLength*longitudinalMovement[frame+shiftFrame[2]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[2]]]), optimization_method='SLSQP') 208 | left_arm_correction = np.array([0, -pi/2, pi/2, 0]) 209 | left_arm_angles = np.round(rad2deg*(left_arm_angles_raw+left_arm_correction)) 210 | left_arm_angles = np.delete(left_arm_angles, np.s_[0,3], axis=0) 211 | left_arm_angles = left_arm_angles.astype(int) 212 | 213 | left_leg_angles_raw = left_leg.inverse_kinematics(target_position = np.array([-stanceBack-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[3]], +bodyWidth/2 , swingFactorLegs*swingHeight*verticalMovement[frame+shiftFrame[3]]]), optimization_method='SLSQP') 214 | left_leg_correction = np.array([0, 0, -pi/2, pi/2, 0]) 215 | left_leg_angles = np.round(rad2deg*(left_leg_angles_raw+left_leg_correction)) 216 | left_leg_angles = np.delete(left_leg_angles, np.s_[0,1,4],axis=0) 217 | left_leg_angles = left_leg_angles.astype(int) 218 | 219 | 220 | # Writing sequence to file 221 | gait_sequence = np.concatenate((left_arm_angles, right_arm_angles, right_leg_angles, left_leg_angles)) 222 | print(frame," ",gait_sequence) 223 | f = open("gait_sequence.csv", "a") 224 | #f.write("[") 225 | np.savetxt(f, gait_sequence[[0,2,4,6,1,3,5,7]], fmt='%3.1i', delimiter=',', newline=', ') 226 | #f.write("],") 227 | f.write("\n") 228 | f.close() 229 | 230 | # Create plot and image for each frame 231 | fig = plt.figure() 232 | ax = p3.Axes3D(fig) 233 | ax.set_box_aspect([3, 3/2,1]) 234 | ax.set_xlim3d([-20, 10]) 235 | ax.set_xlabel('X') 236 | 237 | ax.set_ylim3d([-10, 10]) 238 | ax.set_ylabel('Y') 239 | 240 | ax.set_zlim3d([0.0, 10]) 241 | ax.set_zlabel('Z') 242 | right_arm.plot(right_arm_angles_raw, ax) 243 | right_leg.plot(right_leg_angles_raw, ax) 244 | left_arm.plot(left_arm_angles_raw, ax) 245 | left_leg.plot(left_leg_angles_raw, ax) 246 | 247 | figureName = "Bittle_" + str(frame) 248 | plt.savefig(figureName) 249 | plt.close() 250 | #plt.show() 251 | 252 | -------------------------------------------------------------------------------- /kinematics_nybble.py: -------------------------------------------------------------------------------- 1 | import ikpy 2 | from ikpy.chain import Chain 3 | from ikpy.link import OriginLink, URDFLink 4 | import numpy as np 5 | from numpy import sin, cos, pi 6 | import matplotlib.pyplot as plt 7 | import mpl_toolkits.mplot3d.axes3d as p3 8 | 9 | deg2rad = pi/180 10 | rad2deg = 180/pi 11 | 12 | # Values in cm 13 | armLength = 5 14 | bodyLength = 12 15 | bodyWidth = 10 16 | distanceFloor = 7 17 | stepLength = 5.0 18 | swingHeight = 0.3 19 | 20 | leanLeft = 0 # Not working, yet 21 | leanRight = -leanLeft # Not working, yet 22 | leanForward = 0 # cm 23 | 24 | left_arm = Chain(name='left_arm', links=[ 25 | URDFLink( 26 | name="center", 27 | origin_translation=[leanForward, 0, distanceFloor], 28 | origin_orientation=[0, 0, 0], 29 | rotation=[0, 0, 0], 30 | ), 31 | URDFLink( 32 | name="shoulder", 33 | origin_translation=[0, bodyWidth/2, leanLeft], 34 | origin_orientation=[0, 0, 0], 35 | rotation=[0, 1, 0], 36 | bounds=(0.1*deg2rad, 179.9*deg2rad), 37 | ), 38 | URDFLink( 39 | name="upperArm", 40 | origin_translation=[armLength, 0, 0 ], 41 | origin_orientation=[0, 0, 0], 42 | rotation=[0, 1, 0], 43 | bounds=(-179.9*deg2rad, -0.1*deg2rad), 44 | ), 45 | URDFLink( 46 | name="lowerArm", 47 | origin_translation=[armLength, 0, 0], 48 | origin_orientation=[0, 0, 0], 49 | rotation=[0, 0, 0], 50 | ) 51 | 52 | ]) 53 | 54 | right_arm = Chain(name='right_arm', links=[ 55 | URDFLink( 56 | name="center", 57 | origin_translation=[leanForward, 0, distanceFloor], 58 | origin_orientation=[0, 0, 0], 59 | rotation=[0, 0, 0], 60 | ), 61 | URDFLink( 62 | name="shoulder", 63 | origin_translation=[0, -bodyWidth/2, leanRight], 64 | origin_orientation=[0, 0, 0], 65 | rotation=[0, 1, 0], 66 | bounds=(0.1*deg2rad, 179.9*deg2rad), 67 | ), 68 | URDFLink( 69 | name="upperArm", 70 | origin_translation=[armLength, 0, 0], 71 | origin_orientation=[0,0,0], 72 | rotation=[0,1,0], 73 | bounds=(-179.9*deg2rad, -0.1*deg2rad), 74 | ), 75 | URDFLink( 76 | name="lowerArm", 77 | origin_translation=[armLength, 0, 0 ], 78 | origin_orientation=[0,0,0], 79 | rotation=[0, 1, 0], 80 | ) 81 | 82 | ]) 83 | 84 | left_leg = Chain(name='left_leg', links=[ 85 | URDFLink( 86 | name="center", 87 | origin_translation=[leanForward, 0, distanceFloor], 88 | origin_orientation=[0, 0, 0], 89 | rotation=[0, 0, 0], 90 | ), 91 | URDFLink( 92 | name="butt", 93 | origin_translation=[-bodyLength, 0, 0], 94 | origin_orientation=[0, 0, 0], 95 | rotation=[0, 0, 0], 96 | ), 97 | URDFLink( 98 | name="hip", 99 | origin_translation=[0, bodyWidth/2, leanLeft], 100 | origin_orientation=[0, 0, 0], 101 | rotation=[0, 1, 0], 102 | bounds=(0.1*deg2rad, 120*deg2rad), 103 | ), 104 | URDFLink( 105 | name="upperLeg", 106 | origin_translation=[armLength, 0, 0 ], 107 | origin_orientation=[0, 0, 0], 108 | rotation=[0, 1, 0], 109 | bounds=(0.1*deg2rad, 179.9*deg2rad), 110 | ), 111 | URDFLink( 112 | name="lowerLeg", 113 | origin_translation=[armLength, 0, 0 ], 114 | origin_orientation=[0, 0, 0], 115 | rotation=[0, 1, 0], 116 | ) 117 | 118 | ]) 119 | 120 | right_leg = Chain(name='right_leg', links=[ 121 | URDFLink( 122 | name="center", 123 | origin_translation=[leanForward, 0, distanceFloor], 124 | origin_orientation=[0, 0, 0], 125 | rotation=[0, 0, 0], 126 | ), 127 | URDFLink( 128 | name="butt", 129 | origin_translation=[-bodyLength, 0, 0], 130 | origin_orientation=[0, 0, 0], 131 | rotation=[0, 0, 0], 132 | ), 133 | URDFLink( 134 | name="hip", 135 | origin_translation=[ 0, -bodyWidth/2, leanRight], 136 | origin_orientation=[0, 0, 0], 137 | rotation=[0, 1, 0], 138 | bounds=(0.1*deg2rad, 120*deg2rad), 139 | ), 140 | URDFLink( 141 | name="upperLeg", 142 | origin_translation=[armLength, 0, 0 ], 143 | origin_orientation=[0, 0, 0], 144 | rotation=[0, 1, 0], 145 | bounds=(0.1*deg2rad, 179*deg2rad), 146 | ), 147 | URDFLink( 148 | name="lowerLeg", 149 | origin_translation=[armLength, 0, 0], 150 | origin_orientation=[0, 0, 0], 151 | rotation=[0, 1, 0], 152 | ) 153 | 154 | ]) 155 | 156 | 157 | def buildGait(frames): 158 | frame = np.arange(0,frames) 159 | swingEnd = pi/2 160 | 161 | # longitudinal Movement 162 | swing = -cos(2*(frame*2*pi/frames)) 163 | stance = cos(2/3*(frame*2*pi/frames-swingEnd)) 164 | swingSlice = np.less_equal(frame,swingEnd/(2*pi/frames)) 165 | stanceSlice = np.invert(swingSlice) 166 | longitudinalMovement = np.concatenate((swing[swingSlice], stance[stanceSlice])) 167 | longitudinalMovement = np.concatenate((longitudinalMovement,longitudinalMovement, longitudinalMovement, longitudinalMovement)) 168 | 169 | # vertical Movement 170 | lift = sin(2*(frame*2*pi/frames)) 171 | liftSlice = swingSlice 172 | verticalMovement = np.concatenate((lift[liftSlice], np.zeros(np.count_nonzero(stanceSlice)))) 173 | verticalMovement = np.concatenate((verticalMovement, verticalMovement, verticalMovement, verticalMovement)) 174 | return longitudinalMovement, verticalMovement 175 | 176 | 177 | frames = 43 178 | longitudinalMovement, verticalMovement = buildGait(frames) 179 | plt.plot(longitudinalMovement) 180 | plt.plot(verticalMovement) 181 | plt.show() 182 | 183 | # Walking pattern 184 | # [0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1], # LL 185 | # [0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0], # RL 186 | # [0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0], # LA 187 | # [1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0], # RA 188 | 189 | # Phase shift between arms/legs, [RA, RL, LA, LL] 190 | shiftFrame = np.round(np.array([0, pi/2, pi, 3*pi/2])/2/pi*frames) 191 | #shiftFrame = np.round(np.array([0, 0, pi, pi])/2/pi*frames) 192 | #shiftFrame = np.round(np.array([0, pi, pi, 0])/2/pi*frames) 193 | 194 | shiftFrame = shiftFrame.astype(int) 195 | 196 | for frame in range(0, frames): 197 | right_arm_angles_raw = right_arm.inverse_kinematics(target_position = np.array([stepLength*longitudinalMovement[frame], -bodyWidth/2 ,swingHeight*verticalMovement[frame]]), optimization_method='SLSQP') 198 | right_arm_correction = np.array([0, -pi/2, pi/2, 0]) 199 | right_arm_angles = np.round(rad2deg*(right_arm_angles_raw+right_arm_correction)) 200 | right_arm_angles = np.delete(right_arm_angles, np.s_[0,3], axis=0) 201 | right_arm_angles = right_arm_angles.astype(int) 202 | 203 | right_leg_angles_raw = right_leg.inverse_kinematics(target_position = np.array([-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[1]], -bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[1]]]), optimization_method='SLSQP') 204 | right_leg_correction = np.array([0, 0, -pi/2, -pi/2, 0]) 205 | right_leg_angles = np.round(rad2deg*(right_leg_angles_raw+right_leg_correction)) 206 | right_leg_angles = np.delete(right_leg_angles, np.s_[0,1,4], axis=0) 207 | right_leg_angles = right_leg_angles.astype(int) 208 | 209 | left_arm_angles_raw = left_arm.inverse_kinematics(target_position = np.array([stepLength*longitudinalMovement[frame+shiftFrame[2]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[2]]]), optimization_method='SLSQP') 210 | left_arm_correction = np.array([0, -pi/2, pi/2, 0]) 211 | left_arm_angles = np.round(rad2deg*(left_arm_angles_raw+left_arm_correction)) 212 | left_arm_angles = np.delete(left_arm_angles, np.s_[0,3], axis=0) 213 | left_arm_angles = left_arm_angles.astype(int) 214 | 215 | left_leg_angles_raw = left_leg.inverse_kinematics(target_position = np.array([-bodyLength+stepLength*longitudinalMovement[frame+shiftFrame[3]], +bodyWidth/2 , swingHeight*verticalMovement[frame+shiftFrame[3]]]), optimization_method='SLSQP') 216 | left_leg_correction = np.array([0, 0, -pi/2, -pi/2, 0]) 217 | left_leg_angles = np.round(rad2deg*(left_leg_angles_raw+left_leg_correction)) 218 | left_leg_angles = np.delete(left_leg_angles, np.s_[0,1,4],axis=0) 219 | left_leg_angles = left_leg_angles.astype(int) 220 | 221 | 222 | # Writing sequence to file 223 | gait_sequence = np.concatenate((left_arm_angles, right_arm_angles, right_leg_angles, left_leg_angles)) 224 | print(frame," ",gait_sequence) 225 | f = open("gait_sequence.csv", "a") 226 | #f.write("#") 227 | np.savetxt(f, gait_sequence[[0,2,4,6,1,3,5,7]], fmt='%3.1i', delimiter=',', newline=", ") 228 | #f.write("+") 229 | f.write("\n") 230 | f.close() 231 | 232 | # Create plot and image for each frame 233 | fig = plt.figure() 234 | ax = p3.Axes3D(fig) 235 | ax.set_box_aspect([3, 3/2,1]) 236 | ax.set_xlim3d([-20, 10]) 237 | ax.set_xlabel('X') 238 | 239 | ax.set_ylim3d([-10, 10]) 240 | ax.set_ylabel('Y') 241 | 242 | ax.set_zlim3d([0.0, 10]) 243 | ax.set_zlabel('Z') 244 | right_arm.plot(right_arm_angles_raw, ax) 245 | right_leg.plot(right_leg_angles_raw, ax) 246 | left_arm.plot(left_arm_angles_raw, ax) 247 | left_leg.plot(left_leg_angles_raw, ax) 248 | 249 | figureName = "Nybble_" + str(frame) 250 | plt.savefig(figureName) 251 | plt.close() 252 | 253 | #plt.show() 254 | 255 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.20.3 2 | matplotlib==3.4.2 3 | ikpy==3.2.2 4 | --------------------------------------------------------------------------------