diff --git a/media/accelCalcFromVelSmoothSquared.fig b/media/accelCalcFromVelSmoothSquared.fig new file mode 100644 index 0000000..c3a6c5c Binary files /dev/null and b/media/accelCalcFromVelSmoothSquared.fig differ diff --git a/media/pos.fig b/media/pos.fig new file mode 100644 index 0000000..3a5b4e0 Binary files /dev/null and b/media/pos.fig differ diff --git a/media/vel.fig b/media/vel.fig new file mode 100644 index 0000000..3bf89d9 Binary files /dev/null and b/media/vel.fig differ diff --git a/media/velcalcsmoothsquared.fig b/media/velcalcsmoothsquared.fig new file mode 100644 index 0000000..b594d21 Binary files /dev/null and b/media/velcalcsmoothsquared.fig differ diff --git a/media/wrench_force_y.fig b/media/wrench_force_y.fig new file mode 100644 index 0000000..50ab3e2 Binary files /dev/null and b/media/wrench_force_y.fig differ diff --git a/src/bagfiles/endpos.bag b/src/bagfiles/endpos.bag new file mode 100644 index 0000000..7d62173 Binary files /dev/null and b/src/bagfiles/endpos.bag differ diff --git a/src/bagfiles/jointStates.bag b/src/bagfiles/jointStates.bag new file mode 100644 index 0000000..ab31848 Binary files /dev/null and b/src/bagfiles/jointStates.bag differ diff --git a/src/bagfiles/subset.bag b/src/bagfiles/subset.bag new file mode 100644 index 0000000..d682d6d Binary files /dev/null and b/src/bagfiles/subset.bag differ diff --git a/src/controller.py b/src/controller.py index 05cdbe6..2d0c0e7 100755 --- a/src/controller.py +++ b/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') diff --git a/src/ik_horizontal.py b/src/ik_horizontal.py index b42ba36..abb823e 100755 --- a/src/ik_horizontal.py +++ b/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: