def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_ik', anonymous=True)
robot = moveit_commander.RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
# right_arm.set_goal_position_tolerance(0.01)
# right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
# end_effector_link = right_arm.get_end_effector_link()
# Get the current pose so we can add it as a waypoint
# start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
valid_pose = np.load("valid_pose.npy")
for i in valid_pose:
waypoints.append(i.pose)
no_pros_data = np.load("dmp_euler_traj.npy")
pose_list = []
for idx in no_pros_data:
pose = Pose()
pose.position.x = idx[0]
pose.position.y = idx[1]
pose.position.z = idx[2]
pose.orientation.x = idx[3]
pose.orientation.y = idx[4]
pose.orientation.z = idx[5]
pose.orientation.w = idx[6]
pose_list.append(pose)
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
(plan, fraction) = right_arm.compute_cartesian_path(
pose_list, # waypoint poses
0.01, # eef_step
0.0 # jump_threshold
) # avoid_collisions
print "fraction is %s" % fraction
right_arm.execute(plan)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
rospy.sleep(2)
pose = PoseStamped().pose
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
i = 1
while i <= 10:
waypoints = list()
j = 1
while j <= 5:
# random pose
rand_pose = arm.get_random_pose(arm.get_end_effector_link())
pose.position.x = rand_pose.pose.position.x
pose.position.y = rand_pose.pose.position.y
pose.position.z = rand_pose.pose.position.z
waypoints.append(copy.deepcopy(pose))
j += 1
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
i += 1
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
self.listener = tf.TransformListener()
# self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
self.tomatoboundingBox = BoundingBox()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
self.distance = 0
self.trans = [0.0, 0.0, 0.0]
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print ("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print ("====== current pose : ", self.wpose)
joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait = True)
def go_to_desired_coordinate(self):
self.calculated_tomato.position.x = goal_x
self.calculated_tomato.position.y = goal_y
self.calculated_tomato.position.z = goal_z
self.calculated_tomato.orientation.x = goal_roll
self.calculated_tomato.orientation.y = goal_pitch
self.calculated_tomato.orientation.z = goal_yaw
tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z]
tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato))
(tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print ("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
# rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
try:
(trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0))
goal_x = round(trans[0],2)
goal_y = round(trans[1],2)
goal_z = round(trans[2],2)
print("====== trans [x, y, z]: ", trans)
print("====== rotat [x, y, z, w]: ", rot)
orientation = euler_from_quaternion(rot)
goal_roll = orientation[0]
goal_pitch = orientation[1]
goal_yaw = orientation[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('there is no tf')
pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
### Move to Cartesian position
goal_pose = gen_pose(pos=[0.123, -0.417, 1.361],
euler=[3.1415, 0.0, 1.5707])
mgc.go(goal_pose.pose)
### Move Cartesian linear
goal_pose.pose.position.z -= 0.1
(traj, frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
mgc.execute(traj)
print "Done"
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
self.distance = 0
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.calculated_tomato_coor = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
2.6482303142547607, -0.3752959410296839, -2.1118043104754847,
-0.4801228682147425, -1.4944542090045374, -4.647655431424276
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
def go_to_goal_point(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
print(">> robot arm planning frame: \n", planning_frame)
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation.w = (scale * ori_w)
self.calculated_tomato.orientation.x = (scale * ori_x)
self.calculated_tomato.orientation.y = (scale * ori_y)
self.calculated_tomato.orientation.z = (scale * ori_z)
print(">> robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("======= Press `Enter` to if plan in correct!======")
raw_input()
self.robot_arm.go(True)
'''
def go_to_designated_coordinate(self):
desired_goal_pose = [-0.537,0.166, 0.248]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n",Cplanning_frame)
self.calculated_tomato_coor.position.x = desired_goal_pose[0]
self.calculated_tomato_coor.position.y = desired_goal_pose[1]
self.calculated_tomato_coor.position.z = desired_goal_pose[2]
self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2]))
print(">> goal frame", self.calculated_tomato_coor)
self.robot_arm.set_pose_target(self.calculated_tomato_coor)
tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z]
tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor))
(plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0)
self.display_trajectory(plan)
print("=========== Press `Enter` to if plan is correct!!...")
raw_input()
self.robot_arm.go(True)
'''
def plan_cartesian_path(self, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x -= scale * 0.1
self.wpose.position.y += scale * 0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
self.wpose.position.x -= scale*0.2
waypoints.append(copy.deepcopy(self.wpose))
'''
'''
self.wpose.position.x -= scale*0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
try:
(trans,
rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
rospy.Time(0))
goal_x = trans[0]
goal_y = trans[1]
goal_z = trans[2]
ori_w = rot[0]
ori_x = rot[1]
ori_y = rot[2]
ori_z = rot[3]
print("==== trans[x,y,z]: ", trans)
print("==== rot[x,y,z,w]: ", rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn('there is no tf')
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set an initial position for the arm
start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)
# Plan and execute a trajectory to the goal configuration
arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Move end effector to the right 0.3 meters
wpose.position.y -= 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Move end effector up and back
wpose.position.x -= 0.2
wpose.position.z += 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses
0.025, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def simple_function():
rc = RobotCommander()
mgc = MoveGroupCommander("manipulator")
# print(rc.get_group_names())
# print(rc.get_group('manipulator'))
# exit()
eef_step = 0.01
jump_threshold = 2
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rc.get_current_state()
rospy.logwarn(rc.get_current_state())
sss.wait_for_input()
#rate = rospy.Rate(0.1) # 10hz
rate = rospy.Rate(1) # 10hz
rospy.sleep(1)
theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
while not rospy.is_shutdown():
approach_pose = PoseStamped()
approach_pose.header.frame_id = "table"
approach_pose.header.stamp = rospy.Time(0)
approach_pose.pose.position.x = 0.146
approach_pose.pose.position.y = 0.622
approach_pose.pose.position.z = 0.01
quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
approach_pose.pose.orientation.x = quat[0]
approach_pose.pose.orientation.y = quat[1]
approach_pose.pose.orientation.z = quat[2]
approach_pose.pose.orientation.w = quat[3]
# mgc.set_start_state_to_current_state()
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed before adding collision objects")
# else:
# rospy.logwarn("Planning succeeded before adding collision objects")
#
# rospy.logwarn("waiting for input to add dummy box")
# sss.wait_for_input()
#
box_dummy_pose = PoseStamped()
box_dummy_pose.header.frame_id = "table"
box_dummy_pose.pose.position.x = 0.147
box_dummy_pose.pose.position.y = 0.636
box_dummy_pose.pose.position.z = 0
psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC
# rospy.logwarn("I have added the box")
# rospy.sleep(1)
# rospy.logwarn("waiting for input to try planning with dummy box")
# sss.wait_for_input()
#
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
# else:
# rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#
rospy.logwarn("waiting for input to add end effector box box")
sss.wait_for_input()
# end effector collision object
link_attached_to_ef = "ee_link"
mb_ef_collisonobj = "metal_bracket"
mb_ef_pose = PoseStamped()
mb_ef_pose.header.frame_id = link_attached_to_ef
mb_ef_pose.pose.position.x = 0.065/2.0
mb_ef_pose.pose.position.y = 0.0
mb_ef_pose.pose.position.z = 0.0
mb_ef_size = (0.065,0.06,0.06)
psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
raw_input("0 hi")
mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
rospy.logwarn("I have added the attached box to the end effector")
rospy.sleep(1)
raw_input("1 hi")
rospy.logwarn(rc.get_current_state())
# mgc.attach_object(object_name, link_name, touch_links)
mgc.set_start_state_to_current_state()
rospy.logwarn(rc.get_current_state())
raw_input("2 hi")
rospy.logwarn("waiting for input to try planning with end effector box")
sss.wait_for_input()
(traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
if(frac_approach != 1):
rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
else:
rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
rospy.logwarn("waiting for input to try planning next loop")
sss.wait_for_input()
rate.sleep()
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
gain = 0.2
points = 5
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
wpose.orientation.x = waypoints[i-1].orientation.x
wpose.orientation.y = waypoints[i-1].orientation.y
wpose.orientation.z = waypoints[i-1].orientation.z
wpose.position.y = waypoints[i-1].position.y - 0.0737309
wpose.position.z = waypoints[i-1].position.z + 0.02544397
wpose.position.x = waypoints[i-1].position.x + 0.1991539
waypoints.append(copy.deepcopy(wpose))
(plan_waypoints, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print fraction*100, "% planned"
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
right_arm.execute(plan_waypoints)
print "============ Waiting while RVIZ execute..."
rospy.sleep(5)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path (
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线
cartesian = True #rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('armc_arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('init_pose')
arm.go()
rospy.sleep(1)
# 设置机械臂运动的起始位姿
joint_goal = arm.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi / 6
joint_goal[2] = 0
joint_goal[3] = pi / 3
joint_goal[4] = 0
joint_goal[5] = pi / 3
joint_goal[6] = 0
arm.go(joint_goal)
rospy.sleep(1)
#获取当前位置为规划初始位置
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 如果为True,将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置路点数据，并加入路点列表，所有的点都加入
wpose = deepcopy(start_pose) #拷贝对象
wpose.position.x += 0.1
waypoints.append(deepcopy(wpose))
if cartesian: #如果设置为True，那么走直线
#wpose.position.x += 0.020
waypoints.append(deepcopy(wpose))
else: #否则就走曲线
arm.set_pose_target(wpose) #自由曲线
arm.go()
rospy.sleep(1)
wpose.position.x += 0.01
if cartesian:
#wpose.position.x += 0.030
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
wpose.position.y += 0.1
if cartesian:
#wpose.position.x += 0.040
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
wpose.position.x -= 0.15
wpose.position.y -= 0.1
if cartesian:
#wpose.position.x += 0.050
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
#规划过程
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
#规划路径 ，fraction返回1代表规划成功
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表，这里是5个点
0.01, # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达
0.0, # jump_threshold，跳跃阈值，设置为0代表不允许跳跃
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
rospy.sleep(5)
# 控制机械臂先回到初始化位置
#arm.set_named_target('init_pose')
#arm.go()
#rospy.sleep(5)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_controller', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', False)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的“forward”姿态
arm.set_named_target('forward')
arm.go()
rospy.sleep(10)
# 获取当前位姿数据最机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
# if cartesian:
# waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.01米，向右运动0.01米
wpose = deepcopy(start_pose)
wpose.position.x -= 0.5
wpose.position.y += 0.05
wpose.position.z -= 0
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_start_state_to_current_state()
arm.set_pose_target(wpose)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(10)
# 设置第三个路点数据，并加入路点列表
wpose.position.x += 0
wpose.position.y -= 0.05
wpose.position.z += 0.05
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_start_state_to_current_state()
arm.set_pose_target(wpose)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(10)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_start_state_to_current_state()
arm.set_pose_target(start_pose)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(10)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.001, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction >= 0.99:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_start_state_to_current_state()
arm.set_named_target('home')
traj = arm.plan()
arm.execute(traj)
rospy.sleep(10)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01) # 0.01
right_arm.set_goal_orientation_tolerance(0.1) # 0.05
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_extend_full')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
rospy.loginfo("=============> DEBUG: start pose - EXTENDED")
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.z -= 0.1
if cartesian:
rospy.loginfo("=============> DEBUG: cartesian1")
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
rospy.loginfo("=============> DEBUG: non-cartesian1")
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
#wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
rospy.loginfo("=============> DEBUG: cartesian2")
waypoints.append(deepcopy(wpose))
else:
rospy.loginfo("=============> DEBUG: non-cartesian2")
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
rospy.loginfo("=============> DEBUG: cartesian3")
waypoints.append(deepcopy(start_pose))
else:
rospy.loginfo("=============> DEBUG: non-cartesian3")
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
rospy.loginfo("=============> DEBUG: debug4")
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
rospy.loginfo("=============> DEBUG: debug5")
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# Move normally back to the 'resting' position
rospy.loginfo("=============> DEBUG: done, moving home")
right_arm.set_named_target('right_arm_home')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的“forward”姿态
# arm.set_named_target('up')
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
-1.48277777778, -3.14, 1.57, -3.14, -1.57, 3.14
] #[-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
arm.set_joint_value_target(joint_positions)
arm.go()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
#print "start_pose",start_pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
scale = 1
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
# wpose.position.z -= scale * 0.1 # First move up (z)
# wpose.position.y += scale * 0.2
wpose.position.x += 0.1
#wpose.position.y += 0.01
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# # 设置第三个路点数据，并加入路点列表
# wpose.position.x += 0.01
# #wpose.position.y += 0.015
# #wpose.position.z -= 0.015
#
# if cartesian:
# waypoints.append(deepcopy(wpose))
# else:
# arm.set_pose_target(wpose)
# arm.go()
# rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('up')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState,
self.__joint_state_cb, queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.")
if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False:
rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.")
self.reset_world()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
goal_point = Point(0.45, -0.2, 1.4)
goal_ori = Quaternion(0.0,0.0,0.0,1.0)
goal_pose = Pose(goal_point, goal_ori)
right_arm_mgc.set_pose_reference_frame('base_link')
rospy.loginfo("Planning on frame:" + str(right_arm_mgc.get_planning_frame()))
waypoints = []
waypoints.append(init_pose)#.pose)
waypoints.append(goal_pose)
eef_step_max = 0.05
jump_thresh_max = 10000
while True:
curr_eef_step = eef_step_max * random()
curr_jump_thresh = jump_thresh_max * random()
#path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, curr_eef_step, curr_jump_thresh, False)
path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, 0.01, 1.2, False)
rospy.loginfo("Fraction and path:")
rospy.loginfo(str(fraction))
if fraction <= 0:
rospy.logerr("Couldnt compute path with eef_step:"+ str(curr_eef_step) + " and curr_jump_thresh: " + str(curr_jump_thresh))
else:
rospy.logwarn("!!!!! Found a solution with eef_step: " + str(curr_eef_step) + " and curr_jump_thresh: " + str(curr_jump_thresh))
break
rospy.loginfo(str(path))
#waypoints_pa = PoseArray()
#path = RobotTrajectory()
for point in path.joint_trajectory.points: # THIS ONLY SENDS THE MESSAGE FOR THE FIRST JOINT
joint_stt = JointState()
def callback(pose):
object_position_info = pose.position
object_orientation_info = pose.orientation
print object_position_info
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_cartesian', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
#set cartesian parameters
ur5_manipulator = MoveGroupCommander('manipulator')
ur5_gripper = MoveGroupCommander('gripper')
ur5_manipulator.allow_replanning(True)
ur5_manipulator.set_pose_reference_frame('base_link')
ur5_manipulator.set_goal_position_tolerance(0.01)
ur5_manipulator.set_goal_orientation_tolerance(0.1)
end_effector_link = ur5_manipulator.get_end_effector_link()
ur5_manipulator.set_named_target('home_j')
ur5_manipulator.go()
ur5_gripper.set_named_target('open')
ur5_gripper.go()
#get the end effort information
start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose
print("The first waypoint:")
print(start_pose)
#define waypoints
waypoints = []
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.z = object_position_info.z+0.25
wpose.position.x = object_position_info.x
wpose.position.y = object_position_info.y
print("The second waypoint:")
print(wpose)
waypoints.append(deepcopy(wpose))
print(" ")
print(waypoints)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = ur5_manipulator.compute_cartesian_path (
waypoints,
0.01,
0.0,
True)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
ur5_manipulator.execute(plan)
rospy.sleep(2)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
rospy.sleep(3)
ur5_gripper.set_named_target("close")
plan = ur5_gripper.go()
rospy.sleep(2)
ur5_manipulator.set_named_target('home_j')
ur5_manipulator.go()
rospy.sleep(3)
moveit_commander.roscpp_shutdown()
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
0.07100913858593216, -1.8767615298285376, 2.0393206555899503,
-1.8313959190971882, -0.6278395875738125, 1.6918219826764682
]
self.robot_arm.go(marker_joint_goal, wait=True)
def look_object(self):
look_object = self.robot_arm.get_current_joint_values()
## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함
print "wrist3 joint value(deg, rad): ", math.degrees(
look_object[5]), look_object[5]
look_object[5] = math.radians(90)
'''
if look_object[5] > abs(math.radians(180)):
if look_object[5] < 0:
look_object[5] = look_object[5] + math.radians(90) # wrist3
elif look_object[5] > 0:
look_object[5] = look_object[5] - math.radians(90)
else:
look_object[5] = look_object[5] - math.radians(90) # wrist3
'''
print "wrist3 joint value(deg, rad): ", math.degrees(
look_object[5]), look_object[5]
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap... Please wait...."
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def plan_cartesian_path(self, x_offset, y_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.goal_y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
print "======= pos(meter): ", self.position_list
print "======= orientation: ", self.orientation_list
def go_to_move(self, scale=1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
print "========== robot arm plannig frame: \n", planning_frame
self.calculed_coke_pose.position.x = (
scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale * self.goal_y) - 0.25
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + 0.72 + 0.2 # world to base_link z-offset real version offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 1.54))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [
self.calculed_coke_pose.position.x,
self.calculed_coke_pose.position.y,
self.calculed_coke_pose.position.z
]
tf_display_orientation = [
self.calculed_coke_pose.orientation.x,
self.calculed_coke_pose.orientation.y,
self.calculed_coke_pose.orientation.z,
self.calculed_coke_pose.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def ar_tf_listener(self, msg):
try:
self.marker = msg.markers
ml = len(self.marker)
target_id = 9
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_id:
pass
#target_id_flage = False
elif self.m_idd == target_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[
target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[
target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[
target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[
target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[
target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[
target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[
target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.ar_pose.orientation.x, self.ar_pose.orientation.y,
self.ar_pose.orientation.z, self.ar_pose.orientation.w
]
(self.goal_roll,
self.goal_pitch, self.goal_yaw) = euler_from_quaternion(
self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.2)
arm.set_goal_orientation_tolerance(0.1)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
print(end_effector_link)
# 控制机械臂先回到初始化位置
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
print(start_pose)
# 初始化路点列表
waypoints = []
os.system("rosrun pick_test send_gripper.py --value 0.0")
# 将初始位姿加入路点列表
# arm.set_named_target('test5')
# arm.go()
# rospy.sleep(1)
######
marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot)
listener = tf.TransformListener()
r = rospy.Rate(10)
while not rospy.is_shutdown():
try:
(trans,
rot) = listener.lookupTransform('/base_link', '/ar_marker_4',
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
######
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
print carrot
wpose.position.z -= carrot.z
waypoints.append(deepcopy(wpose))
# wpose.position.z -= carrot.z+0.1
# wpose.position.y += carrot.y
# waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 控制机械臂先回到初始化位置
waypoints = []
test_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(test_pose)
wpose.position.x -= carrot.x + 0.03
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.02, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
########
os.system("rosrun pick_test send_gripper.py --value 0.8")
########
waypoints = []
pick_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(pick_pose)
wpose.position.z += 0.1
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.02, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def main():
# moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('get_position_client')
scene = PlanningSceneInterface()
objects = scene.get_known_object_names()
# print("objects", objects)
# print("scene", scene)
left_arm = MoveGroupCommander("left_arm")
# left_gripper_group = moveit_commander.MoveGroupCommander("left_gripper")
right_arm = MoveGroupCommander("right_arm")
# right_gripper = moveit_commander.MoveGroupCommander("right_gripper")
dual_arm_group = MoveGroupCommander("dual_arm")
grippers = MoveGroupCommander("grippers")
get_position = rospy.ServiceProxy('get_position', target_position)
rate = rospy.Rate(2.0)
done_l = False
done_r = False
# dual_joints = dual_arm_group.get_joints()
# print("dual_joints" , dual_joints)
# dual_joint_values = dual_arm_group.get_current_joint_values()
# print("dual_joint_values", dual_joint_values)
# left_arm_group.set_named_target("left_home")
# plan1 = left_arm_group.plan()
# rospy.sleep(2)
# left_arm_group.go()
# print "...Left Done..."
# rospy.sleep(1)
dual_arm_group.set_named_target("both_home")
plan1 = dual_arm_group.plan()
rospy.sleep(2)
# print("dual_arm plan1", plan1)
dual_arm_group.go()
print "...Both Done..."
rospy.sleep(1)
# print("scene", scene)
while not rospy.is_shutdown():
try:
rospy.wait_for_service('get_position', timeout=5)
res = get_position()
left_handle = res.points[0]
right_handle = res.points[1]
# scene.remove_world_object()
# print "right_handle" , right_handle
# pose_target_l = []
# pose_target_l.append(( left_handle.x - 0.03))
# pose_target_l.append(left_handle.y)
# pose_target_l.append(left_handle.z)
# pose_target_r = []
# pose_target_r.append(right_handle.x)
# pose_target_r.append(right_handle.y)
# pose_target_r.append(right_handle.z)
# print(pose_target_l)
print('left_handle', left_handle, 'right_handle', right_handle)
# left_arm_group.set_position_target(pose_target_l)
pose_target_l = Pose()
pose_target_r = Pose()
# pose_target_l.position = left_handle;
# pose_target_l.position.x -= 0.06
# pose_target_l.position.y -= 0.005
# pose_target_l.position.z -= 0.005
# q = quaternion_from_euler(-1.57, 0, -1.57) # Horizental Gripper : paraller to x-axis
# # q = quaternion_from_euler(-2.432, -1.57, -0.709) # Vertical Gripper: Vertical to x-axis
# pose_target_l.orientation.x = q[0]
# pose_target_l.orientation.y = q[1]
# pose_target_l.orientation.z = q[2]
# pose_target_l.orientation.w = q[3]
# print ("pose_target_l",pose_target_l )
# left_arm_group.set_pose_target(pose_target_l)
# left_arm_group.set_planning_time(10)
# plan1 = left_arm_group.plan()
# rospy.sleep(2)
# left_arm_group.go()
# done_l = True
# pose_target_r.position.x = 0.5232
# pose_target_r.position.y = -0.2743
# pose_target_r.position.z = 0.6846
# right_gripper.set_named_target("right_open")
# right_gripper.plan()
# right_gripper.go()
pose_target_r.position = right_handle
translation = get_translations(right_handle)
# pose_target_r.position.x -= 0.065
pose_target_r.position.x -= translation.x
print("translation.x", translation.x)
pose_target_r.position.y -= translation.y
pose_target_r.position.z -= translation.z
# pose_target_r.position.z = 0.7235
# q = quaternion_from_euler(-1.57, 0, -1.57) # This works from 2.35 : Gripper paraller to x-axis
q = quaternion_from_euler(
-1.57, 1.57, -1.57
) # This work from 2.35 : Gripper Vertical to x-axis (-2.36, 1.5708, -2.36)
pose_target_r.orientation.x = q[0]
pose_target_r.orientation.y = q[1]
pose_target_r.orientation.z = q[2]
pose_target_r.orientation.w = q[3]
# print ("pose_target_r",pose_target_r )
right_arm.set_pose_target(pose_target_r)
right_arm.set_planning_time(10)
plan1 = right_arm.plan()
# rospy.sleep(1)
# right_arm.go()
if done_r == False:
# print ("target pose",pose_target_r)
if (plan1.joint_trajectory.points
): # True if trajectory contains points
# last = (len(plan1.joint_trajectory.points) - 1)
# print("joint_trajectory", (plan1.joint_trajectory.points[-1].positions) ) # getting the last position of trajectory
r_joints = plan1.joint_trajectory.points[-1].positions
d_joints = get_dual_trajectory(r_joints)
# print("l_joints", l_joints)
# d_joints = l_joints + list(r_joints)
# print ("d_joints", d_joints)
dual_arm_group.set_joint_value_target(d_joints)
plan2 = dual_arm_group.plan()
if (plan2.joint_trajectory.points):
move_success = dual_arm_group.execute(plan2, wait=True)
# eef_pose = right_arm.get_current_pose()
# print("eef_pose", eef_pose)
# move_success = right_arm.execute(plan1, wait = True)
if move_success == True:
rospy.sleep(2)
right_arm.set_start_state_to_current_state()
left_arm.set_start_state_to_current_state()
waypoints_l = []
waypoints = []
wpose = right_arm.get_current_pose().pose
wpose_l = left_arm.get_current_pose().pose
print("wpose", wpose)
# Open the gripper to the full position
grippers.set_named_target("both_open")
grippers.plan()
grippers.go()
# Create Cartesian Path to move forward mainting the end-effector pose
# waypoints = []
# rospy.sleep(5)
# wpose = right_arm.get_current_pose().pose
if (translation.x >= 0.0505):
wpose.position.x += (translation.x + 0.00
) # move forward in (x)
wpose_l.position.x += (translation.x + 0.002)
wpose_l.position.z += 0.001
else:
wpose.position.x += 0.051 # move forward in (x)
wpose_l.position.x += 0.053
wpose_l.position.z += 0.001
waypoints.append(deepcopy(wpose))
(plan1,
fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0)
waypoints_l.append(deepcopy(wpose_l))
(plan_l,
fraction) = left_arm.compute_cartesian_path(
waypoints_l, # waypoints to follow
0.01, # eef_step
0.0)
# print("plan1 len", len(plan1.joint_trajectory.points))
# waypoints.append(copy.deepcopy(wpose))
# (plan2, fraction) = dual_arm_group.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.005, # eef_step
# 0.0)
rospy.sleep(1)
if plan1.joint_trajectory.points:
move_success = right_arm.execute(plan1,
wait=True)
if move_success == True:
rospy.loginfo(
"Right Move forward successful")
# done_r = True
# break;
if plan_l.joint_trajectory.points:
move_success_l = left_arm.execute(plan_l,
wait=True)
if move_success_l == True:
rospy.loginfo(
"Left Move forward successful")
# done_r = True
if move_success == True and move_success_l == True:
rospy.sleep(1)
grippers.set_named_target("both_close")
grippers.plan()
grippers.go()
# rospy.sleep(1)
waypoints = []
rospy.sleep(1)
wpose = right_arm.get_current_pose().pose
# q = quaternion_from_euler(-1.57, 1.57, -1.57) # wrist up 5 degrees = 1.66 10deg = 1.75
# wpose.orientation.x = q[0]
# wpose.orientation.y = q[1]
# wpose.orientation.z = q[2]
# wpose.orientation.w = q[3]
wpose.position.z += 0.07 # move up in (z)
waypoints.append(deepcopy(wpose))
(plan1,
fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0)
if plan1.joint_trajectory.points:
# move_success = right_arm.execute(plan1, wait=True) # for testing right arm trajectory
# done_r = True
r_joints = plan1.joint_trajectory.points[
-1].positions
# print ("r_joints", r_joints)
d_joints = get_dual_trajectory(r_joints)
# print ("d_joints", d_joints)
dual_arm_group.set_joint_value_target(
d_joints)
# plan2 = 0
plan2 = dual_arm_group.plan()
# print("planed plan len", len(plan2.joint_trajectory.points))
# print("dual arm trajectory", (plan2.joint_trajectory.points))
# done_r = True
if (plan2.joint_trajectory.points):
move_success = dual_arm_group.go()
print("move_success", move_success)
done_r = True
# traj_points = plan2.joint_trajectory.points[0:-3]
# plan2.joint_trajectory.points = traj_points
# print("eddited plan len", len(plan2.joint_trajectory.points))
# move_success = dual_arm_group.execute(plan2, wait = True)
# # done_r = True
# move_success = right_arm.execute(plan1, wait=True)
# if move_success == True:
# rospy.loginfo ("Lift Successful")
# done_r = True
# r_joints = plan1.joint_trajectory.points[-1].positions
# d_joints = get_dual_trajectory(r_joints)
# # print("l_joints", l_joints)
# # d_joints = l_joints + list(r_joints)
# # print ("d_joints", d_joints)
# dual_arm_group.set_joint_value_target(d_joints)
# plan2 = dual_arm_group.plan()
# if (plan2.joint_trajectory.points) :
# move_success = dual_arm_group.execute(plan2, wait = True)
# if move_success == True:
# rospy.loginfo("Move forward successful")
# rospy.sleep(1)
# grippers.set_named_target("both_close")
# grippers.plan()
# grippers.go()
# rospy.sleep(1)
# waypoints = []
# rospy.sleep(2)
# wpose = right_arm.get_current_pose().pose
# wpose.position.z += 0.1 # move up in (z)
# waypoints.append(copy.deepcopy(wpose))
# (plan1, fraction) = right_arm.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.01, # eef_step
# 0.0)
# if plan1.joint_trajectory.points:
# r_joints = plan1.joint_trajectory.points[-1].positions
# d_joints = get_dual_trajectory(r_joints)
# # print ("d_joints", d_joints)
# dual_arm_group.set_joint_value_target(d_joints)
# plan2 = dual_arm_group.plan()
# if (plan2.joint_trajectory.points) :
# move_success = dual_arm_group.execute(plan2, wait = True)
# done_r = True
# move_success = right_arm.execute(plan1, wait=True)
# if move_success == True:
# rospy.loginfo ("Lift Successful")
# done_r = True
# else:
# rospy.logwarn("Cartesian Paht Planning Failed for forward movement")
else:
print("Execution Completed")
except rospy.ROSException:
rospy.logerr('get_position_server did not respond in 5 sec')
return
rate.sleep()
def inverse_kinematics():
# Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 50
request.ik_request.pose_stamped.header.frame_id = "base"
# Create joint constraints
#This is joint constraint will need to be set at the group
constraints = Constraints()
joint_constr = JointConstraint()
joint_constr.joint_name = "right_j0"
joint_constr.position = TORSO_POSITION
joint_constr.tolerance_above = TOLERANCE
joint_constr.tolerance_below = TOLERANCE
joint_constr.weight = 0.5
constraints.joint_constraints.append(joint_constr)
# Get the transformed AR Tag (x,y,z) coordinates
# Only care about the x coordinate of AR tag; tells use
# how far away wall is
# x,y, z tell us the origin of the AR Tag
x_coord = board_x # DONT CHANGE
y_coord = bounding_points[0]
z_coord = bounding_points[1]
y_width = bounding_points[2]
z_height = bounding_points[3]
#Creating Path Planning
waypoints = []
z_bais = 0
print(
"OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!"
)
print(bounding_points)
for i in range(int(float(z_height) / .03)):
target_pose = Pose()
target_pose.position.x = float(x_coord - PLANNING_BIAS)
if i % 2 == 0:
#Starting positions
target_pose.position.y = float(y_coord)
else:
target_pose.position.y = y_coord + float(y_width)
#Starting positions
target_pose.position.z = float(z_coord + z_bais)
target_pose.orientation.y = 1.0 / 2**(1 / 2.0)
target_pose.orientation.w = 1.0 / 2**(1 / 2.0)
waypoints.append(target_pose)
z_bais += .03
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
try:
#Send the request to the service
response = compute_ik(request)
group = MoveGroupCommander("right_arm")
group.set_max_velocity_scaling_factor(0.75)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
#Creating a Robot Trajectory for the Path Planning
jump_thres = 0.0
eef_step = 0.1
path, fraction = group.compute_cartesian_path([target_pose],
eef_step, jump_thres)
print("Path fraction: {}".format(fraction))
#Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
#Setting the Joint constraint
group.set_path_constraints(constraints)
if fraction < 0.5:
group.go()
else:
group.execute(path)
if i < int(float(z_height) / 0.03) and i > 0:
target2 = target_pose
target2.position.z += 0.03
path, fraction = group.compute_cartesian_path([target2],
eef_step,
jump_thres)
group.set_path_constraints(constraints)
group.execute(path)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
pointing_height = 0.06
grasp_height = 0.05
drop_height = 0.07
cruising_height = 0.1
START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving
GOAL_TOLERANCE = 0.005
moving = False
paused = False
move_group_state = "IDLE"
last_joint_trajectory_goal = ""
last_joint_trajectory_result = ""
def __init__(self, num_planning_attempts=100, safe_zone=None):
rospy.init_node("daarm_server", anonymous=True)
self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}]
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def raise_table(self):
#raises the table obstacle to protect blocks on the table during transport
self.scene.remove_world_object("blockProtector")
self.scene.add_box("blockProtector", self.blockProtectorPose,
self.blockProtectorDimension)
def lower_table(self):
#lowers the table to allow grasping into it
self.scene.remove_world_object("blockProtector")
def init_params(self):
try:
self.grasp_height = get_ros_param(
"GRASP_HEIGHT", "Grasp height defaulting to 0.01")
self.drop_height = get_ros_param("DROP_HEIGHT",
"Drop height defaulting to 0.07")
self.cruising_height = get_ros_param(
"CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
self.pointing_height = get_ros_param(
"POINT_HEIGHT", "Pointing height defaulting to 0.06")
except ValueError as e:
rospy.loginfo(e)
def handle_param_update(self, message):
self.init_params()
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results",
CalibrationParams,
queue_size=1)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs",
String,
queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles,
self.update_joints)
self.joint_trajectory_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/status', GoalStatusArray,
self.update_joint_trajectory_state)
self.joint_trajectory_goal_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal)
self.joint_trajectory_result_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self.update_joint_trajectory_result)
self.finger_position_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/finger_position', FingerPosition,
self.update_finger_position)
self.param_update_subscriber = rospy.Subscriber(
"/param_update", String, self.handle_param_update)
self.moveit_status_subscriber = rospy.Subscriber(
'/move_group/status', GoalStatusArray,
self.update_move_group_status)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback,
self.update_move_group_state)
#Topic for getting joint torques
rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles,
self.monitorJointTorques)
#Topic for getting cartesian force on end effector
rospy.Subscriber('/j2s7s300_driver/out/tool_wrench',
geometry_msgs.msg.WrenchStamped,
self.monitorToolWrench)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer(
"calibrate_arm", CalibrateAction, self.calibrate, auto_start=False)
self.calibration_server.start()
self.move_block_server = actionlib.SimpleActionServer(
"move_block",
MoveBlockAction,
self.handle_move_block,
auto_start=False)
self.move_block_server.start()
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
self.move_pose_server = actionlib.SimpleActionServer(
"move_pose",
MovePoseAction,
self.handle_move_pose,
auto_start=False)
self.move_pose_server.start()
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand,
self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand,
self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand,
self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand,
self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
#
def init_service_clients(self):
self.is_simulation = False
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
#self.home_arm_client()
self.home_arm_kinova()
return "done"
def custom_home_arm(self):
angles_set = [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 539.743164062
]
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angles_set[0]
goal.angles.joint2 = angles_set[1]
goal.angles.joint3 = angles_set[2]
goal.angles.joint4 = angles_set[3]
goal.angles.joint5 = angles_set[4]
goal.angles.joint6 = angles_set[5]
goal.angles.joint7 = angles_set[6]
self.joint_action_client.send_goal(goal)
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
# self.arm.set_named_target("Home")
angles_set = map(np.deg2rad, [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 269.743164062
])
self.arm.clear_pose_targets()
try:
self.arm.set_joint_value_target(angles_set)
except MoveItCommanderException as e:
pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
# This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value
def monitorJointTorques(self, torques):
if abs(torques.joint1) > 1:
return
#self.emergency_stop() #Stop arm driver
#rospy.sleep(1.0)
#self.group.stop() #Stop moveit execution
# This callback function monitors the Joint Wrench and stops the current
# execution if the Joint Wrench exceeds certain value
def monitorToolWrench(self, wrenchStamped):
return
#toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2)
##print toolwrench
#if toolwrench > 100:
# self.emergency_stop() # Stop arm driver
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn)
goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn)
goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
print(message)
pick_x = message.source.x
pick_y = message.source.y
pick_x_threshold = message.source_x_tolerance
pick_y_threshold = message.source_y_tolerance
block_id = message.id
place_x = message.target.x
place_y = message.target.y
place_x_threshold = message.target_x_tolerance
place_y_threshold = message.target_y_tolerance
self.move_block(block_id, pick_x, pick_y, pick_x_threshold,
pick_y_threshold, place_x, place_y, place_x_threshold,
place_y_threshold, message.block_size)
def handle_pick_failure(self, exception):
rospy.loginfo("Pick failed, going home.")
self.open_gripper()
self.home_arm()
raise exception
def handle_place_failure(self, safe_zone, block_size, exception):
#should probably figure out if I'm holding the block first so it doesn't look weird
#figure out how to drop the block somewhere safe
#pass none and none if you are certain you haven't picked up a block yet
if safe_zone is None and block_size is None:
self.home_arm()
raise exception
rospy.loginfo("HANDLING PLACE FAILURE")
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(
safe_zone[0]['x'],
safe_zone[0]['y'],
safe_zone[1]['x_tolerance'],
safe_zone[1]['y_tolerance'],
current_block_state,
block_size,
num_attempts=1000)
try:
arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("panic arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
except Exception as e:
rospy.loginfo(
"ERROR: Cannot panic place the block! Get ready to catch it!")
self.open_gripper()
self.home_arm()
raise exception
def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
return candidate_blocks
def move_block(self,
block_id,
pick_x,
pick_y,
pick_x_tolerance,
pick_y_tolerance,
place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
block_size=None,
safe_zone=None,
pick_tries=2,
place_tries=1,
point_at_block=True):
if block_size is None:
block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
# check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
# moved while we are checking for a drop location
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
None, None,
Exception("no room in the target zone to drop the block"))
# here we are actually building a set of candidate blocks to pick
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
# select best block to pick and pick up
pick_location = None
if len(candidate_blocks) < 1:
raise Exception("no block of id " + str(block_id) +
" found within the source zone")
elif len(candidate_blocks) == 1:
pick_location = Point(candidate_blocks[0]['x'],
candidate_blocks[0]['y'], 0)
else:
pick_location = Point(*self.find_most_isolated_block(
candidate_blocks, current_block_state))
if point_at_block == True:
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.close_gripper()
self.point_at_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0))
self.point_at_block(location=Point(arm_drop_location[0],
arm_drop_location[1], 0))
self.home_arm()
except Exception as e:
rospy.loginfo(str(e))
rospy.loginfo("failed trying to point at block. giving up.")
self.home_arm()
self.move_block_server.set_succeeded(
MoveBlockResult(drop_location))
return
for i in range(pick_tries):
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.pick_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0),
check_grasp=True)
break
except Exception as e:
if i < pick_tries - 1:
rospy.loginfo("pick failed and trying again..." + str(e))
else:
rospy.loginfo("pick failed and out of attempts..." +
str(e))
self.handle_pick_failure(e)
if safe_zone == None:
if self.safe_zone == None:
safe_zone = [{
'x': pick_x,
'y': pick_y
}, {
'x_tolerance': pick_x_tolerance,
'y_tolerance': pick_y_tolerance
}]
else:
safe_zone = self.safe_zone
# calculate drop location
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
safe_zone, block_size,
Exception("no room in the target zone to drop the block"))
rospy.loginfo("tuio drop" + str(drop_location))
for i in range(place_tries):
try:
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
break
except Exception as e:
if i < place_tries - 1:
rospy.loginfo("place failed and trying again..." + str(e))
else:
rospy.loginfo("place failed and out of attempts..." +
str(e))
# check to see if we've defined a safe zone to drop the blocks
self.handle_place_failure(safe_zone, block_size, e)
# assume success if we made it this far
self.move_block_server.set_succeeded(MoveBlockResult(drop_location))
# check if a certain x, y position is within the bounds of another x,y position
@staticmethod
def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold):
if x <= x_origin + x_threshold and x >= x_origin - x_threshold \
and y <= y_origin + y_threshold and y >= y_origin - y_threshold:
return True
return False
# calculate the best location to drop the block
def calculate_drop_location(self,
x,
y,
x_threshold,
y_threshold,
blocks,
block_size,
num_attempts=1000):
attempt = 0
x_original, y_original = x, y
while (attempt < num_attempts):
valid = True
for block in blocks:
if self.check_block_bounds(block['x'], block['y'], x, y,
0.8 * block_size, block_size):
valid = False
break
if valid:
return Point(x, y, 0)
else:
x = random.uniform(x_original - x_threshold,
x_original + x_threshold)
y = random.uniform(y_original - y_threshold,
y_original + y_threshold)
attempt += 1
#if none found in num_attempts, return none
return None
# candidates should have more than one element in it
@staticmethod
def find_most_isolated_block(candidates, all_blocks):
print(candidates)
min_distances = [] # tuples of candidate, distance to closest block
for candidate in candidates:
min_dist = -1
for block in all_blocks:
if block['x'] == candidate['x'] and block['y'] == candidate[
'y']:
continue
else:
dist = DaArmServer.block_dist(candidate, block)
if min_dist == -1 or dist < min_dist:
min_dist = dist
min_distances.append([candidate, min_dist])
most_isolated, _ = max(min_distances, key=lambda x: x[
1]) # get most isolated candidate, and min_distance
return most_isolated['x'], most_isolated['y'], 0
@staticmethod
def block_dist(block_1, block_2):
return np.sqrt((block_2['x'] - block_1['x'])**2 +
(block_2['y'] - block_1['y'])**2)
def update_finger_position(self, message):
self.finger_positions = [
message.finger1, message.finger2, message.finger3
]
def check_grasp(self):
closed_pos = 0.95 * 6800
distance_from_closed = 0
for fp in self.finger_positions:
distance_from_closed += (closed_pos - fp)**2
if np.sqrt(distance_from_closed
) > 130: #this is just some magic number for now
return True #if the fingers aren't fully closed, then grasp is good
else:
return False
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
try:
rospy.loginfo("Opening Gripper!!")
self.move_fingers(50, 50, 50)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
# self.gripper.set_named_target("Close")
# self.gripper.go()
try:
rospy.loginfo("Closing Gripper!!")
self.move_fingers(95, 95, 95)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def handle_move_pose(self, message):
# takes a geometry_msgs/Pose message
self.move_arm_to_pose(message.target.position,
message.target.orientation,
action_server=self.move_pose_server)
self.move_pose_server.set_succeeded()
def check_plan_result(self, target_pose, cur_pose):
#we'll do a very lenient check, this is to find failures, not error
#also only checking position, not orientation
rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose))
if np.abs(target_pose.pose.position.x -
cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2:
print("x error too far")
return False
if np.abs(target_pose.pose.position.y -
cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2:
print("y error too far")
return False
if np.abs(target_pose.pose.position.z -
cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2:
print("z error too far")
return False
print("tolerable error")
return True
# expects cooridinates for position to be in arm space
def move_arm_to_pose(self,
position,
orientation,
delay=0.5,
waypoints=[],
corrections=4,
action_server=None):
for i in range(corrections + 1):
rospy.loginfo("TRY NUMBER " + str(i))
if len(waypoints) > 0 and i < 1:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(
waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
rospy.loginfo("PLANNING TO " + str(p))
self.arm.set_pose_target(p)
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
plan = self.arm.go(wait=False)
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 90 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 90 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
if (self.check_plan_result(p, self.arm.get_current_pose())):
break #we're there, no need to retry
#rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE")
if False:
# # get the last pose to correct if desired
# ptPos = plan.joint_trajectory.points[-1].positions
# # print "=================================="
# # print "Last point of the current trajectory: "
# angle_set = list()
# for i in range(len(ptPos)):
# tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
# angle_set.append(tempPos)
if action_server:
pass
#action_server.publish_feedback(CalibrateFeedback("Plan Found"))
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
self.arm.execute(plan, wait=False)
# this is a bit naive, but I'm going to loop until a new trajectory goal gets published
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 15 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 15 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
#action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
pass
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def update_move_group_status(self, message):
if message.status_list:
#rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status))
self.move_group_status = message.status_list[0].status
def update_joint_trajectory_state(self, message):
# print(message.status_list)
if len(message.status_list) > 0:
self.joint_trajectory_state = message.status_list[0].status
else:
self.joint_trajectory_state = 0
def update_joint_trajectory_goal(self, message):
#print("goalisasis", message.goal_id.id)
self.last_joint_trajectory_goal = message.goal_id.id
def update_joint_trajectory_result(self, message):
#print("resultisasis", message.status.goal_id.id)
self.last_joint_trajectory_result = message.status.goal_id.id
def get_grasp_orientation(self, position):
#return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2))
return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633)
def update_joints(self, joints):
self.joint_angles = [
joints.joint1, joints.joint2, joints.joint3, joints.joint4,
joints.joint5, joints.joint6, joints.joint7
]
def move_z_relative(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def move_z_absolute(self, height):
p = self.arm.get_current_pose()
p.pose.position.z = height
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self,
location,
check_grasp=False,
retry_attempts=0,
delay=0,
action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
# print('Position: ', position)
self.open_gripper()
position = Point(location.x, location.y, self.cruising_height)
orientation = self.get_grasp_orientation(position)
try:
self.raise_table()
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
position = Point(location.x, location.y, self.grasp_height)
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.crusing_height)
self.raise_table()
raise (
e
) #problem because sometimes we get exception e.g. if we're already there
# and it will skip the close if so.
if action_server:
action_server.publish_feedback()
self.close_gripper()
self.move_z_absolute(self.cruising_height)
#try to wait until we're up to check grasp
rospy.sleep(0.5)
if check_grasp is True:
if (self.check_grasp() is False):
print("Grasp failed!")
self.raise_table()
raise (Exception("grasp failed!"))
# for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.raise_table()
rospy.sleep(delay)
def place_block(self,
location,
check_grasp=False,
delay=0,
action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.drop_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z_absolute(self.cruising_height)
self.raise_table()
self.close_gripper()
def point_at_block(self, location, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.pointing_height)
self.move_z_absolute(self.cruising_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.raise_table()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
print("moving on...")
self.calibration_server.publish_feedback(
CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(
CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(
CalibrateFeedback("moving to drop"))
try:
self.move_arm_to_pose(Point(0.4, -0.4, 0.1),
Quaternion(1, 0, 0, 0),
corrections=0)
except Exception as e:
rospy.loginfo("THIS IS TH PRoblem" + str(e))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(
CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0}
calibration_action_belief = {
"action": "add",
"block": calibration_block
}
self.action_belief_publisher.publish(
String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise (ValueError("Failed getting block state to calibrate: " +
str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise (ValueError(
"Failed calibration, either couldn't find block or > 1 block on TUI!"
))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y,
tuio_bounds)
arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('arm')
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set an initial position for the arm
start_position = [
0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1,
3.1415174069
]
# Set the goal pose of the end effector to the stored pose
arm.set_joint_value_target(start_position)
# Plan and execute a trajectory to the goal configuration
arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Move end effector to the right 0.3 meters
wpose.position.y -= 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Move end effector up and back
wpose.position.x -= 0.2
wpose.position.z += 0.3
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses
0.025, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_drawing', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
star_pose = PoseStamped()
star_pose.header.frame_id = reference_frame
star_pose.header.stamp = rospy.Time.now()
star_pose.pose.position.x = 0.40
star_pose.pose.position.y = 0.0
star_pose.pose.position.z = 0.12
star_pose.pose.orientation.w = 1.0
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(star_pose, end_effector_link)
arm.go()
radius = 0.1
centerY = 0.0
centerX = 0.40 - radius
# 初始化路点列表
waypoints = []
starPoints = []
pose_temp = star_pose
for th in numpy.arange(0, 6.2831855, 1.2566371):
pose_temp.pose.position.y = -(centerY + radius * math.sin(th))
pose_temp.pose.position.x = centerX + radius * math.cos(th)
pose_temp.pose.position.z = 0.113
wpose = deepcopy(pose_temp.pose)
starPoints.append(deepcopy(wpose))
# 将圆弧上的路径点加入列表
waypoints.append(starPoints[0])
waypoints.append(starPoints[2])
waypoints.append(starPoints[4])
waypoints.append(starPoints[1])
waypoints.append(starPoints[3])
waypoints.append(starPoints[0])
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点，完成圆弧轨迹
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
#self.clear_octomap()
def ar_callback(self, msg):
#marker = msg.markers[1]
self.marker = msg.markers
ml = len(self.marker)
m_id = self.marker[0].id
print "marker length: ", len(self.marker)
print "marker id: ", m_id
m_idd = []
'''
for ii in range(0, 2):
print(ii)
m_idd[ii] = marker[ii].id
#m_id[ii] = marker[ii].id
print(m_idd[ii])
'''
pos_x = self.marker[0].pose.pose.position.x
pos_y = self.marker[0].pose.pose.position.y
pos_z = self.marker[0].pose.pose.position.z
dist = math.sqrt((pos_x * pos_x) + (pos_y * pos_y))
#ori_x = marker.pose.pose.orientation.x
#ori_y = marker.pose.pose.orientation.y
#ori_z = marker.pose.pose.orientation.z
#ori_w = marker.pose.pose.orientation.w
#print(m_id)
print "==========="
print 'id: ', m_id
print 'pos: ', pos_x, pos_y, pos_z
#print('ori: ', ori_x, ori_y, ori_z, ori_w)
self.goal_x = pos_x - 1.0
self.goal_y = pos_y - pos_y
self.goal_z = pos_z - pos_z
def move_code(self):
robot_state = self.robot_arm.get_current_pose()
print "====== robot pose: \n",
print robot_state.pose.position
#marker_joint_goal = self.robot_arm.get_current_joint_values()
marker_joint_goal = [
0.07100913858593216, -1.8767615298285376, 2.0393206555899503,
-1.8313959190971882, -0.6278395875738125, 1.6918219826764682
]
self.robot_arm.go(marker_joint_goal, wait=True)
print "====== robot joint value: \n"
print marker_joint_goal
self.robot_arm.go(marker_joint_goal, wait=True)
self.robot_arm.go(True)
print "look at the markers"
pose_goal = Pose()
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.4
pose_goal.position.y = -0.4
pose_goal.position.z = 0.7
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
#self.robot_arm.set_pose_target(pose_goal)
#7self.robot_arm.go(True)
def plan_cartesian_path(self, scale=1):
waypoints = []
wpose = self.robot_arm.get_current_pose().pose
wpose.position.z -= scale * 0.2
wpose.position.x -= scale * 0.2
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
rospy.loginfo("Maker messages detected. Starting followers...")
#wpose.orientation.w = 1.0
#wpose.position.x = waypoints[0].position.x + 0.1
#wpose.position.y = waypoints[0].position.y
#wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
print waypoints
# second move down
#wpose.position.z -= 0.10
#waypoints.append(copy.deepcopy(wpose))
# third move to the side
#wpose.position.y += 0.05
#waypoints.append(copy.deepcopy(wpose))
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0, False) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
print fraction
print plan3
rospy.sleep(5)
#print "Sending execution command"
#group.execute(plan3)
#group.go()
#rospy.wait_for_service('compute_ik')
#getJointPosition = rospy.ServiceProxy('compute_ik', GetPositionIK )
#service_request = GetPositionIKRequest()
#service_request.ik_request.group_name = "arm";
#service_request.ik_request.pose_stamped = currentPoseHandLink
if __name__ == '__main__':
rospy.init_node('commander_example', anonymous=True)
group = MoveGroupCommander("right_arm")
# Pose Target 1
pose_target_1 = geometry_msgs.msg.Pose()
pose_target_1.position.x = 0.3
pose_target_1.position.y = -0.1
pose_target_1.position.z = 0.15
pose_target_1.orientation.x = 0.0
pose_target_1.orientation.y = -0.707
pose_target_1.orientation.z = 0.0
pose_target_1.orientation.w = 0.707
# Pose Target 2
pose_target_2 = geometry_msgs.msg.Pose()
pose_target_2 = copy.deepcopy(pose_target_1)
pose_target_2.position.y = -0.5
# Waypoints Motion
waypoints = []
waypoints.append(pose_target_1)
waypoints.append(pose_target_2)
(plan1, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)
rospy.loginfo("Plan:\n{}".format(plan1))
group.execute(plan1)
class Grasp(object):
def __init__(self):
# shadow hand
self.grasp_name = ""
self.joints_and_positions = {}
# smart grasp
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_ball_pose = rospy.ServiceProxy("/gazebo/get_model_state",
GetModelState)
self.__visualisation = rospy.Publisher("~display",
Marker,
queue_size=1,
latch=True)
self.__arm_commander = MoveGroupCommander("arm")
self.__hand_commander = MoveGroupCommander("hand")
self.pick()
def display_grasp(self):
print self.grasp_name
print self.joints_and_positions
def convert_to_xml(self):
grasp = ' <grasp name="' + self.grasp_name + '">' + '\n'
for key, value in self.joints_and_positions.items():
grasp = grasp + ' <joint name="' + \
str(key) + '">' + str(value) + '</joint>\n'
grasp = grasp + ' </grasp>' + '\n'
return grasp
def pick(self):
self.go_to_start()
arm_target = self.compute_arm_target()
self.pre_grasp(arm_target)
self.grasp(arm_target)
self.lift(arm_target)
def compute_arm_target(self):
ball_pose = self.__get_ball_pose.call("cricket_ball", "world")
# come at it from the top
arm_target = ball_pose.pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi / 2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
target_marker = Marker()
target_marker.action = Marker.MODIFY
target_marker.lifetime.from_sec(10.0)
target_marker.type = Marker.ARROW
target_marker.header.stamp = rospy.Time.now()
target_marker.header.frame_id = "world"
target_marker.pose = arm_target
target_marker.scale.z = 0.01
target_marker.scale.x = 0.1
target_marker.scale.y = 0.01
target_marker.color.r = 1.0
target_marker.color.g = 0.8
target_marker.color.b = 0.1
target_marker.color.a = 1.0
self.__visualisation.publish(target_marker)
return arm_target
def pre_grasp(self, arm_target):
self.__hand_commander.set_named_target("open")
plan = self.__hand_commander.plan()
self.__hand_commander.execute(plan, wait=True)
for _ in range(10):
self.__arm_commander.set_start_state_to_current_state()
self.__arm_commander.set_pose_targets([arm_target])
plan = self.__arm_commander.plan()
if self.__arm_commander.execute(plan):
return True
def grasp(self, arm_target):
waypoints = []
waypoints.append(
self.__arm_commander.get_current_pose(
self.__arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.__arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.__arm_commander.compute_cartesian_path(
waypoints, 0.01, 0.0)
print fraction
if not self.__arm_commander.execute(plan):
return False
self.__hand_commander.set_named_target("close")
plan = self.__hand_commander.plan()
if not self.__hand_commander.execute(plan, wait=True):
return False
self.__hand_commander.attach_object("cricket_ball__link")
def lift(self, arm_target):
waypoints = []
waypoints.append(
self.__arm_commander.get_current_pose(
self.__arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.__arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.__arm_commander.compute_cartesian_path(
waypoints, 0.01, 0.0)
print fraction
if not self.__arm_commander.execute(plan):
return False
def go_to_start(self):
self.__arm_commander.set_named_target("start")
plan = self.__arm_commander.plan()
if not self.__arm_commander.execute(plan, wait=True):
return False
self.__hand_commander.set_named_target("open")
plan = self.__hand_commander.plan()
self.__hand_commander.execute(plan, wait=True)
self.__reset_world.call()
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
# self.listener = tf.TransformListener()
#self.goal_x = 0
#self.goal_y = 0
#self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.t_idd = 0
self.t_pose_x = []
self.t_pose_y = []
self.t_pose_z = []
self.t_ori_w = []
self.t_ori_x = []
self.t_ori_y = []
self.t_ori_z = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_move(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
tomato_offset = [0, -0.35, -0.1]
robot_base_offset = 0.873
base_wrist2_offset = 0.1
print(">> robot arm planning frame: \n", planning_frame)
'''
self.calculated_tomato.position.x = (scale*self.goal_x)
self.calculated_tomato.position.y = (scale*self.goal_y)
self.calculated_tomato.position.z = (scale*self.goal_z)
'''
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation = Quaternion(
*quaternion_from_euler(3.14, 0.1, 1.57))
print("=== robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("=== Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
'''
Manipulator is moving to the desired coordinate
Now move to the calculated_tomato_id_goal
'''
calculated_tomato_id_goal = Pose()
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n", Cplanning_frame)
calculated_tomato_id_goal.position.x = desired_goal_pose[0]
calculated_tomato_id_goal.position.y = desired_goal_pose[1]
calculated_tomato_id_goal.position.z = desired_goal_pose[2]
calculated_tomato_id_goal.orientation = Quaternion(
*quaternion_from_euler(desired_goal_euler[0],
desired_goal_euler[1],
desired_goal_euler[2]))
print(">> tomato_id_goal frame: ", desired_goal_pose)
self.robot_arm.set_pose_target(calculated_tomato_id_goal)
tf_display_position = [
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.z
]
tf_display_orientation = [
calculated_tomato_id_goal.orientation.x,
calculated_tomato_id_goal.orientation.y,
calculated_tomato_id_goal.orientation.z,
calculated_tomato_id_goal.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(
copy.deepcopy(calculated_tomato_id_goal))
(tomato_id_goal_plan,
tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(
tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
while rospy.get_time() == 0.0: pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
### Move to Cartesian position
goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
mgc.go(goal_pose.pose)
### Move Cartesian linear
goal_pose.pose.position.z -= 0.1
(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
mgc.execute(traj)
print "Done"
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
wpose.orientation.z = waypoints[i-1].orientation.z - 0.308
wpose.position.y = waypoints[i-1].position.y - 0.1
wpose.position.z = waypoints[i-1].position.z
wpose.position.x = waypoints[i-1].position.x + 0.5
waypoints.append(copy.deepcopy(wpose))
(right, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays Right..."
rospy.sleep(5)
right_arm.execute(right)
roscpp_shutdown()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(ARM_GROUP)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置（单位：米）和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = 'link_6'
# 控制机械臂运动到预定姿态
arm.set_named_target(HOME_POSE)
arm.go()
# rospy.sleep(2)
# 第二段运动：移动到停泊点
anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0]
arm.set_joint_value_target(anchor_pose)
arm.go()
rospy.loginfo("已移动到停泊点")
# 第三段运动：移动到下料点
middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
arm.set_joint_value_target(middle_pose)
arm.go()
rospy.loginfo("已移动到中间点")
start_pose = arm.get_current_pose(END_EFFECTOR_LINK).pose
# 提前规划:下料点位置
place_middle_pose = deepcopy(start_pose)
place_middle_pose.position.x = 0
place_middle_pose.position.y = 0.7
place_middle_pose.position.z = 0.40
place_middle_pose.orientation.x = -0.5
place_middle_pose.orientation.y = 0.5
place_middle_pose.orientation.z = 0.5
place_middle_pose.orientation.w = 0.5
place_pose = deepcopy(start_pose)
place_pose.position.x = -0.52
place_pose.position.y = 0.6
place_pose.position.z = 0.05 + HEIGHT_OF_END_EFFECTOR - 0.14
place_pose.orientation.x = -0.5
place_pose.orientation.y = 0.5
place_pose.orientation.z = 0.5
place_pose.orientation.w = 0.5
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
waypoints.append(place_middle_pose)
waypoints.append(place_pose)
if cartesian:
fraction = 0.0 # 路径规划覆盖率
maxtries = 100 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机械比当前的状态为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # 路点列表
0.01, # 终端步进值
0.0, # 跳跃阈值
True # 避障规划
)
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功，则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.set_start_state_to_current_state()
arm.execute(plan)
# rospy.loginfo(type(plan))
# f = open("plan_of_cartisen_demo.txt", "w")
# f.write(str(plan))
# f.close()
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + "success after " +
str(maxtries) + " attempts.")
rospy.loginfo(arm.get_current_pose(end_effector_link))
def callback(data):
rospy.loginfo(data)
rospy.Subscriber('joint_states', JointState, callback)
rospy.sleep(10)
# 返回运动：移动到中间点
middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0]
arm.set_joint_value_target(middle_pose)
arm.go()
rospy.loginfo("已移动到中间点")
# 返回运动: 移动到停泊点
anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0]
arm.set_joint_value_target(anchor_pose)
arm.go()
rospy.loginfo("已移动到停泊点")
# # 控制机械臂回到初始化位置
# arm.set_named_target(HOME_POSE)
# arm.go()
# rospy.sleep(1)
# print(arm.get_current_pose(end_effector_link))
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
print start_pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(start_pose)
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
wpose.position.z -= 0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.1
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('oval_trajectory_planning', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.3)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('start')
arm.go()
rospy.sleep(1)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.38665
target_pose.pose.position.y = 0
target_pose.pose.position.z = 0.37226
target_pose.pose.orientation.x = 1
target_pose.pose.orientation.y = 0.00007
target_pose.pose.orientation.z = -0.0024684
target_pose.pose.orientation.w = 0.00003
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
# 初始化路点列表
waypoints = []
# 将圆弧上的路径点加入列表
waypoints.append(target_pose.pose)
centerA = target_pose.pose.position.y
centerB = target_pose.pose.position.z
long_axis = 0.03
short_axis=0.06
for th in numpy.arange(0, 6.28, 0.005):
target_pose.pose.position.y = centerA + long_axis * math.cos(th)-0.03
target_pose.pose.position.z = centerB + short_axis * math.sin(th)
wpose = deepcopy(target_pose.pose)
waypoints.append(deepcopy(wpose))
#print('%f, %f' % (Y, Z))
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点，完成圆弧轨迹
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
rospy.loginfo("位姿控制---->椭圆轨迹规划 成功!")
# 控制机械臂先回到初始化位置
arm.set_named_target('start')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the arm move group
arm = MoveGroupCommander('robot')
gripper = MoveGroupCommander("gripper")
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
# arm.set_goal_position_tolerance(0.01)
# arm.set_goal_orientation_tolerance(0.1)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1) #(0.523599)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
gripper.set_named_target("GripperOpen")
gripper.go(wait=True)
rospy.sleep(3) #5
gripper.set_named_target("GripperClose")
gripper.go(wait=True)
rospy.sleep(1)
# # Start in the "cartesian_z" (demo) configuration stored in the SRDF file
arm.set_named_target('cartesian_z')
# # Plan and execute a trajectory to the goal configuration
arm.go(wait=True)
rospy.sleep(5)
# Execute the position again in order to start from there - !!!!workaround
# arm.set_named_target('cartesian_z')
# arm.go(wait=True)
# Get the current pose so we can add it as a waypoint
start_pose = arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.0 #05#0.01
wpose.position.y -= 0.0
wpose.position.z -= 0.25
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.0 #0.05#0.01
wpose.position.y += 0.0
wpose.position.z += 0.38
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.0 #0.05#0.01
wpose.position.y += 0.0
wpose.position.z -= 0.38
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
wpose.position.x += 0.0
wpose.position.y += 0.0
wpose.position.z += 0.25
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0
maxtries = 100 #100
attempts = 0
# Set the internal state to the current state
arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan, wait=True)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# Move normally back to the 'transport_position' position
# arm.set_named_target('cartesian_z')
# arm.go(wait=True)
rospy.sleep(22)
arm.set_named_target('transport_position')
arm.go(wait=True)
rospy.sleep(27)
gripper.set_named_target("GripperOpen")
gripper.go(wait=True)
rospy.sleep(5)
gripper.set_named_target("GripperClose")
gripper.go(wait=True)
rospy.sleep(5)
arm.set_named_target('transport_position')
arm.go(wait=True)
rospy.sleep(27)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def cartesian_move_to(pose_, execute=False):
goal_pose = deepcopy(pose_)
######################################## experiment start
# for hardcoded aruco pose
# some known arm positions
# aruco pose in simulation: 0.52680940312, -0.0490591337742, 0.921301848054, 0.504516550338, 0.506271228009, 0.497290765692, 0.49178693403
# in base_footprint: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0
# pick pose: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0
# final hand pose in simulation: 0.47653800831, -0.396454309047, 1.05656705145, -0.630265833017, -0.318648344327, 0.582106845777, 0.402963810396
# goal_pose = PoseStamped()
# goal_pose.header.frame_id = 'base_footprint'
# goal_pose.pose.position.x = 0.52680940312
# goal_pose.pose.position.y = -0.0490591337742
# goal_pose.pose.position.z = 0.871301848054
# goal_pose.pose.orientation.x = 0
# goal_pose.pose.orientation.y = 0
# goal_pose.pose.orientation.z = 0
# goal_pose.pose.orientation.w = 1
arm = MoveGroupCommander('arm')
arm.allow_replanning(True)
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.03)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_footprint'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
rospy.sleep(2)
start_pose = arm.get_current_pose(end_effector_link).pose
rospy.loginfo("End effector start pose: " + str(start_pose))
rospy.loginfo("Aruco pose: " + str(goal_pose))
waypoints = []
waypoints.append(start_pose)
# goal_pose.pose.orientation.x = -0.5
# goal_pose.pose.orientation.y = -0.5
# goal_pose.pose.orientation.z = -0.5
# goal_pose.pose.orientation.w = 1
# goal_pose.pose.orientation.y -= 0.1*(1.0/2.0)
waypoints.append(deepcopy(goal_pose.pose))
fraction = 0.0
maxtries = 100
attempts = 0
executed = 0
max_execute = 10
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(waypoints, 0.01, 0.0,
True)
attempts += 1
if attempts % 20 == 0:
if (attempts < 100):
rospy.loginfo("still trying after" + str(attempts) +
" attempts...")
else:
rospy.loginfo("Finished after " + str(attempts) +
" attempts...")
if fraction > 0.89:
rospy.loginfo("path compute successfully. Arm is moving.")
# print(plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1].positions)
# print(plan)
#for item in plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1]
if execute:
arm.execute(plan)
rospy.loginfo("path execution complete. ")
rospy.sleep(2)
break # to break infinite loop
if (attempts % 100 == 0 and fraction < 0.9):
rospy.loginfo("path planning failed with only " +
str(fraction * 100) + "% success after " +
str(maxtries) + " attempts")
return fraction
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置每次运动规划的时间限制：1s
arm.set_planning_time(1)
# 控制机械臂运动到之前设置的“home”姿态
arm.set_named_target('home')
arm.go()
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# --------------------- 第一段轨迹生成，关节空间插值 ---------------------#
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0, -1, 1, 0, -1, 0]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(0.5)
# 获取当前位姿数据为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# --------------------- 第一段轨迹生成，关节空间插值 ---------------------#
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.3米，向右运动0.3米
wpose = deepcopy(start_pose)
wpose.orientation.x = 0
wpose.orientation.y = 0.707106781186547
wpose.orientation.z = 0
wpose.orientation.w = 0.707106781186547
wpose.position.x -= 0.2
# wpose.position.y -= 0.4
wpose.position.z -= 1
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.orientation.x = 0
wpose.orientation.y = 0.707106781186547
wpose.orientation.z = 0
wpose.orientation.w = 0.707106781186547
# wpose.position.x -= 0.5
wpose.position.y -= 0.5
wpose.position.z += 0.2
# wpose.position.x += 0.15
# wpose.position.y += 0.1
# wpose.position.z -= 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
gain = 0.1
points = 5
for i in xrange(points):
start_pose = geometry_msgs.msg.Pose()
start_pose.orientation.w = waypoints[i - 1].orientation.w
start_pose.orientation.x = waypoints[i - 1].orientation.x
start_pose.orientation.y = waypoints[i - 1].orientation.y
start_pose.orientation.z = waypoints[i - 1].orientation.z
start_pose.position.y = waypoints[i - 1].position.y - 0.096416
start_pose.position.z = waypoints[i - 1].position.z + 0.2
start_pose.position.x = waypoints[i - 1].position.x - 0.01
waypoints.append(copy.deepcopy(start_pose))
(plan_waypoints, fraction) = right_arm.compute_cartesian_path(
waypoints, 0.01, 0.0 # waypoints to follow # eef_step
) # jump_threshold
print fraction * 100, "% planned"
print "============ Waiting while RVIZ displays come..."
rospy.sleep(5)
right_arm.execute(plan_waypoints)
print "============ Waiting while RVIZ execute..."
roscpp_shutdown()
class WarehousePlanner(object):
def __init__(self):
rospy.init_node('moveit_warehouse_planner', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(2)
group_id = rospy.get_param("~arm_group_name")
self.eef_step = rospy.get_param("~eef_step", 0.01)
self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
self.group = MoveGroupCommander(group_id)
# self._add_ground()
self._robot_name = self.robot._r.get_robot_name()
self._robot_link = self.group.get_end_effector_link()
self._robot_frame = self.group.get_pose_reference_frame()
self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9)
rospy.sleep(4)
rospy.loginfo("Waiting for warehouse services...")
rospy.wait_for_service('moveit_warehouse_services/list_robot_states')
rospy.wait_for_service('moveit_warehouse_services/get_robot_state')
rospy.wait_for_service('moveit_warehouse_services/has_robot_state')
rospy.wait_for_service('/compute_fk')
self._list_states = rospy.ServiceProxy(
'moveit_warehouse_services/list_robot_states', ListStates)
self._get_state = rospy.ServiceProxy(
'moveit_warehouse_services/get_robot_state', GetState)
self._has_state = rospy.ServiceProxy(
'moveit_warehouse_services/has_robot_state', HasState)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
rospy.loginfo("Service proxies connected")
self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list',
PlanTrajectoryFromList,
self._plan_from_list_cb)
self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix',
PlanTrajectoryFromPrefix,
self._plan_from_prefix_cb)
self._execute_tr_srv = rospy.Service('execute_planned_trajectory',
ExecutePlannedTrajectory,
self._execute_plan_cb)
self.__plan = None
def get_waypoint_names_by_prefix(self, prefix):
regex = "^" + str(prefix) + ".*"
waypoint_names = self._list_states(regex, self._robot_name).states
return waypoint_names
def get_pose_from_state(self, state):
header = Header()
fk_link_names = [self._robot_link]
header.frame_id = self._robot_frame
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0].pose
def get_cartesian_waypoints(self, waypoint_names):
waypoints = []
waypoints.append(self.group.get_current_pose().pose)
for name in waypoint_names:
if self._has_state(name, self._robot_name).exists:
robot_state = self._get_state(name, "").state
waypoints.append(self.get_pose_from_state(robot_state))
else:
rospy.logerr("Waypoint %s doesn't exist for robot %s.", name,
self._robot_name)
return waypoints
def _add_ground(self):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below the dome
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.01))
rospy.sleep(1)
def plan_from_filter(self, prefix):
waypoint_names = self.get_waypoint_names_by_prefix(prefix)
waypoint_names.sort()
if 0 == len(waypoint_names):
rospy.logerr(
"No waypoints found for robot %s with prefix %s. " +
"Can't make trajectory :(", self._robot_name, str(prefix))
return False
rospy.loginfo(
"Creating trajectory with %d waypoints selected by " +
"prefix %s.", len(waypoint_names), str(prefix))
return self.plan_from_list(waypoint_names)
def plan_from_list(self, waypoint_names):
self.group.clear_pose_targets()
waypoints = self.get_cartesian_waypoints(waypoint_names)
if len(waypoints) != len(waypoint_names) + 1:
# +1 because current position is used as first waypiont.
rospy.logerr("Not all waypoints existed, not executing.")
return False
(plan,
fraction) = self.group.compute_cartesian_path(waypoints,
self.eef_step,
self.jump_threshold)
if fraction < self._min_wp_fraction:
rospy.logerr(
"Only managed to generate trajectory through" +
"%f of waypoints. Not executing", fraction)
return False
self.__plan = plan
return True
def _plan_from_list_cb(self, request):
# check all exist
self.__plan = None
rospy.loginfo("Creating trajectory from points given: %s",
",".join(request.waypoint_names))
return self.plan_from_list(request.waypoint_names)
def _plan_from_prefix_cb(self, request):
self.__plan = None
rospy.loginfo("Creating trajectory from points filtered by prefix %s",
request.prefix)
return self.plan_from_filter(request.prefix)
def _execute_plan_cb(self, request):
if self.__plan is None:
rospy.logerr("No plan stored!")
return False
rospy.loginfo("Executing stored plan")
response = self.group.execute(self.__plan)
self.__plan = None
return response
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# arm.set_goal_position_tolerance(0.01)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
waypoints = list()
pose = PoseStamped().pose
# start with the current pose
waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose))
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
# first pose
pose.position.x = 0.35
pose.position.y = 0.25
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# second pose
pose.position.x = 0.25
pose.position.y = -0.3
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# third pose
pose.position.x += 0.15
waypoints.append(copy.deepcopy(pose))
# fourth pose
pose.position.y += 0.15
waypoints.append(copy.deepcopy(pose))
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
