def get_box_plan(self, num, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.arm.set_joint_value_target(self.box_pose[num])
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a box plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state, plan)
def get_cartesian_plan(self, trans, z_offset, start_state, grasp):
# set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# set waypoints
waypoints = []
self.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
wpose = Pose()
wpose.position = copy.deepcopy(self.target_pose.position)
wpose.orientation = copy.deepcopy(self.target_pose.orientation)
wpose.position.z = copy.deepcopy(self.target_pose.position.z +
z_offset)
waypoints.append(wpose)
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a cartesian plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state)
def get_plan(self, trans, z_offset, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z + z_offset
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
self.arm.set_pose_target(self.target_pose)
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
print("---------debug--------{}".format(
len(plan.joint_trajectory.points)))
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state)
def get_plan(self, trans, z_offset, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z + z_offset
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
self.arm.set_pose_target(self.target_pose)
# plan
# for i in range(5):
# print "plan: " +str(i)
# plan = RobotTrajectory()
# counter = 0
# while len(plan.joint_trajectory.points) == 0 :
# plan = self.arm.plan()
# counter+=1
# self.arm.set_planning_time(self.planning_limitation_time+counter*5.0)
# if counter > 1 :
# return (False, start_state)
# self.arm.set_planning_time(self.planning_limitation_time)
# # sleep(1)
# rospy.sleep(0.5)
#plan
threshold = np.sqrt((moving_x * 100) ^ 2 + (moving_y * 100) ^ 2)
threshold = 20
while (1):
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
if grasp == 0:
break
if len(plan.joint_trajectory.points) < threshold:
# print("---------debug0--------{}".format(len(plan.joint_trajectory.points)))
break
# print "input key A to continue."
# key = raw_input('>>> ')
# if key == "a":
# break
# plan = RobotTrajectory()
# counter = 0
# print("---------debug1--------{}".format(len(plan.joint_trajectory.points)))
# while len(plan.joint_trajectory.points) == 0 :
# plan = self.arm.plan()
# counter+=1
# self.arm.set_planning_time(self.planning_limitation_time+counter*5.0)
# if counter > 1 :
# return (False, start_state)
# self.arm.set_planning_time(self.planning_limitation_time)
# print("------------debug------------{}".format(len(plan.joint_trajectory.points)))
rospy.loginfo("!! Got a plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
# print("---------debug2--------{}".format(len(plan.joint_trajectory.points)))
pub_msg.trajectory = plan
# print "input key A to continue."
# while(1):
# key = raw_input('>>> ')
# if key == "a":
# break
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state)