def run():
rospy.init_node('euleranglecal', anonymous=True)
rospy.wait_for_service('foundations_hw2/interpolate_problem')
getinter = rospy.ServiceProxy('foundations_hw2/interpolate_problem',
Interpolate)
try:
interp = getinter()
except rospy.ServiceException as exc:
print("service not available")
global q1, q2, pub, times, t, count, timer
euler = EulerAngles()
euler.phi = interp.initial.phi
euler.theta = interp.initial.theta
euler.psi = interp.initial.psi
t = 0
q1 = quaternion_from_euler(euler.psi, euler.theta, euler.phi)
q2 = quaternion_from_euler(interp.final.psi, interp.final.theta,
interp.final.phi)
rospy.loginfo("following is the final place")
rospy.loginfo(interp.final.psi)
rospy.loginfo(interp.final.theta)
rospy.loginfo(interp.final.phi)
times = interp.seconds
pub = rospy.Publisher('vrep/shape_pose', EulerAngles, queue_size=10)
timer = rospy.Timer(rospy.Duration(0.1), callback)
rospy.spin()
def euler_angle():
rospy.wait_for_service('foundations_hw2/interpolate_problem')
try:
rospy.init_node("hw2p4a", anonymous=True)
haha = rospy.Publisher("vrep/shape_pose", EulerAngles, queue_size=10)
rate = rospy.Rate(10)
get_EA = rospy.ServiceProxy('foundations_hw2/interpolate_problem',
Interpolate)()
EA_initial = get_EA.initial
EA_final = get_EA.final
given_second = get_EA.seconds
delta_phi = (EA_final.phi - EA_initial.phi) / (given_second * 10)
delta_theta = (EA_final.theta - EA_initial.theta) / (given_second * 10)
delta_psi = (EA_final.psi - EA_initial.psi) / (given_second * 10)
EA = EulerAngles()
print("in the show")
for i in range(int(given_second) * 10):
EA.phi = EA_initial.phi + delta_phi * i
EA.theta = EA_initial.theta + delta_theta * i
EA.psi = EA_initial.psi + delta_psi * i
haha.publish(EA)
rospy.loginfo(EA)
rate.sleep()
rospy.spin()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def p4a():
rospy.init_node('p4a')
rospy.wait_for_service('foundations_hw2/interpolate_problem')
test = rospy.ServiceProxy('foundations_hw2/interpolate_problem', Interpolate)
ini=test().initial
fin=test().final
sec=test().seconds*10
step1=(fin.phi-ini.phi)/sec
step2=(fin.theta-ini.theta)/sec
step3=(fin.psi-ini.psi)/sec
num=0
i_phi=[]
i_theta=[]
i_psi=[]
while True:
i_phi.append(ini.phi+num*step1)
i_theta.append(ini.theta+num*step2)
i_psi.append(ini.psi+num*step3)
num=num+1
if num>sec:
break
pub=rospy.Publisher('vrep/shape_pose', EulerAngles, queue_size=10)
rate = rospy.Rate(10)
i=0
while not rospy.is_shutdown():
if i>=sec:
break
ea=EulerAngles(i_phi[i],i_theta[i],i_psi[i])
pub.publish(ea)
rospy.loginfo(ea)
i=i+1
rate.sleep()
rospy.spin()
def callback(event):
global q1, q2, pub, count, times, timer, t
t = t + 0.1
rospy.loginfo(t)
q3 = slerp(q1, q2, t / times)
(x, y, z) = euler_from_quaternion(q3)
eu = EulerAngles()
rospy.loginfo(x)
rospy.loginfo(y)
rospy.loginfo(z)
eu.psi = x
eu.theta = y
eu.phi = z
pub.publish(eu)
if (t >= times):
timer.shutdown()
def run():
rospy.init_node('euleranglecal', anonymous=True)
rospy.wait_for_service('foundations_hw2/interpolate_problem')
getinter = rospy.ServiceProxy('foundations_hw2/interpolate_problem',
Interpolate)
try:
interp = getinter()
except rospy.ServiceException as exc:
print("service not available")
global dphi, dtheta, dpsi, euler, pub, times, count, timer
count = 0
euler = EulerAngles()
euler.phi = interp.initial.phi
euler.theta = interp.initial.theta
euler.psi = interp.initial.psi
times = interp.seconds * 10
dphi = (interp.final.phi - interp.initial.phi) / (interp.seconds * 10)
dtheta = (interp.final.theta - interp.initial.theta) / (interp.seconds *
10)
dpsi = (interp.final.psi - interp.initial.psi) / (interp.seconds * 10)
pub = rospy.Publisher('vrep/shape_pose', EulerAngles, queue_size=10)
timer = rospy.Timer(rospy.Duration(0.1), callback)
rospy.spin()
def quaternion():
rospy.wait_for_service('foundations_hw2/interpolate_problem')
try:
rospy.init_node("hw2p4b", anonymous=True)
haha = rospy.Publisher("vrep/shape_pose", EulerAngles, queue_size=10)
rate = rospy.Rate(10)
get_EA = rospy.ServiceProxy('foundations_hw2/interpolate_problem',
Interpolate)()
EA_initial = get_EA.initial #phi for z, theta for y, psi for x
EA_final = get_EA.final
ini_quaternion = quaternion_from_euler(EA_initial.psi,
EA_initial.theta,
EA_initial.phi)
fin_quaternion = quaternion_from_euler(EA_final.psi, EA_final.theta,
EA_final.phi)
given_second = int(get_EA.seconds)
t = 0.0
print("in the show")
for i in range(given_second * 10):
t += 1.0 / (given_second * 10)
qm = slerp(ini_quaternion, fin_quaternion, t)
euler = euler_from_quaternion(qm)
tmp_EA = EulerAngles()
tmp_EA.psi = euler[0]
tmp_EA.theta = euler[1]
tmp_EA.phi = euler[2]
haha.publish(tmp_EA)
rospy.loginfo(tmp_EA)
rate.sleep()
rospy.spin()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def p4b():
rospy.init_node('p4b')
rospy.wait_for_service('foundations_hw2/interpolate_problem')
test = rospy.ServiceProxy('foundations_hw2/interpolate_problem', Interpolate)
ini=test().initial
fin=test().final
sec=test().seconds*10
q_ini=mat([[math.cos(ini.phi/2)*math.cos(ini.theta/2)*math.cos(ini.psi/2)+math.sin(ini.phi/2)*math.sin(ini.theta/2)*math.sin(ini.psi/2)],[math.sin(ini.phi/2)*math.cos(ini.theta/2)*math.cos(ini.psi/2)-math.cos(ini.phi/2)*math.sin(ini.theta/2)*math.sin(ini.psi/2)],[math.cos(ini.phi/2)*math.sin(ini.theta/2)*math.cos(ini.psi/2)+math.sin(ini.phi/2)*math.cos(ini.theta/2)*math.sin(ini.psi/2)],[math.cos(ini.phi/2)*math.cos(ini.theta/2)*math.sin(ini.psi/2)-math.sin(ini.phi/2)*math.sin(ini.theta/2)*math.cos(ini.psi/2)]])
q_fin=mat([[math.cos(fin.phi/2)*math.cos(fin.theta/2)*math.cos(fin.psi/2)+math.sin(fin.phi/2)*math.sin(fin.theta/2)*math.sin(fin.psi/2)],[math.sin(fin.phi/2)*math.cos(fin.theta/2)*math.cos(fin.psi/2)-math.cos(fin.phi/2)*math.sin(fin.theta/2)*math.sin(fin.psi/2)],[math.cos(fin.phi/2)*math.sin(fin.theta/2)*math.cos(fin.psi/2)+math.sin(fin.phi/2)*math.cos(fin.theta/2)*math.sin(fin.psi/2)],[math.cos(fin.phi/2)*math.cos(fin.theta/2)*math.sin(fin.psi/2)-math.sin(fin.phi/2)*math.sin(fin.theta/2)*math.cos(fin.psi/2)]])
#abini=q_ini[1][0]*q_ini[1][0]+q_ini[2][0]*q_ini[2][0]+q_ini[3][0]*q_ini[3][0]
#inif=mat([[],[],[],[]])
i_phi=[]
i_theta=[]
i_psi=[]
i=1
while i<sec:
t=sec/i
result=mat([[],[],[],[]])
ad=mat([[-1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,-1]])
c=q_ini.T*q_fin
if c<0:
q_fin=ad*q_fin
c=-c
if abs(c)>0.9995:
k0=mat([[1-1/t,0,0,0],[0,1-1/t,0,0],[0,0,1-1/t,0],[0,0,0,1-1/t]])
k1=mat([[1/t,0,0,0],[0,1/t,0,0],[0,0,1/t,0],[0,0,0,1/t]])
result=k0*q_ini+k1*q_fin
else:
s=math.pow(1-c*c,0.5)
o=math.atan2(s,c)
os=1/s
k0=mat([[sin((1-1/t)*o)*os,0,0,0],[0,sin((1-1/t)*o)*os,0,0],[0,0,sin((1-1/t)*o)*os,0],[0,0,0,sin((1-1/t)*o)*os]])
k1=mat([[sin(o/t)*os,0,0,0],[0,sin(o/t)*os,0,0],[0,0,sin(o/t)*os,0],[0,0,0,sin(o/t)*os]])
result=k0*q_ini+k1*q_fin
w=result[0][0]
x=result[1][0]
y=result[2][0]
z=result[3][0]
sqw=w*w
sqx=x*x
sqy=y*y
sqz=z*z
unit=sqx+sqy+sqz+sqw
test=x*y+z*w
heading=attitude=bank=0
if test>0.499*unit:
heading=2*math.atan2(x,w)
attitude=math.pi/2
bank=0
elif test<-0.499*unit:
heading=-2*math.atan2(x,w)
attitude=-math.pi/2
bank=0
else:
heading=math.atan2(2*y*w-2*x*z,1-2*sqy-2*sqz)
attitude=math.asin(2*test/unit)
bank=math.atan2(2*x*w-2*y*z,1-2*sqx-2*sqz)
i_phi.append(bank)
i_theta.append(attitude)
i_psi.append(heading)
i=i+1
pub=rospy.Publisher('vrep/shape_pose', EulerAngles, queue_size=10)
rate = rospy.Rate(10)
i=0
while not rospy.is_shutdown():
if i>=sec-1:
break
ea=EulerAngles(i_phi[i],i_theta[i],i_psi[i])
pub.publish(ea)
rospy.loginfo(ea)
i=i+1
rate.sleep()
rospy.spin()