Added cpg code

sin_cpg.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+import rospy
+from std_msgs.msg import Float64
+
+pi = 3.141569
+amp = 3
+camp = 0.8
+phase = [pi, -pi]
+offset = np.array([0,0])
+coff = 0.1
+frequency = 0.1
+dt = 0.0005
+result = 0
+time = np.arange(0,100,dt)
+iterations = 1000000
+
+rospy.init_node("CPG")
+rate = rospy.Rate(76800)
+
+pub1 = rospy.Publisher("leg1",Float64,queue_size=10)
+pub2 = rospy.Publisher("leg2",Float64,queue_size=10)
+
+def dp(frequency, idx, phase, phase_offset_target):
+    if idx == 0:
+        return 2 * pi * frequency + 1 * amp * math.sin(phase[1]-phase[0]-phase_offset_target)
+    else:
+        return 2 * pi * frequency + 1 * amp * math.sin(phase[0]-phase[1]+phase_offset_target)
+
+def da(amp_target):
+    return camp * (amp_target - amp)
+
+def do(offset_target):
+    return coff * (offset_target - offset)
+
+def update_values(amp_target = 1, offset_target = np.array([0,0]), frequency_target = 0.1, phase_offset_target = pi):
+    global phase, amp, offset
+    for i in range(2):
+        phase[i] = phase[i] + dp(frequency_target, i, phase, phase_offset_target) * dt
+
+    amp = amp + da(amp_target) * dt
+    offset = offset + do(offset_target)
+
+def output():
+    return amp * np.sin(phase) + offset
+
+outputs = []
+n1 = []
+n2 = []
+
+
+# for i in range(iterations):
+while not rospy.is_shutdown():
+    # if i < iterations / 3:
+    #     update_values()
+    # elif i < 2 * iterations / 3:
+    #     update_values(2,offset_target = np.array([1,5]),frequency_target = 0.2,phase_offset_target = pi + 1.57)
+    # else:
+    #     update_values(offset_target = np.array([1,1]), phase_offset_target = 2*pi+1.57)
+    update_values()
+
+
+    result = output()
+    msg1 = Float64()
+    msg2 = Float64()
+
+    msg1.data = result[0]
+    msg2.data = result[1]
+
+    pub1.publish(msg1)
+    pub2.publish(msg2)
+    # n1.append(result[0])
+    # n2.append(result[1])
+    rate.sleep()
+
+
+# plt.plot(time, n1)
+# plt.plot(time, n2)
+# plt.show()

test.py

@@ -0,0 +1,161 @@
+#!/usr/bin/env python3
+import rospy
+from cmath import pi
+import numpy as np
+from std_msgs.msg import Float32MultiArray, Float64
+
+rospy.init_node("test")
+angl = 0
+r = rospy.Rate(76800)
+def convangs(arr):
+	jts = np.array(np.float_(arr))
+
+	return jts #returns a np array after converting string array to a float array
+
+def normalize_pi(j):
+	for j_ in j:
+		while 1:
+			if j_>360:
+				j_-=360
+			elif j_<0:
+				j_+=360
+			else:
+				break
+
+# file = open("/home/erc/catkin_ws/src/spiderbot/src/oneleg.txt",'r')
+# lines = file.readlines()
+def call1(msg):
+	global angl
+	angl = msg.data 
+	angl = angl*180/np.pi
+	angl = angl + 90
+	#print(angl)
+
+sub = rospy.Subscriber("leg1",Float64,call1)
+
+pub=rospy.Publisher("/leg123",Float32MultiArray,queue_size = 5)
+pub2=rospy.Publisher("/leg456",Float32MultiArray,queue_size = 5)
+
+r = rospy.Rate(10)
+#for line in lines:
+	#x = line.split(",",)
+	#x.pop(-1)
+
+	# j = convangs(x) #converts string array to float array
+	# j = j*(180/pi)  #multiply each element of np array with (180/pi)
+	# # j = normalize_pi(j)
+
+# Variables for leg angles for leg 1 2 3
+l1j1 = 90
+l1j2 = 110
+l1j3 = 105
+l2j1 = 90
+l2j2 = 110
+l2j3 = 105
+l3j1 = 90
+l3j2 = 110
+l3j3 = 105
+
+# Variables for leg angles for leg 4 5 6
+l6j3 = 105
+l6j2 = 110
+l6j1 = 90
+l5j3 = 105
+l5j1 = 90
+l5j2 = 110
+
+l4j3 = 105
+l4j2 = 110
+l4j1 = 90
+
+# Variables for leg angles for leg 1 2 3
+#l1j1 = 0
+#l1j2 = 0
+#l1j3 = 0
+#l2j1 = 0
+#l2j2 = 0
+#l2j3 = 0
+#l3j1 = 0
+#l3j2 = 0
+#l3j3 = 0
+
+# Variables for leg angles for leg 4 5 6
+#l6j3 = 0
+#l6j2 = 0
+#l6j1 = 0
+#l5j3 = 0
+#l5j1 = 0
+#l5j2 = 0
+
+#l4j3 = 0
+#l4j2 = 0
+#l4j1 = 0
+
+leg1step1 = [90, 60, 145, 120, 85, 145, 90, 60, 145]
+leg2step1 = [100, 85, 90, 145, 60, 90, 145, 85, 60]
+
+leg1step2 = [90, 85, 145, 90, 85, 145, 90, 85, 145]
+leg2step2 = [145, 85, 90, 145, 85, 90, 145, 85, 90]
+
+leg1step3 = [120, 85, 145, 90, 60, 145, 90, 85, 100]
+leg2step3 = [145, 60, 90, 145, 85, 60, 145, 60, 90]
+
+leg1step4 = [90, 85, 145, 90, 60, 145, 90, 85, 145]
+leg2step4 = [145, 60, 90, 145, 85, 90, 145, 60, 90]
+
+
+msg=Float32MultiArray()
+msg2=Float32MultiArray()
+#j=[l1j1,l1j2,l1j3,l2j1,l2j2,l2j3,l3j1,l3j2,l3j3]
+#j=[angl,l1j2,l1j3,l2j1,l2j2,l2j3,l3j1,l3j2,l3j3]
+# j2 starts with l6j3
+j2=[l6j3,l6j2,l6j1,l5j3,l5j2,l5j1,l4j3,l4j2,l4j1]
+# j2=[90,85,145,90,85,145,90,85,145][::-1]
+#j=[90,90,90,90,90,90,90,90,90]
+#j2=[90,90,90,90,90,90,90,90,90]
+# j=[0,0,0,0,0,0,0,0,0]
+# j2=[0,0,0,0,0,0,0,0,0]
+#j=[90,85,145,90,50,0,90,85,145]
+#j2=[90,50,0,90,85,145,90,50,0]
+#j3=[70,85,145,70,85,145,70,85,145,70,85,145,70,85,145,70,85,145]
+#j5=[60,75,150,60,75,150,60,75,150]
+#j6=[60,75,165,60,75,165,60,75,165]
+
+count = 0
+
+while not rospy.is_shutdown():
+	j=[angl,l1j2,l1j3,l2j1,l2j2,l2j3,l3j1,l3j2,l3j3]
+	if count == 0:	
+		msg.data = j
+		msg2.data = j2
+		#count=1
+	elif count == 1:
+		msg.data = leg1step1
+		msg2.data = leg2step1
+		count = 2
+	elif count == 2:
+		msg.data = leg1step2
+		msg2.data = leg2step2
+		count=3
+	elif count == 3:
+		msg.data = leg1step3
+		msg2.data = leg2step3
+		count=4
+	elif count == 4:
+		msg.data = leg1step4
+		msg2.data = leg2step4
+		count = 0
+
+	print(msg)
+	print(msg2)
+	print(count)
+	pub.publish(msg)
+	pub2.publish(msg2)
+	#print(angl)
+	#rospy.sleep(2)
+	r.sleep()
+
+
+# file.close()
+
+