Commit 03008361 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scripts] update to use of array instead of matrix

parent c8e717ed
......@@ -87,7 +87,7 @@ q_goal [0:3] = [radius*np.sin(alpha), -radius*np.cos(alpha), 0.465]
vx = np.matrix([1,0,0]).T
v_goal = np.matrix([q_goal[0],q_goal[1],0]).T
quat = Quaternion.FromTwoVectors(vx,v_goal)
q_goal[3:7] = quat.coeffs().T.tolist()[0]
q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
q_goal[-6] = vGoal*np.sin(alpha)
q_goal[-5] = -vGoal*np.cos(alpha)
......
......@@ -93,7 +93,7 @@ q_goal[0:3] = [random.uniform(X_BOUNDS[0],X_BOUNDS[1]),random.uniform(Y_BOUNDS[0
vx = np.matrix([1,0,0]).T
v_init = np.matrix([q_goal[0]-q_init[0],q_goal[1]-q_init[1],0]).T
quat = Quaternion.FromTwoVectors(vx,v_init)
q_init[3:7] = quat.coeffs().T.tolist()[0]
q_init[3:7] = quat.coeffs().tolist()
q_goal[3:7] = q_init[3:7]
......
......@@ -88,7 +88,7 @@ q_goal [0:3] = [radius*np.sin(alpha), -radius*np.cos(alpha), 1.]
vx = np.matrix([1,0,0]).T
v_goal = np.matrix([q_goal[0],q_goal[1],0]).T
quat = Quaternion.FromTwoVectors(vx,v_goal)
q_goal[3:7] = quat.coeffs().T.tolist()[0]
q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
q_goal[-6] = vGoal*np.sin(alpha)
q_goal[-5] = -vGoal*np.cos(alpha)
......
......@@ -109,7 +109,7 @@ y_goal = random.uniform(y_bound_goal[0],y_bound_goal[1])
vx = np.matrix([1,0,0]).T
v_init = np.matrix([x_goal-x_init,y_goal-y_init,0]).T
quat = Quaternion.FromTwoVectors(vx,v_init)
q_init[3:7] = quat.coeffs().T.tolist()[0]
q_init[3:7] = quat.coeffs().tolist()
q_goal=q_init[::]
q_goal[0:2] = [x_goal,y_goal]
......
......@@ -101,7 +101,7 @@ else :
# set final orientation to be along the circle :
vx = np.matrix([1,0,0]).T
quat = Quaternion.FromTwoVectors(vx,v_goal)
q_goal[3:7] = quat.coeffs().T.tolist()[0]
q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
q_goal[-6] = vGoal*np.sin(alpha)
q_goal[-5] = -vGoal*np.cos(alpha)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment