def cartesian_vel_to_joint_vel(vel, angular_speed, current_configuration, side,
horizontal, endlink_angle, endeffector_offset):
cur_pos, cur_ang, ret_endlink_angle = np.array(
forward_kinematics(current_configuration,
side,
horizontal,
return_endlink_angle=True,
endeffector_offset=endeffector_offset))
cur_point = Point(*cur_pos)
cur_target_joints = call_ik_solver(cur_point, side, horizontal,
endlink_angle, endeffector_offset)
if cur_target_joints is None:
return np.array([0.] * len(joint_names))
cur_target_joints[4] -= cur_ang
cart_speed = np.linalg.norm(vel)
if cart_speed > MAX_DIST_LIN_SPEEDS:
scale = MAX_DIST_LIN_SPEEDS / np.linalg.norm(vel)
else:
scale = 1
target_vector = cur_pos + scale * vel
goal = Point(*list(target_vector))
target_joints = call_ik_solver(goal, side, horizontal, endlink_angle,
endeffector_offset)
if target_joints is not None:
target_joints[4] = limit_joint_5(target_joints[4] - cur_ang)
joint_velocities = (target_joints -
np.array(current_configuration)) / scale
joint_velocities[4] -= angular_speed
# print joint_velocities[4]
return joint_velocities
else:
return (cur_target_joints - np.array(current_configuration)) / scale
def cartesian_vel_to_joint_vel(vel, angular_speed, current_configuration, side, horizontal, endlink_angle, endeffector_offset):
cur_pos, cur_ang, ret_endlink_angle = np.array(forward_kinematics(current_configuration, side, horizontal, return_endlink_angle=True, endeffector_offset=endeffector_offset))
cur_point = Point(*cur_pos)
cur_target_joints = call_ik_solver(cur_point, side, horizontal, endlink_angle, endeffector_offset)
if cur_target_joints is None:
return np.array([0.] * len(joint_names))
cur_target_joints[4] -= cur_ang
cart_speed = np.linalg.norm(vel)
if cart_speed > MAX_DIST_LIN_SPEEDS:
scale = MAX_DIST_LIN_SPEEDS / np.linalg.norm(vel)
else:
scale = 1
target_vector = cur_pos + scale * vel
goal = Point(*list(target_vector))
target_joints = call_ik_solver(goal, side, horizontal, endlink_angle, endeffector_offset)
if target_joints is not None:
target_joints[4] = limit_joint_5(target_joints[4] - cur_ang)
joint_velocities = (target_joints - np.array(current_configuration)) / scale
joint_velocities[4] -= angular_speed
# print joint_velocities[4]
return joint_velocities
else:
return (cur_target_joints - np.array(current_configuration)) / scale
def execute_arm_ik_action(self, action_goal):
assert isinstance(action_goal, MoveArmIKGoal)
conf = call_ik_solver(action_goal.position, action_goal.side,
action_goal.horizontal,
action_goal.endlink_angle,
action_goal.endeffector_offset)
res = MoveArmIKResult()
if conf is None:
res.success = False
res.reason = 'No IK solution found'
return res
conf[4] = get_forward_angle_joint_5(conf[4], action_goal.angle)
action_result = MoveArmIKResult()
if action_goal.velocity_controlled:
if action_goal.max_speed is 0.0:
speed = MAX_SPEED
else:
speed = action_goal.max_speed
res.success = self.go_velocity_controlled([conf],
speed,
wait=False)
client = self.arm_velocity_joint_client
else:
res.success = self.go_position_controlled([conf], wait=False)
client = self.arm_position_joint_client
rate = rospy.Rate(CHECK_STATUS_RATE)
while client.get_state() is not GoalStatus.SUCCEEDED:
if self.arm_ik_action_server.is_preempt_requested():
client.cancel_all_goals()
action_result.success = False
action_result.reason = 'Action Canceled'
self.arm_ik_action_server.set_aborted(result=action_result)
return
if client.get_state() in [
GoalStatus.PREEMPTED, GoalStatus.ABORTED
]:
client.cancel_all_goals()
action_result.success = False
action_result.reason = 'Action failed'
self.arm_ik_action_server.set_preempted(result=action_result)
return
rate.sleep()
action_result.success = True
action_result.reason = 'Action succeeded'
self.arm_ik_action_server.set_succeeded(result=action_result)
def move_arm_ik(self, req):
assert isinstance(req, MoveArmIKRequest)
conf = call_ik_solver(req.position, req.side, req.horizontal, req.endlink_angle,req.endeffector_offset)
res = MoveArmIKResponse()
if conf is None:
res.success = False
res.reason = 'No IK solution found'
return res
conf[4] = get_forward_angle_joint_5(conf[4], req.angle)
if req.velocity_controlled:
if req.max_speed == 0.0:
speed = MAX_SPEED
else:
speed = req.max_speed
res.success = self.go_velocity_controlled([conf], speed, wait=req.blocking)
else:
res.success = self.go_position_controlled([conf], wait=req.blocking)
return res
def execute_linear_action(self, goal):
assert isinstance(goal, MoveArmLinearGoal)
over_effort_counter = 0
rate = rospy.Rate(50)
result = MoveArmLinearResult()
side = goal.side
horizontal = goal.horizontal
endlink_angle = goal.endlink_angle
max_speed = goal.max_speed
endeffector_offset = goal.endeffector_offset
if max_speed < ARM_MOVING_THRESHOLD:
result.success = False
result.reason = 'Max speed is lower than arm moving threshold'
self.linear_action_server.set_preempted(result)
return
if len(goal.points) < 2:
result.success = False
result.reason = 'Less than two points given'
self.linear_action_server.set_preempted(result)
return
for target_pose in goal.points:
conf = call_ik_solver(target_pose.position,
goal.side,
horizontal=goal.horizontal,
endlink_angle=endlink_angle,
endeffector_offset=endeffector_offset)
if conf is None:
result.success = False
result.reason = 'No IK solution found'
self.linear_action_server.set_preempted(result)
return
first_conf = call_ik_solver(goal.points[0].position,
goal.side,
horizontal=goal.horizontal,
endlink_angle=endlink_angle,
endeffector_offset=endeffector_offset)
first_conf[4] = limit_joint_5(first_conf[4] - goal.points[0].angle)
last_conf = call_ik_solver(goal.points[-1].position,
goal.side,
horizontal=goal.horizontal,
endlink_angle=endlink_angle,
endeffector_offset=endeffector_offset)
last_conf[4] = limit_joint_5(last_conf[4] - goal.points[-1].angle)
if not self.go_velocity_controlled(
[first_conf],
NORMAL_SPEED,
action_server=self.linear_action_server):
result.success = False
result.reason = 'Stopped while going to first conf'
self.linear_action_server.set_aborted(result)
self.stop_arm()
return
for endeffector_pose in goal.points:
target_pose = [
endeffector_pose.position.x, endeffector_pose.position.y,
endeffector_pose.position.z
]
target_ang = endeffector_pose.angle
print target_pose
while not rospy.is_shutdown():
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
result.success = False
rospy.logerr("arm over-effort")
result.reason = "arm over effort"
self.arm_up_recover()
self.linear_action_server.set_preempted(result)
return
if self.linear_action_server.is_preempt_requested():
self.stop_arm()
result.success = False
result.reason = 'Action aborted'
self.linear_action_server.set_aborted(result)
return
cur_pose, cur_ang = forward_kinematics(self.configuration,
side, horizontal,
endlink_angle,
endeffector_offset)
if np.linalg.norm(np.array(cur_pose) - np.array(target_pose)) < THRESHOLD_LINEAR_MOVEMENT \
and abs(cur_ang - target_ang) < THRESHOLD_ANGULAR_MOVEMENT:
break
msg = self.get_linear_movement_msg(
target_pose,
target_ang,
max_speed,
side,
horizontal=horizontal,
endlink_angle=endlink_angle,
endeffector_offset=endeffector_offset)
self.velocity_pub.publish(msg)
rate.sleep()
if not self.go_velocity_controlled(
[last_conf], NORMAL_SPEED,
action_server=self.linear_action_server):
result.success = False
result.reason = 'Stopped while going to last conf'
self.linear_action_server.set_aborted(result)
self.stop_arm()
return
self.stop_arm()
result.success = True
result.reason = 'Movement Succeeded'
self.linear_action_server.set_succeeded(result)
def get_sine(a, c, x):
ret_y = a*math.sin(c*x)
return x, ret_y
a = 0.075
c = 12.5 * math.pi
max_x = 0.155
height = -0.14
steps = 100
srv = rospy.ServiceProxy('/move_arm_linear', MoveArmLinear)
req = MoveArmLinearRequest()
req.horizontal = False
req.side = 'left'
req.max_speed = 0.03
step = 0
for i in range(steps+1):
pos = EndeffectorPosition()
x, y = get_sine(a, c, step)
step += max_x/steps
pos.position.x = origin[0] + y
pos.position.y = origin[1] + max_x/2. - x - 0.025
pos.position.z = height
print x, y, call_ik_solver(pos.position, req.side)
req.points.append(pos)
srv.call(req)
def execute_linear_action(self, goal):
assert isinstance(goal, MoveArmLinearGoal)
over_effort_counter = 0
rate = rospy.Rate(50)
result = MoveArmLinearResult()
side = goal.side
horizontal = goal.horizontal
endlink_angle = goal.endlink_angle
max_speed = goal.max_speed
endeffector_offset = goal.endeffector_offset
if max_speed < ARM_MOVING_THRESHOLD:
result.success = False
result.reason = 'Max speed is lower than arm moving threshold'
self.linear_action_server.set_preempted(result)
return
if len(goal.points) < 2:
result.success = False
result.reason = 'Less than two points given'
self.linear_action_server.set_preempted(result)
return
for target_pose in goal.points:
conf = call_ik_solver(target_pose.position, goal.side, horizontal=goal.horizontal,
endlink_angle=endlink_angle, endeffector_offset=endeffector_offset)
if conf is None:
result.success = False
result.reason = 'No IK solution found'
self.linear_action_server.set_preempted(result)
return
first_conf = call_ik_solver(goal.points[0].position, goal.side, horizontal=goal.horizontal,
endlink_angle=endlink_angle, endeffector_offset=endeffector_offset)
first_conf[4] = limit_joint_5(first_conf[4] - goal.points[0].angle)
last_conf = call_ik_solver(goal.points[-1].position, goal.side, horizontal=goal.horizontal,
endlink_angle=endlink_angle, endeffector_offset=endeffector_offset)
last_conf[4] = limit_joint_5(last_conf[4] - goal.points[-1].angle)
if not self.go_velocity_controlled([first_conf], NORMAL_SPEED, action_server=self.linear_action_server):
result.success = False
result.reason = 'Stopped while going to first conf'
self.linear_action_server.set_aborted(result)
self.stop_arm()
return
for endeffector_pose in goal.points:
target_pose = [endeffector_pose.position.x, endeffector_pose.position.y, endeffector_pose.position.z]
target_ang = endeffector_pose.angle
print target_pose
while not rospy.is_shutdown():
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
result.success = False
rospy.logerr("arm over-effort")
result.reason = "arm over effort"
self.arm_up_recover()
self.linear_action_server.set_preempted(result)
return
if self.linear_action_server.is_preempt_requested():
self.stop_arm()
result.success = False
result.reason = 'Action aborted'
self.linear_action_server.set_aborted(result)
return
cur_pose, cur_ang = forward_kinematics(self.configuration, side, horizontal, endlink_angle, endeffector_offset)
if np.linalg.norm(np.array(cur_pose) - np.array(target_pose)) < THRESHOLD_LINEAR_MOVEMENT \
and abs(cur_ang - target_ang) < THRESHOLD_ANGULAR_MOVEMENT:
break
msg = self.get_linear_movement_msg(target_pose, target_ang, max_speed, side, horizontal=horizontal,
endlink_angle=endlink_angle,
endeffector_offset=endeffector_offset)
self.velocity_pub.publish(msg)
rate.sleep()
if not self.go_velocity_controlled([last_conf], NORMAL_SPEED, action_server=self.linear_action_server):
result.success = False
result.reason = 'Stopped while going to last conf'
self.linear_action_server.set_aborted(result)
self.stop_arm()
return
self.stop_arm()
result.success = True
result.reason = 'Movement Succeeded'
self.linear_action_server.set_succeeded(result)