def test_m44_default_sparse(self):
test_name = 'mm44_default_sparse.txt'
print("Test: ", test_name)
# QP Options
options = Options()
options.setToDefault()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 0, 'One ore more tests failed.'
def test_m44_default_sparse(self):
test_name = 'mm44_default_sparse.txt'
print("Test: ", test_name)
# QP Options
options = Options()
options.setToDefault()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 0, 'One ore more tests failed.'
def qpoases_calculation(self, error_posit):
# Variable initialization
self._feedback.sim_trajectory = [self.current_state]
error_orient = np.array([0.0, 0.0, 0.0])
eef_pose_array = PoseArray()
reached_orientation = False
self.trajectory_length = 0.0
self.old_dist = 0.0
# Setting up QProblem object
vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0])
options = Options()
options.setToDefault()
options.printLevel = PrintLevel.LOW
vel_calculation.setOptions(options)
Opt = np.zeros(self.problem_size[1])
i = 0
# Config iai_naive_kinematics_sim, send commands
clock = ProjectionClock()
velocity_msg = JointState()
velocity_msg.name = self.all_joint_names
velocity_msg.header.stamp = rospy.get_rostime()
velocity_array = [0.0 for x in range(len(self.all_joint_names))]
effort_array = [0.0 for x in range(len(self.all_joint_names))]
velocity_msg.velocity = velocity_array
velocity_msg.effort = effort_array
velocity_msg.position = effort_array
clock.now = rospy.get_rostime()
clock.period.nsecs = 1000000
self.pub_clock.publish(clock)
# Plot for debugging
'''t = np.array([i])
base_weight = np.array(self.jweights[0, 0])
arm_weight = np.array(self.jweights[4, 4])
triang_weight = np.array(self.jweights[3, 3])
low, pos, high, vel_p, error = [], [], [], [], []
error = np.array([0])
for x in range(8+3):
low.append(np.array([self.joint_limits_lower[x]]))
pos.append(np.array(self.joint_values[x]))
vel_p.append(np.array(Opt[x]))
high.append(np.array([self.joint_limits_upper[x]]))#'''
tic = rospy.get_rostime()
while not self.reach_position or not reached_orientation or not self.reach_pregrasp:
i += 1
# Check if client cancelled goal
if not self.active_goal_flag:
self.success = False
break
if i > 500:
self.abort_goal('time_out')
break
# Solve QP, redefine limit vectors of A.
eef_pose_msg = Pose()
nWSR = np.array([100])
[ac_lim_lower, ac_lim_upper] = self.acceleration_limits(Opt)
self.joint_dist_lower_lim = self.joint_limits_lower - self.joint_values
self.joint_dist_upper_lim = self.joint_limits_upper - self.joint_values
self.lbA = np.hstack((error_posit, error_orient,
self.joint_dist_lower_lim, ac_lim_lower))
self.ubA = np.hstack((error_posit, error_orient,
self.joint_dist_upper_lim, ac_lim_upper))
# Recalculate H matrix
self.calculate_weigths(error_posit)
vel_calculation = SQProblem(self.problem_size[1],
self.problem_size[0])
vel_calculation.setOptions(options)
return_value = vel_calculation.init(self.H, self.g, self.A,
self.lb, self.ub, self.lbA,
self.ubA, nWSR)
vel_calculation.getPrimalSolution(Opt)
if return_value != returnValue.SUCCESSFUL_RETURN:
rospy.logerr("QP-Problem returned without success! ")
print 'joint limits: ', self.joint_limits_lower
print 'joint values: ', self.joint_values
print 'position error:', error_posit / self.prop_gain
print 'eef ori: [%.3f %.3f %.3f] ' % (
self.eef_pose.p[0], self.eef_pose.p[1], self.eef_pose.p[2])
print 'goal ori : [%.3f %.3f %.3f] ' % (
self.goal_orient_euler[0],
self.goal_orient_euler[0],
self.goal_orient_euler[0],
)
self.abort_goal('infeasible')
break
for x, vel in enumerate(Opt[4:self.nJoints]):
velocity_msg.velocity[self.start_arm[x]] = vel
velocity_msg.velocity[self.triang_list_pos] = Opt[3]
velocity_msg.velocity[0] = Opt[0]
velocity_msg.velocity[1] = Opt[1]
# Recalculate Error in EEF position
error_posit, limit_p = self.calc_posit_error(error_posit)
# Recalculate Error in EEF orientation
eef_orient = qo.rotation_to_quaternion(self.eef_pose.M)
error_orient, reached_orientation = self.calc_orient_error(
eef_orient, self.goal_quat, 0.35, limit_p)
# Get trajectory length
self.trajectory_length = self.get_trajectory_length(
self.eef_pose.p, self.trajectory_length)
# Print velocity for iai_naive_kinematics_sim
velocity_msg.header.stamp = rospy.get_rostime()
self.pub_velocity.publish(velocity_msg)
clock.now = rospy.get_rostime()
self.pub_clock.publish(clock)
# Action feedback
self.action_status.status = 1
self._feedback.sim_status = self.action_status.text = 'Calculating trajectory'
self._feedback.sim_trajectory.append(self.current_state)
self._feedback.trajectory_length = self.trajectory_length
self.action_server.publish_feedback(self.action_status,
self._feedback)
self.action_server.publish_status()
self.success = True
# Store EEF pose for plotting
eef_pose_msg.position.x = self.eef_pose.p[0]
eef_pose_msg.position.y = self.eef_pose.p[1]
eef_pose_msg.position.z = self.eef_pose.p[2]
eef_pose_msg.orientation.x = eef_orient[0]
eef_pose_msg.orientation.y = eef_orient[1]
eef_pose_msg.orientation.z = eef_orient[2]
eef_pose_msg.orientation.w = eef_orient[3]
eef_pose_array.poses.append(eef_pose_msg)
# Plot for debuging
'''for x in range(8+3):
low[x] = np.hstack((low[x], self.joint_limits_lower[x]))
pos[x] = np.hstack((pos[x], self.joint_values[x]))
vel_p[x] = np.hstack((vel_p[x], Opt[x]))
high[x] = np.hstack((high[x], self.joint_limits_upper[x]))
e = error_posit / self.prop_gain
e_p = sqrt(pow(e[0], 2) + pow(e[1], 2) + pow(e[2], 2))
error = np.hstack((error, e_p))
base_weight = np.hstack((base_weight, self.jweights[0, 0]))
arm_weight = np.hstack((arm_weight, self.jweights[4, 4]))
triang_weight = np.hstack((triang_weight, self.jweights[3, 3]))
t = np.hstack((t, i))#'''
# print '\n iter: ', i
# goal = euler_from_quaternion(self.goal_quat)
# print 'joint_vel: ', Opt[:-6]
# print 'error pos: ', error_posit / self.prop_gain
# print 'eef ori: [%.3f %.3f %.3f] '%(self.eef_pose.p[0],self.eef_pose.p[1], self.eef_pose.p[2])
# print 'ori error: [%.3f %.3f %.3f] '%(error_orient[0], error_orient[1], error_orient[2])
# print 'goal ori : [%.3f %.3f %.3f] '%(goal[0], goal[1], goal[2])
# print 'slack : ', Opt[-6:]
self.final_error = sqrt(
pow(error_posit[0], 2) + pow(error_posit[1], 2) +
pow(error_posit[2], 2))
eef_pose_array.header.stamp = rospy.get_rostime()
eef_pose_array.header.frame_id = self.gripper
self.pub_plot.publish(eef_pose_array)
# Plot
'''plt.style.use('ggplot')
plt.plot(t, arm_weight, 'g-.', lw=3, label='arm')
plt.plot(t, base_weight, 'k--', lw=3, label='base')
plt.plot(t, triang_weight, 'm', lw=2, label='torso')
plt.xlabel('Iterations')
plt.ylabel('Weights')
plt.title('Arm, base and torso weights')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('weights.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
plt.plot(t, error, 'k', lw=2)
plt.xlabel('Iterations')
plt.ylabel('Position Error [m]')
plt.title('Position Error')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('error.tex', figureheight='4cm', figurewidth='16cm')
plt.cla()#'''
'''for x in range(8+3):
plt.plot(t, low[x], lw=1)
plt.plot(t, pos[x], lw=3)
plt.plot(t, vel_p[x], lw=3)
plt.plot(t, high[x], lw=1)
plt.xlabel('Iterations')
plt.ylabel('Position / Velocity')
plt.grid(True)
# plt.show()
tikz_save('joint'+str(x)+'.tex', figureheight='5cm', figurewidth='4.5cm')
plt.cla()'''
'''plt.plot(t, pos[0], 'r--', lw=3, label='x')
plt.plot(t, pos[1], 'b', lw=2, label='y')
plt.xlabel('Iterations')
plt.ylabel('Base Position [m]')
plt.title('Trajectory of the base [m]')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('base_pos.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
step = 2
plt.plot(t[0:-1:step], pos[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0')
plt.plot(t[0:-1:step], pos[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1')
plt.plot(t[0:-1:step], pos[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2')
plt.plot(t[0:-1:step], pos[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3')
plt.plot(t, pos[7], 'k-.', lw=3, label='j4')
plt.plot(t, pos[8], '--', lw=3, label='j5')
plt.plot(t, pos[9], lw=3, label='j6')
plt.xlabel('Iterations')
plt.ylabel('Position [rad]')
plt.title('Trajectory of the joints')
plt.grid(True)
plt.legend(loc='upper left')
tikz_save('joint_pos.tex', figureheight='8cm', figurewidth='16cm')
plt.cla()
plt.plot(t, vel_p[0], 'r--', lw=3, label='x')
plt.plot(t, vel_p[1], 'b',lw=2, label='y')
plt.xlabel('Iterations')
plt.ylabel('Base Velocity [m/sec]')
plt.title('Velocity of the base')
plt.grid(True)
plt.legend()
tikz_save('base_vel.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
plt.plot(t[0:-1:step], vel_p[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0')
plt.plot(t[0:-1:step], vel_p[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1')
plt.plot(t[0:-1:step], vel_p[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2')
plt.plot(t[0:-1:step], vel_p[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3')
plt.plot(t, vel_p[7], 'k-.', lw=3, label='j4')
plt.plot(t, vel_p[8], '--', lw=3, label='j5')
plt.plot(t, vel_p[9], lw=3, label='j6')
plt.xlabel('Iterations')
plt.ylabel('Arms Joints Velocity [rad/sec]')
plt.title("Velocity of the arm's joints")
plt.legend(loc='upper left')
plt.grid(True)
tikz_save('joint_vel.tex', figureheight='8cm', figurewidth='16cm')
plt.cla()#'''
toc = rospy.get_rostime()
print(toc.secs - tic.secs), 'sec Elapsed'
return 0
