updated controls and stuff

src/controller.py

@@ -1,22 +1,74 @@
 #!/usr/bin/env python
 import rospy
-from std_msgs.msg import Float32
-from math import cos,sin
+from std_msgs.msg import Float32, Int16
+from math import cos,sin,atan,sqrt,pi
 import time
 from geometry_msgs.msg import Point
+from intera_core_msgs.msg import EndpointState
 class pen_controller(object):
 
     def __init__(self):
         rospy.loginfo("Creating pendulum controller class")
         self.ystate = rospy.Publisher("yPoint",Point, queue_size=3)
-        self.int_timer = rospy.Timer(rospy.Duration(1/100.), self.yPublish)
+        self.rolla = rospy.Subscriber("rollAngle",Float32,self.angle)
+        self.roll_vel = rospy.Subscriber("ang_vel",Int16,self.speed)
+        self.xP = rospy.Subscriber("/robot/limb/right/endpoint_state",EndpointState,self.cart_states)
+        self.xbegin = -.208
+        self.xPos = 0
+        self.xdot = 0
+        self.prevroll = 0
+        self.command_vel = 0
+        self.vel_limit = .5
+        self.dt = 0.01
+        self.xfinal = -.208
+        # self.xaccel = rospy.Subscriber("xAccel",Int16,self.ax)
+        # self.yaccel = rospy.Subscriber("yAccel",Int16,self.ay)
+        # self.zaccel = rospy.Subscriber("zAccel",Int16,self.az)
+        # self.xa = 0
+        # self.ya = 0
+        # self.za = 10
+        self.av = 0
+        self.roll = 0
+        self.int_timer = rospy.Timer(rospy.Duration(1/1000.), self.yPublish)
 
     def yPublish(self,tdat):
-        x = .3*sin(rospy.get_time()) + .4
-        y = cos(rospy.get_time())
-        # y = -.8
-        rospy.loginfo(y)
-        self.ystate.publish(x,y,0)
+        a = .1*(self.xPos-self.xbegin)+.5123*self.xdot + 22.6320*self.roll*(pi/180) + 7.2327*self.av*(pi/180)
+        # y = cos(rospy.get_time())
+        self.command_vel = self.command_vel + a*self.dt
+        if (self.command_vel > self.vel_limit):
+            self.command_vel = self.vel_limit
+        if (self.command_vel < -self.vel_limit):
+            self.command_vel = -self.vel_limit
+        self.xfinal =self.xfinal + self.command_vel*self.dt
+        rospy.loginfo(self.command_vel)
+        # if (self.xa is not None and self.ya is not None and self.za is not None):
+        #     roll = atan(sqrt(self.xa**2+self.ya**2)/self.za)*180/pi
+        # rospy.loginfo(self.roll)
+        self.ystate.publish(self.xfinal,self.command_vel,0)
+    def angle(self,rollAngle):
+        self.prevroll = self.roll
+        self.roll = -rollAngle.data
+        self.av = (self.roll-self.prevroll)/.01
+        # rospy.loginfo(self.av*pi/180)
+    # def ax(self,ax_data):
+    #     self.xa = ax_data.data
+    #     # rospy.loginfo(yacceleration)
+    #
+    # def ay(self,ay_data):
+    #     self.ya = ay_data.data
+    #     # rospy.loginfo(yacceleration)
+    #
+    # def az(self,az_data):
+    #     self.za = az_data.data
+    #     # rospy.loginfo(yacceleration)
+
+    def speed(self,ang_velocity):
+        # self.av = ang_velocity.data;
+        i = 0
+    def cart_states(self,x_states):
+        self.xPos = x_states.pose.position.y
+        self.xdot = x_states.twist.linear.y
+        # rospy.loginfo(self.xdot)
 
 def main():
     rospy.init_node('controller')

src/ik_horizontal.py

@@ -17,6 +17,7 @@ class IKhorizontal(object):
 
     def __init__(self):
         self.angles = [0,0,pi/2,-pi/2,pi/2,pi/2,0]
+        self.vely = 0
         self.ik_begin = False
         self.goal = mr.FKinBody(smr.M,smr.Blist,self.angles)
         self.goal[0:3,0:3] = np.eye(3)
@@ -33,26 +34,28 @@ def __init__(self):
         self.mutex = threading.Lock()
         self.yp = rospy.Subscriber("yPoint", Point, self.Pnt)
         self.jss = rospy.Subscriber("/robot/joint_states", JointState,self.get_js)
-        self.tim = rospy.Timer(rospy.Duration(1/100.), self.step_ik)
+        self.tim = rospy.Timer(rospy.Duration(1/1000.), self.step_ik)
         self.key = rospy.Timer(rospy.Duration(1/10.), self.key_press)
 
     def Pnt(self,points):
         # self.goal[2,3] = points.x
-        self.goal[1,3] = points.y
-
+        # self.goal[1,3] = points.x
+        self.vely = points.y
     def get_js(self,joint_st):
         self.js = joint_st.position[1:-1]
         # self.vel = joint_st.velocity[1:-1]
 
     def key_press(self,td):
         key = raw_input()
-        if (key == "s"):
-            # self.zero_time = rospy.get_time()
-            self.ik_begin = True
+        if (key == ""):
+            if (self.ik_begin == False):
+                self.ik_begin = True
+            else:
+                self.ik_begin = False
+                self.limb.move_to_joint_positions(self.start_angles)
+        # elif (key == "r"):
+        #     self.ik_begin = False
 
-        elif (key == "r"):
-            self.ik_begin = False
-            self.limb.move_to_joint_positions(self.start_angles)
             # velocity_list = np.array(self.vellist)
             # veltime = np.array(self.veltime)
             # plt.plot(range(0,velocity_list.shape[0]),velocity_list[:,0],label='wx')
@@ -81,6 +84,7 @@ def step_ik(self,tdat):
             err = np.dot(mr.TransInv(mr.FKinBody(smr.M, smr.Blist, current_js)), g_sd)
             # now convert this to a desired twist
             Vb = mr.se3ToVec(mr.MatrixLog6(err))
+            Vb[4] = self.vely
             # calculate body Jacobian at current config
             J_b = mr.JacobianBody(smr.Blist, current_js)
             # now calculate an appropriate joint angle velocity: