def tel():
n = 250
r = 0.5
x0 = -0.25
y0 = 1
# star = Trajectory.Star2D(n,5,r,x0,y0)
circle = Trajectory.Circle2D(n, r, x0, y0, arclength=2 * np.pi)
# nPoly = Trajectory.nPoly2D(n,4,r,x0,y0)
# spiral = Trajectory.Spiral2D(n,0.6,0.1,x0,y0,6*np.pi)
x_tel = circle.xtest
pub = rospy.Publisher('teleop', numpy_msg(Floats), queue_size=10)
pub2 = rospy.Publisher('point', Point, queue_size=10)
R = 10
# R = rospy.get_param('~pub_rate')
rospy.init_node('Teleop_node')
rate = rospy.Rate(R)
i = 0
N = len(x_tel)
while not rospy.is_shutdown():
if i > N - 1:
i = 0
# x = np.array(x_tel[i],dtype = np.float64)
# x = np.array([float(x_tel[i,0]),float(x_tel[i,1]),float(0)])
# x.astype(np.float64)
# x = np.array([trans[0],trans[1]], dtype=numpy.float32)
# pub.publish(x)
newpoint = Point()
newpoint.x = x_tel[i, 0]
newpoint.y = x_tel[i, 1]
newpoint.z = float(i)
pub2.publish(newpoint)
# rospy.loginfo(newpoint)
i += 1
rate.sleep()
"Test Settings" n = 200 # number of points in trajetory r =0.3 # radius of trajectory YStart = np.column_stack(np.deg2rad([-30,45,30])) # initial joint position #XStart = robot.fkin(YStart) # initial taskspace position XStart,_ = robot2D.fkin(YStart) # initial taskspace position x0 = XStart[0][0]-r # trajectory x center y0 = XStart[0][1] # y trajectory y center "Choose Which Test Trajectory" Test = TestTrajectory(Trajectory.Circle2D(n,r,x0,y0,arclength = 2*np.pi), robot2D) #Test = TestTrajectory(Trajectory.nPoly2D(n,4,r,x0,y0, arclength = -2*np.pi,flip =0), robot) #Test = TestTrajectory(Trajectory.Rect2D(n, width = 1, height = 1), robot) #Test = TestTrajectory(Trajectory.Star2D(n,5,r,x0,y0), robot) #Test = TestTrajectory(Trajectory.Spiral2D(n,1,0.5,x0,y0,6*np.pi), robot) workspace = Trajectory.Circle2D(n,np.sum(robot2D.dh_table[:,1]),0,0) # size of reachable workspace X_test = Test.xtest #Y_test = Test.ytest # if inverse kinematics is provided njit = 25 #number of jittered data points num_inducing = 10 # max number of induced/support points per model " Initialize Local Models"
#YStart = rospy.get_param('~YStart')
#YStart = np.array(np.deg2rad(YStart)).reshape(1,2)
YStart = np.array(np.deg2rad([0, 45])).reshape(1, 2)
XStart = twolink.fkin(YStart)
#data = rospy.wait_for_message('experience',Point)
#XStart = np.array([data.x,data.y]).reshape(1,2)
x0 = XStart[0][0] - r
y0 = XStart[0][1]
#x0 = -0.25
#y0 = 1
# star = Trajectory.Star2D(n,5,r,x0,y0)
circle = Trajectory.Circle2D(n, r, x0, y0, arclength=2 * np.pi)
# nPoly = Trajectory.nPoly2D(n,4,r,x0,y0)
# spiral = Trajectory.Spiral2D(n,0.6,0.1,x0,y0,6*np.pi)
x_tel = circle.xtest
def nextPoint(resp):
i = resp.i % n
x = x_tel[i, 0]
y = x_tel[i, 1]
z = float(resp.i)
print([x, y])
return x, y, z
XStart = twolink.fkin(YStart)
x0 = XStart[0][0] - r
y0 = XStart[0][1]
#x0 = -.25
#y0 = -1
#twolink = RobotModels.TwoLink2D(l1,l2)
#Test = TestTrajectory(Trajectory.Circle2D(n,r,x0,y0,arclength = 2*np.pi), twolink)
#Test = TestTrajectory(Trajectory.nPoly2D(n,4,r,x0,y0), twolink)
#Test = TestTrajectory(Trajectory.Rect2D(n, width = 1, height = 1), twolink)
Test = TestTrajectory(Trajectory.Star2D(n, 5, r, x0, y0), twolink)
#Test = TestTrajectory(Trajectory.Spiral2D(n,0.6,0.1,x0,y0,6*np.pi), twolink)
workspace = Trajectory.Circle2D(n, np.sum(Test.robot.Links), 0, 0)
X_test = Test.xtest
#Y_test = YStart
Y_test = Test.ytest
njit = 25
#[XI,YI] = jitter(njit,Y_test[0],Test.robot)
#
num_inducing = 25
" Initialize Local Models"
local = LocalModels(num_inducing,
wgen=0.975,
ndim=Test.robot.num_links,
robot=Test.robot)
