def runTest(self):
rot=np.array([[-0.5057639,-0.1340537,0.8521928], \
[0.6456962,-0.7139224,0.2709081], \
[0.5720833,0.6872731,0.4476342]])
q_t = np.array([0.2387194, 0.4360402, 0.2933459, 0.8165967])
q = rox.R2q(rot)
np.testing.assert_allclose(q_t, q, atol=1e-6)
def plan_robot_motion(self, dxf_points, sines):
self.dxf_points = dxf_points
self.sines = sines
waypoints = []
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#wpose = self.move_group.get_current_pose().pose
#waypoints.append(wpose)
#state = self.robot.get_current_state()
#self.move_group.set_start_state(state)
for i in range(len(dxf_points)):
pose_goal = Pose()
rpy = [math.pi, 0.0, sines[i]]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - dxf_points[i][0]) * 0.0254
y_val = dxf_points[i][1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
print(pose_goal)
waypoints.append(pose_goal)
replan_value = 0
error_code = None
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
while (replan_value < 3 and fraction < 1.0):
print(fraction)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
replan_value += 1
print("WARNING Portion of plan failed, replanning")
return plan, fraction, waypoints
def runTest(self):
q_1 = np.array([0.63867877, 0.52251797, 0.56156573, 0.06089615])
q_2 = np.array([0.35764716, 0.61051424, 0.11540801, 0.69716703])
R_t = rox.q2R(q_1).dot(rox.q2R(q_2))
q_t = rox.R2q(R_t)
q = rox.quatproduct(q_1).dot(q_2).reshape((4, ))
np.testing.assert_allclose(q, q_t, atol=1e-6)
def test_geometry_util_array_types():
node = RR.RobotRaconteurNode()
node.SetLogLevelFromString("DEBUG")
node.Init()
try:
RRC.RegisterStdRobDefServiceTypes(node)
geom_util = GeometryUtil(node)
_do_array_test(geom_util.xy_to_vector2, geom_util.vector2_to_xy, (2, ),
"Vector2", node)
_do_array_test(geom_util.xyz_to_vector3, geom_util.vector3_to_xyz,
(3, ), "Vector3", node)
_do_array_test(geom_util.abgxyz_to_vector6,
geom_util.vector6_to_abgxyz, (6, ), "Vector6", node)
_do_array_test(geom_util.xy_to_point2d, geom_util.point2d_to_xy, (2, ),
"Point2D", node)
_do_array_test(geom_util.xyz_to_point, geom_util.point_to_xyz, (3, ),
"Point", node)
_do_array_test(geom_util.wh_to_size2d, geom_util.size2d_to_wh, (2, ),
"Size2D", node)
_do_array_test(geom_util.whd_to_size, geom_util.size_to_whd, (3, ),
"Size", node)
_do_array_test(geom_util.q_to_quaternion, geom_util.quaternion_to_q,
(3, ), "Quaternion", node,
lambda a: rox.R2q(rox.rpy2R(a)))
_do_array_test(geom_util.R_to_quaternion, geom_util.quaternion_to_R,
(3, ), "Quaternion", node, lambda a: rox.rpy2R(a))
_do_array_test(geom_util.rpy_to_quaternion,
geom_util.quaternion_to_rpy, (3, ), "Quaternion", node)
_do_array_test(geom_util.array_to_spatial_velocity,
geom_util.spatial_velocity_to_array, (6, ),
"SpatialVelocity", node)
_do_array_test(geom_util.array_to_spatial_acceleration,
geom_util.spatial_acceleration_to_array, (6, ),
"SpatialAcceleration", node)
_do_array_test(geom_util.array_to_wrench, geom_util.wrench_to_array,
(6, ), "Wrench", node)
finally:
node.Shutdown()
def move_to_seam_start(self, motion_point, sine):
pose_goal = Pose()
rpy = [math.pi, 0.0, sine - math.pi / 2]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - motion_point[0]) * 0.0254
y_val = motion_point[1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
pose_goal.position.x += 0.1
#self.robot.set_start_state_to_current_state()
self.move_group.set_pose_target(pose_goal)
result = self.move_group.go(wait=True)
def main():
# initialize EGM interface instance
egm = rpi_abb_irc5.EGM()
# initialize a robot instance
abb_robot = abb_irb6640_180_255_robot()
# desired force
#Fd = 1000
# desired velocity in y
#vdy = -0.5
# feedback gain
Kp = 0.0002
Kd = 0.0008
Ki = 0.0004
# time step
delta_t = 0.004
# initial configuration in degree
init = [-91.85, 2.53, 38.20, 0.00, 49.27, -1.85]
n = 2000
# quadprog to solve for joint velocity
quadprog = qp.QuadProg(abb_robot)
# force sensor interface
try:
if (len(sys.argv) < 2):
raise Exception('IP address of ATI Net F/T sensor required')
host = sys.argv[1]
netft = rpi_ati_net_ft.NET_FT(host)
netft.set_tare_from_ft()
netft.start_streaming()
except KeyboardInterrupt:
pass
####### trapezoidal desired force in z #######
tt = np.linspace(0, 4 * np.pi, n)
desired_f = np.zeros((1, n))
vdy = np.zeros((1, n))
# form trap force and trap motion
for i in range(n):
if tt[i] >= 0 and tt[i] < np.pi:
desired_f[0, i] = 50 + 302 * tt[i]
vdy[0, i] = -0.2 * tt[i]
elif tt[i] >= np.pi and tt[i] < 3 * np.pi:
desired_f[0, i] = 50 + 302 * np.pi
vdy[0, i] = -0.2 * np.pi
else:
desired_f[0, i] = 50 + 302 * np.pi - 302 * (tt[i] - 3 * np.pi)
vdy[0, i] = -0.2 * np.pi + 0.2 * (tt[i] - 3 * np.pi)
#plt.plot(vdy[0, :])
#plt.show()
######## change here ########
#pos = 0
#acce = 0
#v_l_pre = 0
#########
# output force
force_out = np.zeros((6, n))
# pos of eef
eef_pos = np.zeros((3, n))
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
# referece height of coupon that achieves desired force
z_ref = 0.89226
x_ref = 0
y_ref = -1.35626
# determine if robot has reached the initial configuration init
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print sum(np.rad2deg(state.joint_angles))- sum(init)
if np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init)) < 1e-3:
tag = False
time.sleep(1)
print '--------start force control--------'
### drain the force sensor buffer ###
for i in range(1000):
flag, ft = netft.read_ft_streaming(.1)
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
flag, ft = netft.read_ft_streaming(.1)
F0 = ft[5]
print F0
time.sleep(2)
cnt = 0
while cnt < n:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
# forward kinematics
pose = rox.fwdkin(abb_robot, state.joint_angles)
R = pose.R
flag, ft = netft.read_ft_streaming(.1)
# map torque/force from sensor frame to base frame
T_b = np.matmul(R, ft[0:3])
F_b = np.matmul(R, ft[3:None])
F = F_b[2] # first three torques and then three forces
Fd0 = 50
# do not start motion in y until robot barely touches coupon (50 N)
if F < Fd0 - 0.1 and cnt == 0:
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
# will shake if gain too large, here use 0.0002
v_z = Kp * 10 * (F - Fd0)
v_l = np.array([0, 0, v_z])
else:
# deadzone for Fx
if abs(F_b[0]) < 30:
F_x = 0
# deadzone for Fy
if abs(F_b[1]) < 30:
F_y = 0
v_x = Kp / 2 * (F_x - 0)
v_y = vdy[0, cnt] + Kp / 2 * (F_y - 0)
z = pose.p[2]
#print desired_f[0, cnt]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
v_z = Kp * (F - desired_f[0, cnt])
v_l = np.array([v_x, v_y, v_z])
force_out[:, cnt] = np.concatenate((T_b, F_b), axis=0)
eef_pos[:, cnt] = pose.p
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt += 1
print F
#### change here ####
#pos = pos + v_l[2]*delta_t
#acce = (v_l[2]-v_l_pre)/delta_t
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, v_l[0], v_l[1], v_l[2]])
# emergency stop if force too large
if abs(F) > 2000:
spatial_velocity_command = np.array([0, 0, 0, 0, 0, 0])
print "force too large, stop..."
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, state.joint_angles)
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
state.joint_angles, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = state.joint_angles + joints_vel * delta_t
egm.send_to_robot(q_c)
####### change here ########
#v_l_pre = v_l[2]
#if t_new - t_pre < delta_t:
# time.sleep(delta_t - t_new + t_pre)
if cnt == n:
csv_dat = np.hstack((desired_f.T, vdy.T, force_out.T, eef_pos.T,
eef_orien.T, tim.T))
np.savetxt(
'trap_force_trap_motion_020520.csv',
csv_dat,
fmt='%6.5f',
delimiter=',') #, header='desired joint, optimal input')
print "done"
def trajopt_plan(self,
target_pose,
json_config_str=None,
json_config_name=None,
target_joints=None):
with self.lock:
if (json_config_str is None and json_config_name is not None):
json_config_str = self.load_json_config(json_config_name)
robot = self.controller_commander.rox_robot
#vel_upper_lim = np.array(robot.joint_vel_limit) * speed_scalar
#vel_lower_lim = -vel_upper_lim
#joint_lower_limit = np.array(robot.joint_lower_limit)
#joint_upper_limit = np.array(robot.joint_upper_limit)
joint_names = robot.joint_names
joint_positions = self.controller_commander.get_current_joint_values(
)
if target_pose is not None:
p = PoseArray()
p.header.frame_id = "world"
p.header.stamp = rospy.Time.now()
p.poses.append(
rox_msg.transform2pose_msg(
self.controller_commander.compute_fk(joint_positions)))
p.poses.append(rox_msg.transform2pose_msg(target_pose))
self.waypoint_plotter.publish(p)
self.tesseract_env.setState(joint_names, joint_positions)
init_pos = self.tesseract_env.getCurrentJointValues()
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
np.reshape(init_pos, (1, 6)))
planner = tesseract.TrajOptPlanner()
manip = "move_group"
end_effector = "vacuum_gripper_tool"
pci = tesseract.ProblemConstructionInfo(self.tesseract_env)
pci.fromJson(json_config_str)
pci.kin = self.tesseract_env.getManipulator(manip)
pci.init_info.type = tesseract.InitInfo.STATIONARY
#pci.init_info.dt=0.5
if target_pose is not None:
#Final target_pose constraint
pose_constraint = tesseract.CartPoseTermInfo()
pose_constraint.term_type = tesseract.TT_CNT
pose_constraint.link = end_effector
pose_constraint.timestep = pci.basic_info.n_steps - 1
q = rox.R2q(target_pose.R)
pose_constraint.wxyz = np.array(q)
pose_constraint.xyz = np.array(target_pose.p)
pose_constraint.pos_coefs = np.array(
[1000000, 1000000, 1000000], dtype=np.float64)
pose_constraint.rot_coefs = np.array([10000, 10000, 10000],
dtype=np.float64)
pose_constraint.name = "final_pose"
pci.cnt_infos.push_back(pose_constraint)
elif target_joints is not None:
joint_constraint = tesseract.JointPosTermInfo()
joint_constraint.term_type = tesseract.TT_CNT
joint_constraint.link = end_effector
joint_constraint.first_step = pci.basic_info.n_steps - 2
joint_constraint.last_step = pci.basic_info.n_steps - 1
#joint_constraint.coeffs = tesseract.DblVec([10000]*6)
joint_constraint.targets = tesseract.DblVec(
list(target_joints))
print target_joints
pci.cnt_infos.push_back(joint_constraint)
else:
assert False
prob = tesseract.ConstructProblem(pci)
config = tesseract.TrajOptPlannerConfig(prob)
config.params.max_iter = 1000
planning_response = planner.solve(config)
if (planning_response.status_code != 0):
raise Exception(
"TrajOpt trajectory planning failed with code: %d" %
planning_response.status_code)
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
planning_response.trajectory[:, 0:6])
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = joint_names
trajectory_time = np.cumsum(1.0 /
planning_response.trajectory[:, 6])
trajectory_time = trajectory_time - trajectory_time[0]
for i in xrange(planning_response.trajectory.shape[0]):
jt_p = JointTrajectoryPoint()
jt_p.time_from_start = rospy.Duration(trajectory_time[i])
jt_p.positions = planning_response.trajectory[i, 0:6]
jt.points.append(jt_p)
return jt
def plan_robot_motion(self, dxf_points, sines):
self.dxf_points = dxf_points
self.sines = sines
waypoints = []
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
print("hello")
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#waypoints.append(self.move_group.get_current_pose().pose)
#wpose = self.move_group.get_current_pose().pose
#waypoints.append(wpose)
#state = self.robot.get_current_state()
#self.move_group.set_start_state(state)
for i in range(len(dxf_points)):
pose_goal = Pose()
rpy = [math.pi, 0.0, sines[i] - math.pi / 2]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - dxf_points[i][0]) * 0.0254
y_val = dxf_points[i][1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
print(pose_goal)
waypoints.append(pose_goal)
"""
euclidean_distances=[]
for i in range(1,len(waypoints)):
distance=pow(pow(waypoints[i].position.x-waypoints[i-1].position.x,2)+pow(waypoints[i].position.y-waypoints[i-1].position.y,2)+pow(waypoints[i].position.z-waypoints[i-1].position.z,2),0.5)
print(distance)
euclidean_distances.append(distance)
error_code=None
"""
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print(type(plan))
#self.scene.motion_plan_request
replan_value = 0
while (replan_value < 3 and fraction < 1.0):
print(fraction)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
replan_value += 1
print("WARNING Portion of plan failed, replanning")
if (replan_value > 2):
return 0, 0, 0
#print(len(euclidean_distances))
#print(len(plan.joint_trajectory.points))
#print(waypoints[0])
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(3))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(7))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(11))
#print(plan.joint_trajectory.points)
total_distance = 0
distances = []
for i in range(1, len(plan.joint_trajectory.points)):
old_cartesian = self.kdl_kin.forward(
plan.joint_trajectory.points[i - 1].positions)
new_cartesian = self.kdl_kin.forward(
plan.joint_trajectory.points[i].positions)
distance = pow(
pow(new_cartesian.item(3) - old_cartesian.item(3), 2) +
pow(new_cartesian.item(7) - old_cartesian.item(7), 2) +
pow(new_cartesian.item(11) - old_cartesian.item(11), 2), 0.5)
distances.append(distance)
total_distance += distance
#new_time=plan.joint_trajectory.points[i].time_from_start.to_sec()+(distance/self.speed)
#old_time=plan.joint_trajectory.points[i].time_from_start.to_sec()
#print("new time")
#print(new_time)
#print(distance/self.speed)
#print(old_time)
#if(new_time > old_time):
# plan.joint_trajectory.points[i].time_from_start.from_sec(new_time)
#else:
# print("Error, speed faster than possible execution time")
print(total_distance)
print(distances)
total_time = plan.joint_trajectory.points[-1].time_from_start.to_sec()
print(total_time)
times = []
for i in distances:
new_time = (i / total_distance) * total_time
times.append(new_time)
"""
if(new_timestamp > old_timestamp)
next_waypoint->time_from_start.fromSec(new_timestamp);
else
{
//ROS_WARN_NAMED("setAvgCartesianSpeed", "Average speed is too fast. Moving as fast as joint constraints allow.");
}"""
#print point.time_from_start.secs
#for i in range(len(plan.joint_trajectory.points)-1):
# print("time between points:\n")
# print(plan.joint_trajectory.points[i+1].time_from_start.nsecs-plan.joint_trajectory.points[i].time_from_start.nsecs)
#for i in range(1,len(plan.joint_trajectory.points)-1):
# plan.joint_trajectory.points[i].velocities=[-0.1,0.05,0.11,0.00000001,-0.05,0.2]
#plan=self.move_group.retime_trajectory(moveit_robot_state,plan,velocity_scaling_factor=1.0, algorithm="iterative_spline_parameterization")
if (self.display):
self.display_robot_trajectory(plan)
#self.actiongoal= control_msgs.msg.FollowJointTrajectoryActionGoal()
self.goal = control_msgs.msg.FollowJointTrajectoryGoal()
#self.actiongoal.header=std_msgs.msg.Header()
self.goal.trajectory.joint_names = joint_state.name
#self.goal.trajectory.points.resize(len(plan.joint_trajectory.points))
traj = Trajectory()
time = 0.01
traj.add_point(plan.joint_trajectory.points[0].positions, time)
#time=0.0
for i in range(1, len(plan.joint_trajectory.points)):
time += times[i - 1]
traj.add_point(plan.joint_trajectory.points[i].positions, time)
#traj.start()
#traj.wait(15.0)
"""
trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions=plan.joint_trajectory.points[i].positions
trajectory_point.velocities=plan.joint_trajectory.points[i].velocities
#trajectory_point.accelerations=plan.joint_trajectory.points[i].accelerations
trajectory_point.time_from_start=plan.joint_trajectory.points[i].time_from_start
self.goal.trajectory.points.append(trajectory_point)
print(len(plan.joint_trajectory.points))
self.actiongoal.goal=self.goal
self.followjointaction.publish(self.actiongoal)
print("published goal")
"""
return plan, fraction, waypoints
def rpy_to_quaternion(self, rpy, dtype=np.float64):
return self.q_to_quaternion(rox.R2q(rox.rpy2R(rpy)), dtype)
def R_to_quaternion(self, R, dtype=np.float64):
return self.q_to_quaternion(rox.R2q(R), dtype)
def first_half(input, num_iter):
# stop the active RAPID program
rapid.stop()
# reset the pointer to main
rapid.resetpp()
print 'first half'
print 'reset PP to main'
time.sleep(2)
# start the RAPID program
rapid.start()
# determine if robot has reached the initial configuration
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init))
if np.fabs(sum(state.joint_angles) - sum(init)) < 1e-4:
tag = False
time.sleep(1)
# out is composed of 5 velocity and 1 force in z
out = np.zeros((6, n))
force_out = np.zeros((1, n))
# pos of eef
eef_pos = np.zeros((3, n))
# orientation of eef (quaternion)
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
q_hat = np.zeros((6, 1))
qhat_dot = np.zeros((6, 1))
# for observer k should be symmetric and positive definite
kl = 0.5
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
time.sleep(2)
cnt = 0
step_done = False
while cnt < n:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
q_new = np.deg2rad(state.joint_angles)
# step-over
if not step_done:
print '--------start step-over motion--------'
# do step-over of 0.25 mm in +x in world frame
# current eef pose
pose_cur = rox.fwdkin(abb_robot, q_new)
pose_cur.p[0] = pose_cur.p[0] + num_iter * 2 * step_over
# solve for inverse kinematics and pick the one that is closest to current configuration
sol = rox.robot6_sphericalwrist_invkin(abb_robot, pose_cur, q_new)
try:
tar = sol[0] # raise exception if no solution
except:
tar = q_new
# move to the new position after step-over
egm.send_to_robot(tar)
step_done = True
q_new = tar
### drain the EGM buffer, or it will use the obsolete EGM feedback###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
print '--------step-over motion done--------'
time.sleep(2)
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, q_new)
R = pose.R
Fd0 = 50
if pose.p[2] >= pos_obj:
F = 0
v_z = Kp * 30 * (F - Fd0) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
F = -k * (pose.p[2] - pos_obj)
if F < Fdz - 0.5 and cnt == 0:
v_z = Kp * (F - Fdz) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
# formalize entire twist
# nominal input composed of F and v
spatial_velocity_command = input[:, cnt]
v_z = Kp * (F - spatial_velocity_command[5])
# nominal input only contains v
spatial_velocity_command[5] = v_z
# approximation of joint velocity
#q_new = np.deg2rad(state.joint_angles)
######### change here, use observer instead of approximation to calculate q_dot ########
if cnt == 0:
q_hat = q_new
qhat_dot = joints_vel + kl * (q_new - q_hat)
#q_dot = (q_new - q_pre)/delta_t
J = rox.robotjacobian(abb_robot, q_new)
# estimate velocity
v_est = J.dot(qhat_dot)
# formalize the nominal output composed of F and v
out[:, cnt] = np.append(v_est[0:5], F)
force_out[:, cnt] = F
eef_pos[:, cnt] = pose.p
R = pose.R
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt = cnt + 1
print F
# solve for joint velocity
# Jacobian inverse
J = rox.robotjacobian(abb_robot, q_new)
joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
#joints_vel = quadprog.compute_joint_vel_cmd_qp(q_new, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = q_new + joints_vel * delta_t
egm.send_to_robot(q_c)
# joint angle at previous time step
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
error = out - desired
# flip the error
err_flip = np.fliplr(error)
print np.linalg.norm(error, 'fro')
return out, err_flip, np.linalg.norm(
error, 'fro'), force_out, eef_pos, eef_orien, tim
def first_half(input, num_iter):
# stop the active RAPID program
#rapid.stop()
# reset the pointer to main
#rapid.resetpp()
print 'first half'
print 'reset PP to main'
time.sleep(5)
# start the RAPID program
#rapid.start()
# determine if robot has reached the initial configuration
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init))
if np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init)) < 1e-4:
tag = False
time.sleep(1)
# out is composed of 5 velocity and 1 force in z
out = np.zeros((6, n))
force_out = np.zeros((6, n))
# pos of eef
eef_pos = np.zeros((3, n))
# orientation of eef (quaternion)
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
############### change ################ or there will be errors that q_pre referred before assigned
#q_pre = np.deg2rad(state.joint_angles)
q_hat = np.zeros((6, 1))
qhat_dot = np.zeros((6, 1))
# for observer k should be symmetric and positive definite
kl = 0.1
### drain the force sensor buffer ###
count = 0
while count < 1000:
flag, ft = netft.read_ft_streaming(.1)
#print ft[5]
count = count + 1
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
# substract the initial force for bias
flag, ft = netft.read_ft_streaming(.1)
F0 = ft[5]
print F0
time.sleep(3)
cnt = 0
step_done = False
while cnt < n:
#t_pre = time.time()
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
q_new = state.joint_angles
# step-over
if not step_done:
print '--------start step-over motion--------'
# do step-over of 0.25 mm in +x in world frame
# current eef pose
pose_cur = rox.fwdkin(abb_robot, q_new)
pose_cur.p[0] = pose_cur.p[0] + num_iter * 2 * step_over
# solve for inverse kinematics and pick the one that is closest to current configuration
sol = rox.robot6_sphericalwrist_invkin(abb_robot, pose_cur, q_new)
try:
tar = sol[0] # raise exception if no solution
except:
tar = q_new
# move to the new position after step-over
egm.send_to_robot(tar)
step_done = True
q_new = tar
### drain the EGM buffer, or it will use the obsolete EGM feedback###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
print '--------step-over motion done--------'
time.sleep(2)
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, q_new)
R = pose.R
flag, ft = netft.read_ft_streaming(.1)
# map torque/force from sensor frame to base frame
T_b = np.matmul(R, ft[0:3])
F_b = np.matmul(R, ft[3:None])
F = F_b[2] # - F0# first three torques and then three forces
# start motion in y direction when robot barely touches coupon
Fd0 = 50 # this should be equal to the Fdz[0]
if F < Fd0 - 0.1 and cnt == 0:
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
# will shake if gain too large, here use 0.0002
v_z = Kp * 10 * (F - Fd0) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
# formalize entire twist
# nominal input composed of F and v
spatial_velocity_command = input[:,
cnt] #np.array([0, 0, 0, vdx, 0, Fd])
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
v_z = Kp * (F - spatial_velocity_command[5]
) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# nominal input only contains v
spatial_velocity_command[5] = v_z
# approximation of joint velocity
#q_new = np.deg2rad(state.joint_angles)
######### change here, use observer instead of approximation to calculate q_dot ########
if cnt == 0:
q_hat = q_new
qhat_dot = joints_vel + kl * (q_new - q_hat)
#q_dot = (q_new - q_pre)/delta_t
J = rox.robotjacobian(abb_robot, q_new)
# estimate velocity
v_est = J.dot(qhat_dot)
# formalize the nominal output composed of F and v
out[:, cnt] = np.append(v_est[0:5], F)
force_out[:, cnt] = np.concatenate((T_b, F_b), axis=0)
eef_pos[:, cnt] = pose.p
#eef_pos[2, cnt] = z
R = pose.R
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt = cnt + 1
print F
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, q_new)
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# emergency stop if force too large
if abs(F) > 2000:
spatial_velocity_command = np.array([0, 0, 0, 0, 0, 0])
print "force too large, stop..."
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
q_new, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = q_new + joints_vel * delta_t
egm.send_to_robot(q_c)
# joint angle at previous time step
#q_pre = q_new
############ change here ##############
# make the time interval 0.004 s
#t_new = time.time()
#if t_new - t_pre < delta_t:
# time.sleep(delta_t - t_new + t_pre)
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
# interpolate to filter the output
t_inter = np.arange(0, n * delta_t, delta_t)
# interpolate each row of output
for i in range(6):
y = out[i, :]
tck = interpolate.splrep(t_inter, y,
s=0.01) # s = 0 means no interpolation
ynew = interpolate.splev(t_inter, tck, der=0)
out[i, :] = ynew
error = out - desired
# flip the error
err_flip = np.fliplr(error)
print np.linalg.norm(error, 'fro')
return out, err_flip, np.linalg.norm(
error, 'fro'), force_out, eef_pos, eef_orien, tim
