Add files via upload

control.yaml

@@ -0,0 +1,18 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+imu_joint1_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: imu_joint1
+  pid: {p: 3.0, i: 1.0, d: 1.0}
+
+imu_joint2_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: imu_joint2
+  pid: {p: 5.0, i: 1.5, d: 1.25}
+
+imu_joint3_controller:
+  type: velocity_controllers/JointVelocityController
+  joint: imu_joint3
+  pid: {p: 1.2, i: 0.4, d: 0}

gazebo.launch

@@ -0,0 +1,41 @@
+<?xml	version="1.0" encoding="UTF-8"?>
+<launch>
+		<!--	these	are	the	arguments	you	can	pass	this	launch	file,	for	example	
+paused:=true	-->
+		<arg	name="paused"	default="true"	/>
+		<arg	name="use_sim_time"	default="false"	/>
+		<arg	name="gui"	default="true"	/>
+		<arg	name="headless"	default="false"	/>
+		<arg	name="debug"	default="false"	/>
+		<!--	We	resume	the	logic	in	empty_world.launch,	changing	only	the	name	of	the	
+world	to	be	launched	-->
+		<include	file="$(find	gazebo_ros)/launch/empty_world.launch">
+				<arg	name="world_name"	value="$(find	urdf_tutorial)/robot.world"	/>
+				<arg	name="debug"	value="$(arg	debug)"	/>
+				<arg	name="gui"	value="$(arg gui)"	/>
+				<arg	name="paused"	value="$(arg	paused)"	/>
+				<arg	name="use_sim_time"	value="$(arg	use_sim_time)"	/>
+				<arg	name="headless"	value="$(arg	headless)"	/>
+		</include>
+		<!--	Load	the	URDF	into	the	ROS	Parameter	Server	-->
+
+		<param	name="robot_description"	command="$(find	xacro)/xacro.py $(find urdf_tutorial)/wheel.xacro"/>
+                <rosparam file="$(find urdf_tutorial)/control.yaml" command="load"/>
+
+			Run	a	python	script	to	the	send	a	service	call	to	gazebo_ros	to	spawn	a	
+URDF	robot	
+		<node	name="urdf_spawner" pkg="gazebo_ros"	type="spawn_model"	respawn="false"	
+output="screen"	args="-urdf	-model	robot1	-param robot_description"	/>
+
+<node name="controller_spawn" pkg="controller_manager" type="spawner" output="screen" respawn="false"
+   args="/joint_state_controller
+         /imu_joint1_controller
+        /imu_joint2_controller
+        /imu_joint3_controller">
+</node>
+<node name="robot_states" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
+</node>
+
+		<node	name="urdf_spawner1" 	pkg="gazebo_ros"	type="spawn_model"	respawn="false"	
+output="screen"	args="-urdf	-model	robot2	-file $(find	urdf_tutorial)/ball.urdf"	/>
+</launch>

talker.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+import rospy
+import tf
+from math import *
+from std_msgs.msg import String
+from std_msgs.msg import Float64
+from sensor_msgs.msg import Imu
+from gazebo_msgs.msg import ModelStates
+rospy.init_node('talker', anonymous=True)
+pub1 = rospy.Publisher("/imu_joint1_controller/command", Float64, queue_size=10)
+pub2 = rospy.Publisher("/imu_joint2_controller/command", Float64, queue_size=10)
+pub3 = rospy.Publisher("/imu_joint3_controller/command", Float64, queue_size=10)
+rate = rospy.Rate(10) # 10hz
+
+erp=0
+epp=0
+eyp=0
+sumr=0
+sump=0
+sumy=0
+pos1=0
+pos2=0
+pos3=0
+def callback(data):
+    global erp
+    global epp
+    global eyp
+    global sumr
+    global sump
+    global sumy
+    global pos1
+    global pos2
+    global pos3
+    #rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.orientation)
+    x =  data.orientation.x
+    y =  data.orientation.y
+    z =  data.orientation.z
+    w =  data.orientation.w
+    l = [x,y,z,w]
+    roll, pitch, yaw = tf.transformations.euler_from_quaternion(l)
+    eroll = 0-roll
+    epitch = 0-pitch
+    eyaw = 0-yaw
+    kp=65
+    kd=80
+    ki=8
+    wx = kp*eroll + kd*(eroll-erp) + ki*((sumr))
+    wy = kp*epitch + kd*(epitch-epp) + ki*((sump))
+    wz = kp*eyaw + kd*(eyaw-eyp) + ki*((sumy))
+    erp = eroll
+    epp = epitch
+    eyp = eyaw
+    sumr+=eroll
+    sump+=epitch
+    sumy+=eyaw
+
+    w1= (0.333)*(wz + (1.414*((wx*cos(yaw))-(wy*sin(yaw)))))
+    w2= (0.333)*(wz+ ((1/1.414)*((sin(yaw)*((-1.732*wx)+wy))-(cos(yaw)*(wx+(1.732*wy))))))
+    w3= (0.333)*(wz+ ((1/1.414)*((sin(yaw)*((1.732*wx)+wy))+(cos(yaw)*(-wx+(1.732*wy))))))
+    pos1+=w1
+    pos2+=w2
+    pos3+=w3
+    rospy.loginfo(rospy.get_caller_id() + "I heard yo ass %s", pos1)
+    pub1.publish(w2)
+    pub2.publish(w3)
+    pub3.publish(w1)
+def listener():
+
+    # In ROS, nodes are uniquely named. If two nodes with the same
+    # node are launched, the previous one is kicked off. The
+    # anonymous=True flag means that rospy will choose a unique
+    # name for our 'listener' node so that multiple listeners can
+    # run simultaneously.
+    rospy.Subscriber("/imu_data", Imu, callback)
+
+    # spin() simply keeps python from exiting until this node is stopped
+
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        listener()
+    except rospy.ROSInterruptException:
+        pass