def compute_cartesian_path(
self,
waypoints,
eef_step,
jump_threshold,
avoid_collisions=True,
path_constraints=None,
):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException(
"Unable to set path constraints, unknown constraint type "
+ type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path(
[conversions.pose_to_list(p) for p in waypoints],
eef_step,
jump_threshold,
avoid_collisions,
constraints_str,
)
else:
(ser_path, fraction) = self._g.compute_cartesian_path(
[conversions.pose_to_list(p) for p in waypoints],
eef_step,
jump_threshold,
avoid_collisions,
)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor):
ser_ref_state_in = conversions.msg_to_string(ref_state_in)
ser_traj_in = conversions.msg_to_string(traj_in)
ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor):
ser_ref_state_in = conversions.msg_to_string(ref_state_in)
ser_traj_in = conversions.msg_to_string(traj_in)
ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
def runTrajectory(req):
global Traj_server;
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
else:
file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);
if req.task == "Forward":
file_name = file_root+"/forward";
elif req.task == "Drop":
file_name = file_root+"/drop";
else :
return taskResponse(False);
f = open(file_name,"r");
plan_file = RobotTrajectory();
buf = f.read();
plan_file.deserialize(buf);
plan = copy.deepcopy(plan_file);
arm_right_group = moveit_commander.MoveGroupCommander("arm_right");
arm_left_group = moveit_commander.MoveGroupCommander("arm_left");
arm_left_group.set_start_state_to_current_state();
StartPnt = JointTrajectoryPoint();
StartPnt.positions = arm_left_group.get_current_joint_values();
StartPnt.velocities = [0]*len(StartPnt.positions);
StartPnt. accelerations = [0]*len(StartPnt.positions);
#print StartPnt;
plan.joint_trajectory.points[0] = StartPnt;
#print plan;
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory();
robot = moveit_commander.RobotCommander();
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while", file_name, " is visualized (again)..."
arm_left_group.execute(plan);
print "Trajectory ", file_name, " finished!"
f.close();
return taskResponse(True);
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization"):
ser_ref_state_in = conversions.msg_to_string(ref_state_in)
ser_traj_in = conversions.msg_to_string(traj_in)
ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor, algorithm)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
def compute_cartesian_path(self,
waypoints,
eef_step,
jump_threshold,
avoid_collisions=True):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
(ser_path, fraction) = self._g.compute_cartesian_path(
[conversions.pose_to_list(p) for p in waypoints], eef_step,
jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str)
else:
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def runTrajectory(req):
global Traj_server
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(os.path.dirname(__file__),
"../trajectories/Torso/bin" + req.bin_num)
else:
file_root = os.path.join(os.path.dirname(__file__),
"../trajectories/bin" + req.bin_num)
if req.task == "Forward":
file_name = file_root + "/forward"
elif req.task == "Drop":
file_name = file_root + "/drop"
elif req.task == "Pick":
file_name = file_root + "/Pick"
elif req.task == "Scan":
file_name = file_root + "/scan"
elif req.task == "Dump":
file_name = file_root + "/Dump"
elif req.task == "Lift":
file_name = file_root + "/Lift"
elif req.task == "Home":
file_name = file_root + "/Home"
elif req.task == "Rotate":
file_name = file_root + "/Rotate"
else:
return taskResponse(False)
f = open(file_name, "r")
plan_file = RobotTrajectory()
buf = f.read()
plan_file.deserialize(buf)
plan = copy.deepcopy(plan_file)
f.close()
return GetTrajectoryResponse(plan, True)
def plan(self, joints=None):
"""Return a tuple of the motion planning results such as
(success flag : boolean, trajectory message : RobotTrajectory,
planning time : float, error code : MoveitErrorCodes)"""
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif joints is not None:
try:
self.set_joint_value_target(
self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (
error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code,
)
def plan(self, joints = None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = RobotTrajectory()
plan.deserialize(self._g.compute_plan())
return plan
def plan(self, joints = None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = RobotTrajectory()
plan.deserialize(self._g.compute_plan())
return plan
def plan(self, joints=None):
""" Return a tuple of the motion planning results such as
(success flag : boolean, trajectory message : RobotTrajectory,
planning time : float, error code : MoveitErrorCodes) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif joints is not None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code)
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def runTrajectory(req):
global Traj_server
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
if req.using_torso == "y":
file_root = os.path.join(
os.path.dirname(__file__),
"../../optimize_trajectories/Torso/bin" + req.bin_num)
else:
file_root = os.path.join(
os.path.dirname(__file__),
"../../optimize_trajectories/bin" + req.bin_num)
if req.task == "Forward":
file_name = file_root + "/forward"
elif req.task == "Drop":
file_name = file_root + "/drop"
else:
return taskResponse(False)
f = open(file_name, "r")
plan_file = RobotTrajectory()
buf = f.read()
plan_file.deserialize(buf)
plan = copy.deepcopy(plan_file)
arm_right_group = moveit_commander.MoveGroupCommander("arm_right")
arm_left_group = moveit_commander.MoveGroupCommander("arm_left")
arm_left_group.set_start_state_to_current_state()
StartPnt = JointTrajectoryPoint()
StartPnt.positions = arm_left_group.get_current_joint_values()
StartPnt.velocities = [0] * len(StartPnt.positions)
StartPnt.accelerations = [0] * len(StartPnt.positions)
#print StartPnt;
plan.joint_trajectory.points[0] = StartPnt
#print plan;
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
robot = moveit_commander.RobotCommander()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while", file_name, " is visualized (again)..."
arm_left_group.execute(plan)
print "Trajectory ", file_name, " finished!"
f.close()
return taskResponse(True)
"
if type(joints) is JointState : self.set_joint_value_target(joints)
elif type(joints) is Pose : self.set_pose_target(joints)
elif joints is not None : try : self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except MoveItCommanderException:
self.set_joint_value_target(joints)
(error_code_msg, trajectory_msg, planning_time) = self._g.plan()
error_code = MoveItErrorCodes()
error_code.deserialize(error_code_msg)
plan = RobotTrajectory()
return (error_code.val == MoveItErrorCodes.SUCCESS,
plan.deserialize(trajectory_msg),
planning_time,
error_code)
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, algorithm, avoid_collisions=True, path_constraints=None):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, algorithm, avoid_collisions, constraints_str)
else:
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, algorithm, avoid_collisions)
path = RobotTrajectory()
def runTrajectory(req):
print "---------------------------------"
print req.task
print " "
print req.bin_num
print " "
print req.using_torso
print "---------------------------------"
# Get the trajectory
file_root = os.path.join(os.path.dirname(__file__), "../trajectories");
#file_root = os.path.join(os.path.dirname(__file__), "../traj_updates");
traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso");
plan = RobotTrajectory();
# Gripper Hand
if req.task == "use_tray":
file_name = file_root + "/left_arm_drop2home";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso");
elif req.task == "use_gripper":
file_name = file_root + "/left_arm_home2drop";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso");
elif req.task == "Forward":
file_name = file_root+"/bin" + req.bin_num +"/forward";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso");
elif req.task == "Drop":
file_name = file_root+"/bin" + req.bin_num+"/drop";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso");
elif req.task == "Scan":
file_name = file_root+"/bin" + req.bin_num+"/scan";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso");
# Tray Hand
elif req.task == "Pick":
file_name = file_root+"/bin" + req.bin_num+"/Pick";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso");
elif req.task == "Dump":
file_name = file_root+"/bin" + req.bin_num+"/Dump";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso");
elif req.task == "Lift":
file_name = file_root+"/bin" + req.bin_num+"/Lift";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso");
elif req.task == "Home":
file_name = file_root+"/bin" + req.bin_num+"/Home";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso");
elif req.task == "Rotate":
file_name = file_root+"/bin" + req.bin_num+"/Rotate";
traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso");
else :
print "No plan selected, exit!";
return GetTrajectoryResponse(plan,True);
f = open(file_name,"r");
buf = f.read();
plan_file = RobotTrajectory();
plan_file.deserialize(buf);
plan = copy.deepcopy(plan_file);
move_to_start_pos(plan, traj_execute_group);
if check_trajectory_validity(plan):
print "Trajectory is valid";
# Display Current Trajectory
robot = moveit_commander.RobotCommander();
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
display_trajectory = moveit_msgs.msg.DisplayTrajectory();
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while", file_name, " is visualized (again)..."
pose = PoseStamped()
pose.header.frame_id = "/arm_left_link_7_t"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = -0.35
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
#attach_sphere("arm_left_link_7_t", "Object", pose, 0.17, ["hand_left_finger_1_link_2", "hand_left_finger_1_link_3", "hand_left_finger_1_link_3_tip", "hand_left_finger_2_link_2", "hand_left_finger_2_link_3", "hand_left_finger_2_link_3_tip", "hand_left_finger_middle_link_2", "hand_left_finger_middle_link_3", "hand_left_finger_middle_link_3_tip"]);
traj_execute_group.execute(plan);
print "Trajectory ", file_name, " finished!"
f.close();
remove_object();
return GetTrajectoryResponse(plan,True);
else:
print "Trajectory is not valid! Test failed!";
return GetTrajectoryResponse(plan,True);
