if target_turn > control_turn:
control_turn = min( target_turn, control_turn + 0.1 )
elif target_turn < control_turn:
control_turn = max( target_turn, control_turn - 0.1 )
else:
control_turn = target_turn
twist = TwistStamped();
for i in range(6):
dXnp[i] = dX[i]
if(changeframe!=1): # Change the frame of the veloccity
R=my.quaternion_matrix([rot[3],rot[0],rot[1],rot[2]])
A=np.concatenate((R[0:3][:,0:3],np.zeros(shape=(3,3))),1)
B=np.concatenate((np.zeros(shape=(3,3)),R[0:3][:,0:3]),1)
Screw=np.vstack((A,B))
dXnp=np.dot(Screw,dXnp)
twist.twist.linear.x = control_speed*dXnp[0]* direction;
twist.twist.linear.y = control_speed*dXnp[1]* direction;
twist.twist.linear.z = control_speed*dXnp[2]* direction;
twist.twist.angular.x = control_turn*dXnp[3]* direction;
twist.twist.angular.y = control_turn*dXnp[4]* direction;
twist.twist.angular.z = control_turn*dXnp[5]* direction;
pub.publish(twist)
#print("loop: {0}".format(count))
