def collision_free_plan(self,
joint_start,
joint_target,
initial_trajectory=None,
start_value=0):
'''
uses Moveit and OMPL to plan a path and generate the trajectory. The
trajectory is sent point by point to the robot. A final message is sent
to signify the end of the trajectory and to trigger the motion.
'''
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = self.group.get_joints()[:-1]
joint_state.position = joint_start
robot_state = RobotState()
robot_state.joint_state = joint_state
self.group.set_start_state(robot_state)
self.group.set_joint_value_target(joint_target)
if initial_trajectory is not None:
initial_trajectory.joint_trajectory.points = initial_trajectory.joint_trajectory.points[
start_value:]
self.initial_trajectory_proxy(
initial_trajectory.joint_trajectory, 1,
len(initial_trajectory.joint_trajectory.points) - 2)
else:
self.initial_trajectory_proxy(JointTrajectory(), -1, -1)
self.group.set_workspace([-3, -3, -3, 3, 3, 3])
plan = self.group.plan()
return plan
def getIkPose(self, pose, group="right_arm", previous_state=None):
"""Get IK of the pose specified, for the group specified, optionally using
the robot_state of previous_state (if not, current robot state will be requested) """
# point point to test if there is ik
# returns the answer of the service
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = group
rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
rqst.ik_request.pose_stamped.header.frame_id = 'base_link'
# Set point to check IK for
rqst.ik_request.pose_stamped.pose.position = pose.position
rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
if previous_state == None:
current_joint_state = rospy.wait_for_message(DEFAULT_JOINT_STATES, JointState)
cs = RobotState()
cs.joint_state = current_joint_state
#cs = self.r_commander.get_current_state() # old code
rqst.ik_request.robot_state = cs
else:
rqst.ik_request.robot_state = previous_state
ik_answer = GetPositionIKResponse()
if DEBUG_MODE:
timeStart = rospy.Time.now()
ik_answer = self.ik_serv.call(rqst)
if DEBUG_MODE:
durationCall= rospy.Time.now() - timeStart
rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
return ik_answer
def joint_to_robot_state(joint_state, robot_state=None):
"""
Converts joint state to robot state filling missing joints from the current
or given robot state.
@joint_state: JointState to convert
@robot_state: Optional RobotState that is used to fill missing values, if
None current state is used.
"""
# Get robot state if empty
if robot_state is None:
robot_state = get_robot_state()
# Convert joint state to dictionary
js_dict = dict(
(x, y) for x, y in zip(joint_state.name, joint_state.position))
# Fill joint_state values into robot_state
rs = RobotState()
for i, k in enumerate(robot_state.joint_state.name):
if k in js_dict:
rs.joint_state.name.append(k)
rs.joint_state.position.append(js_dict[k])
rs.joint_state.velocity.append(0)
else:
rs.joint_state.name.append(k)
rs.joint_state.position.append(robot_state.joint_state.position[i])
rs.joint_state.velocity.append(robot_state.joint_state.velocity[i])
rs.multi_dof_joint_state = robot_state.multi_dof_joint_state
rs.attached_collision_objects = robot_state.attached_collision_objects
return rs
def move_to_joint_position(self, joints):
"""
Adds the given joint values to the current joint values, moves to position
"""
joint_state = self.joint_state
joint_dict = dict(zip(joint_state.name, joint_state.position))
for i in range(len(JOINT_NAMES)):
joint_dict[JOINT_NAMES[i]] += joints[i]
joint_state = JointState()
joint_state.name = JOINT_NAMES
joint_goal = [joint_dict[joint] for joint in JOINT_NAMES]
joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX)
joint_state.position = joint_goal
header = Header()
robot_state = RobotState()
robot_state.joint_state = joint_state
link_names = ['ee_link']
position = self.get_fk_client(header, link_names, robot_state)
target_p = position[0].pose.position
x, y, z = target_p.x, target_p.y, target_p.z
conditions = [
x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL,
y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15
]
print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z))
for condition in conditions:
if not condition:
return
self.move_to_target(joint_goal)
rospy.sleep(0.15)
def get_box_plan(self, num, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.arm.set_joint_value_target(self.box_pose[num])
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a box plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state, plan)
def getIkPose(self, pose, group="right_arm", previous_state=None):
"""Get IK of the pose specified, for the group specified, optionally using
the robot_state of previous_state (if not, current robot state will be requested) """
# point point to test if there is ik
# returns the answer of the service
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = group
rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
rqst.ik_request.pose_stamped.header.frame_id = 'base_link'
# Set point to check IK for
rqst.ik_request.pose_stamped.pose.position = pose.position
rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
if previous_state == None:
current_joint_state = rospy.wait_for_message(
DEFAULT_JOINT_STATES, JointState)
cs = RobotState()
cs.joint_state = current_joint_state
#cs = self.r_commander.get_current_state() # old code
rqst.ik_request.robot_state = cs
else:
rqst.ik_request.robot_state = previous_state
ik_answer = GetPositionIKResponse()
if DEBUG_MODE:
timeStart = rospy.Time.now()
ik_answer = self.ik_serv.call(rqst)
if DEBUG_MODE:
durationCall = rospy.Time.now() - timeStart
rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
return ik_answer
def move_group_python_interface_tutorial(max_planning_time=100):
global robot
global scene
global group
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm_group")
group.clear_pose_targets()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
joint_state.position = [
-0.399785047899, 0.547865794198, -0.53408570763, -1.53819544195,
-0.400005105505, -4.7477817452
]
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
group.set_joint_value_target([
0.41523042671, 0.740434785089, -1.26737863948, -3.86136759133,
-0.658526198241, -2.53533941067
])
group.set_planning_time(max_planning_time)
plan2 = group.plan()
group.set_pose_targets()
moveit_commander.roscpp_shutdown()
def execute_planned_trajectory(self, plan):
#wpose = self.move_group.get_current_pose().pose
#self.move_group.set_pose_target(wpose)
#time.sleep(0.5)
#result=self.move_group.go(wait=True)
#self.move_group.stop()
#print("plan:")
#print(plan)
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
print(message)
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
#print(joint_state)
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#add check to see if plan start state agrees with start state or consider adding back in getting current pose and appending it to waypoints potentially
result = self.move_group.execute(plan, wait=True)
#print(result)
if (not result):
print("not executed")
wpose = self.move_group.get_current_pose().pose
#print(wpose)
wpose.position.z += 0.1
self.move_group.set_pose_target(wpose)
result = self.move_group.go(wait=True)
plan, fraction, waypoints = self.plan_robot_motion(
self.dxf_points, self.sines)
self.execute_planned_trajectory(plan)
def js_cb(self, a):
rospy.loginfo('Received array')
js = JointState()
js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
jList = a.data
jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
i = 0
for pos in jMatrix:
rospy.loginfo(pos)
js.position = pos
gsvr = GetStateValidityRequest()
rs = RobotState()
rs.joint_state = js
gsvr.robot_state = rs
gsvr.group_name = "both_arms"
resp = self.coll_client(gsvr)
if (resp.valid == False):
rospy.loginfo('false')
rospy.loginfo(i)
self.js_pub.publish(0)
return
i = i + 1
self.js_pub.publish(1)
rospy.loginfo('true')
return
def __init__(self, name, hand_or_arm="both", hand_h=False):
self.__save = rospy.ServiceProxy('save_robot_state', SaveState)
self.__name = name
self.__hand_h = hand_h
if hand_or_arm == "arm":
self.__commander = SrArmCommander()
elif hand_or_arm == 'hand':
if self.__hand_h:
self.__commander = SrRobotCommander("hand_h", prefix="H1_")
else:
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self.__commander = SrHandCommander(
hand_parameters=hand_parameters, hand_serial=hand_serial)
else:
self.__arm_commander = SrArmCommander()
if self.__hand_h:
self.__hand_commander = SrRobotCommander("hand_h",
prefix="H1_")
else:
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self.__hand_commander = SrHandCommander(
hand_parameters=hand_parameters, hand_serial=hand_serial)
self.__hand_or_arm = hand_or_arm
rs = RobotState()
current_dict = {}
if self.__hand_or_arm == "both":
current_dict = self.__arm_commander.get_robot_state_bounded()
robot_name = self.__arm_commander.get_robot_name()
elif self.__hand_or_arm == "arm":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
elif self.__hand_or_arm == "hand":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
else:
rospy.logfatal("Unknown save type")
exit(-1)
rospy.loginfo(current_dict)
rs.joint_state = JointState()
rs.joint_state.name = current_dict.keys()
rs.joint_state.position = current_dict.values()
self.__save(self.__name, robot_name, rs)
def publish_robot_state(robot_state_publisher,
state,
state_validity_checker=None,
duration=1.0,
display_status='all'):
robot_state = RobotState()
robot_state.joint_state = convertStateToJointState(
state) # convert this to a way-point first
# robot_state.is_diff = False
robot_state_msg = DisplayRobotState()
robot_state_msg.state = robot_state
if display_status != 'all':
result = state_validity_checker.getStateValidity(
robot_state, group_name='both_arms_torso', constraints=None)
# print("The result for validity check is: %s" % result)
if display_status == 'valid' and result.valid or (
display_status == 'inValid' and not result.valid):
robot_state_publisher.publish(robot_state_msg)
time.sleep(duration)
if not result.valid:
print("The result for validity check is: %s" % result)
return True
else:
robot_state_publisher.publish(robot_state_msg)
time.sleep(duration)
return True
return False
def move_arm():
# plan to a random location
group = moveit_commander.MoveGroupCommander("arm")
robot = RobotCommander()
a = robot.arm
a.set_start_state(RobotState())
p = a.plan([0, 0])
print "Solution:"
print p
a.execute(p)
rospy.sleep(10)
a.set_start_state(RobotState())
p = a.plan([1.57, -1.57])
print "Solution:"
print p
a.execute(p)
rospy.sleep(10)
a.set_start_state(RobotState())
p = a.plan([3.14, 3.14])
print "Solution:"
print p
a.execute(p)
rospy.sleep(10)
moveit_commander.roscpp_shutdown()
print "Finishing"
def save_state(self, current_dict, robot_name):
rs = RobotState()
rospy.loginfo(current_dict)
rs.joint_state = JointState()
rs.joint_state.name = current_dict.keys()
rs.joint_state.position = current_dict.values()
rospy.logwarn(rs)
self._save(self._name, robot_name, rs)
def plan_robot_motion(self, dxf_points, sines):
self.dxf_points = dxf_points
self.sines = sines
waypoints = []
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#wpose = self.move_group.get_current_pose().pose
#waypoints.append(wpose)
#state = self.robot.get_current_state()
#self.move_group.set_start_state(state)
for i in range(len(dxf_points)):
pose_goal = Pose()
rpy = [math.pi, 0.0, sines[i]]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - dxf_points[i][0]) * 0.0254
y_val = dxf_points[i][1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
print(pose_goal)
waypoints.append(pose_goal)
replan_value = 0
error_code = None
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
while (replan_value < 3 and fraction < 1.0):
print(fraction)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
replan_value += 1
print("WARNING Portion of plan failed, replanning")
return plan, fraction, waypoints
def __init__(self, name, hand_or_arm="both"):
self.__save = rospy.ServiceProxy(
'save_robot_state', SaveState)
self.__name = name
if name is None or name == '':
raise ValueError("Cannot save with empty name.")
if hand_or_arm == "arm":
self.__commander = SrArmCommander()
if not self.__commander.arm_found():
raise ValueError("'No arm found.'")
elif hand_or_arm == 'hand':
self.__commander = SrHandCommander()
else:
double_error = []
try:
self.__hand_commander = SrHandCommander()
except Exception as e:
double_error.append(str(e))
self.__arm_commander = SrArmCommander()
if not self.__arm_commander.arm_found():
double_error.append("'No arm found.'")
if len(double_error) != 0:
raise ValueError(" ".join(double_error))
self.__hand_or_arm = hand_or_arm
rs = RobotState()
current_dict = {}
if self.__hand_or_arm == "both":
current_dict = self.__arm_commander.get_robot_state_bounded()
robot_name = self.__arm_commander.get_robot_name()
elif self.__hand_or_arm == "arm":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
elif self.__hand_or_arm == "hand":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
else:
rospy.logfatal("Unknown save type")
exit(-1)
rospy.loginfo(current_dict)
rs.joint_state = JointState()
rs.joint_state.name = current_dict.keys()
rs.joint_state.position = current_dict.values()
self.__save(self.__name, robot_name, rs)
def get_cartesian_plan(self, trans, z_offset, start_state, grasp):
# set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# set waypoints
waypoints = []
self.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
wpose = Pose()
wpose.position = copy.deepcopy(self.target_pose.position)
wpose.orientation = copy.deepcopy(self.target_pose.orientation)
wpose.position.z = copy.deepcopy(self.target_pose.position.z +
z_offset)
waypoints.append(wpose)
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a cartesian plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state)
def __init__(self):
self.group = MoveGroupInterface("arm", "base_link", plan_only=True)
self.gripper = MoveGroupInterface("gripper",
"base_link",
plan_only=True)
self.virtual_arm_state = RobotState()
self.virtual_gripper_state = RobotState()
# Sleep to allow RVIZ time to start up.
print "============ Waiting for RVIZ..."
rospy.sleep(5)
print "============ Starting planning"
def plan_trajectory(move_group, destination_pose, start_joint_angles):
current_joint_state = JointState()
current_joint_state.name = joint_names
current_joint_state.position = start_joint_angles
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = current_joint_state
move_group.set_start_state(moveit_robot_state)
move_group.set_pose_target(destination_pose)
return planCompat(move_group.plan())
def set_global_defaults( self, group ):
"""
Initializes the proxy with default parameters to use with connections
@param group : string move group name
"""
# Default planning parameters for new connections
num_planning_attempts=5
allowed_planning_time=10.0
plan_only=True
planner_id='RRTConnectkConfigDefault'
# Set the default plan request data
plan_request = MotionPlanRequest()
plan_request.start_state.is_diff = True # Flags start state as empty to use current state on server
plan_request.num_planning_attempts = num_planning_attempts
plan_request.allowed_planning_time = allowed_planning_time
plan_request.planner_id = planner_id
plan_request.group_name = group
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._motion_plan_requests[group] = plan_request
ProxyMoveItClient._default_motion_plan_requests[group] = copy.deepcopy(plan_request)
planning_options = PlanningOptions()
planning_options.plan_only = plan_only
planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._planning_options[group] = planning_options
ProxyMoveItClient._default_planning_options[group] = copy.deepcopy(planning_options)
# Set up the default joint constraints
joint_constraints = {}
for name in ProxyMoveItClient._joint_names[group]:
joint_constraints[name] = JointConstraint(joint_name=name, tolerance_above=0.05, tolerance_below=0.05,weight=1.0)
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._joint_constraints[group] = joint_constraints
ProxyMoveItClient._default_joint_constraints[group] = copy.deepcopy(joint_constraints)
# @TODO - add real constraints
ProxyMoveItClient._position_constraints[group] = []
ProxyMoveItClient._default_position_constraints[group] = []
ProxyMoveItClient._orientation_constraints[group] = []
ProxyMoveItClient._default_orientation_constraints[group] = []
ProxyMoveItClient._visibility_constraints[group] = []
ProxyMoveItClient._default_visibility_constraints[group] = []
ProxyMoveItClient._robot_states[group] = RobotState()
ProxyMoveItClient._default_robot_states[group] = RobotState()
def srvPlanMovementCallback(data):
group_name = data.group
if group_name in groups_names:
if data.rand_target:
target_position = groups[group_name].get_random_joint_values()
else:
target_position = data.target_pos
try:
groups[group_name].set_joint_value_target(target_position)
except:
rospy.logwarn(rospy.get_caller_id() +
' Incorrect target positions')
return PlanMovementResponse(3, [], [], [])
rospy.loginfo(rospy.get_caller_id() + " Moving group '" + group_name +
"' to " + str(data.target_pos))
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.header.stamp.secs += 1
joint_state.name = groups[group_name].get_active_joints()
if data.rand_start:
start_position = groups[group_name].get_random_joint_values()
else:
if data.current_start:
start_position = groups[group_name].get_current_joint_values()
else:
start_position = data.start_pos
joint_state.position = start_position
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
groups[group_name].set_start_state(moveit_robot_state)
plan = groups[group_name].plan()
if len(plan.joint_trajectory.points) > 0:
rospy.loginfo(rospy.get_caller_id() + ' Correct plan')
if data.execute:
groups[group_name].execute(plan, wait=data.wait)
if data.ret_plan:
traj = planToNumpy(plan, data.ret_fps)
plans = []
for joint in traj['data']:
plans.append(Trajectory(joint, traj['data'][joint]))
return PlanMovementResponse(0, start_position, target_position,
plans)
else:
return PlanMovementResponse(0, [], [], [])
else:
rospy.logwarn(rospy.get_caller_id() + ' Could not plan')
return PlanMovementResponse(2, [], [], [])
else:
rospy.logwarn(rospy.get_caller_id() + ' Incorrect group name: ' +
group_name)
return PlanMovementResponse(1, [], [], [])
def compute_fk_client():
try:
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.prefix + '/base'
rs = RobotState()
rs.joint_state = joint_state
res = self.compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of troubles return 0 pose stamped
return []
def compute_fk_client():
try:
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.prefix + '/base'
rs = RobotState()
rs.joint_state = joint_state
res = self.compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# in case of troubles return 0 pose stamped
return []
def _make_request_msg(root_link, tip_link, target_pose, robot_object, joints):
"""
This method generates an ik request message for the kdl_ik_service. The message is
of the type moveit_msgs/PositionIKRequest and contains all information
contained in the parameter.
:param root_link: The first link of the chain of joints to be altered
:param tip_link: The last link of the chain of joints to be altered
:param target_pose: A list of two lists, the first is the pose in world coordinate frame
the second is the orientation as quanternion in world coordinate frame
:param robot_object: The robot for which the ik should be generated
:param joints: A list of joint names between the root_link and tip_link that should be altered.
:return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter
"""
pose_sta = PoseStamped()
pose_sta.header.frame_id = root_link
pose_sta.header.stamp = rospy.Time.now()
tar_pose = target_pose[0]
tar_rotation = target_pose[1]
pose = Pose()
pose.position.x = tar_pose[0]
pose.position.y = tar_pose[1]
pose.position.z = tar_pose[2]
pose.orientation.x = tar_rotation[0]
pose.orientation.y = tar_rotation[1]
pose.orientation.z = tar_rotation[2]
pose.orientation.w = tar_rotation[3]
pose_sta.pose = pose
robot_state = RobotState()
joint_state = JointState()
names, poses = _get_position_for_all_joints(robot_object)
joint_state.name = joints
joint_state.position = _get_position_for_joints(robot_object, joints)
joint_state.velocity = [0.0 for x in range(len(joints))]
joint_state.effort = [0.0 for x in range(len(joints))]
#joint_state.name = names
#joint_state.position = poses
robot_state.joint_state = joint_state
msg_request = PositionIKRequest()
#msg_request.group_name = "arm"
msg_request.ik_link_name = tip_link
msg_request.pose_stamped = pose_sta
msg_request.avoid_collisions = False
msg_request.robot_state = robot_state
#msg_request.timeout = rospy.Duration(secs=10000)
#msg_request.attempts = 1000
return msg_request
def talker():
display_robot_state_publisher = rospy.Publisher(
'/move_group/fake_controller_joint_states', JointState, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10)
print "============ Starting setup"
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("arm")
print "============ Robot Groups: %s" % robot.get_group_names()
print "============ End effector: %s" % group.get_end_effector_link()
active_joints = group.get_active_joints()
print "============ Active joints: ", active_joints
joints_values_str = group.get_current_joint_values()
print "============ Joint values: ", joints_values_str
group.clear_pose_targets()
print "============ Set start state..."
i = 0
while i != len(active_joints):
a = float(input("Vvedite znachenie %s: " % active_joints[i]))
joints_values_str[i] = a
i = i + 1
start_joint_state = JointState()
start_joint_state.header.stamp = rospy.Time.now()
start_joint_state.name = active_joints
start_joint_state.position = joints_values_str
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = start_joint_state
group.set_start_state(moveit_robot_state)
#group.set_start_state_to_current_state()
display_robot_state_publisher.publish(start_joint_state)
print "============ Display start state..."
while not rospy.is_shutdown():
#display_robot_state = moveit_msgs.msg.DisplayRobotState()
#display_robot_state.state = moveit_robot_state
#Object_color = ObjectColor()
#Object_color.id = active_joints
#Object_color.color = ColorRGBA(250, 118, 0, 1)
#display_robot_state.highlight_links = [250, 118, 0, 1]
#display_robot_state_publisher.publish(start_joint_state)
#group.set_start_state(moveit_robot_state)
rate.sleep()
def _convertRobotState(self, context, jointStates):
si = context.getSceneInformation()
robotInfo = si.getRobotInfo()
config = robotInfo.configuration
robotStateMsg = RobotState()
jointStateMsg = JointState(
header=Header(stamp=rospy.Time.now(), frame_id='/world'))
for jointName in jointStates:
jointStateMsg.name.append(jointName)
jointStateMsg.position.append(config[jointName])
# jointStateMsg.velocity.append(0.0)
# jointStateMsg.effort.append(0.0)
robotStateMsg.joint_state = jointStateMsg
return robotStateMsg
def joint_state_to_robot_state(self, joint_state):
"""
Create a RobotState message from a JointState message or a dictionary mapping joint names to their values
@param joint_state: JointState message or dictionary with the following format {'joint_name': joint_value}
"""
if isinstance(joint_state, dict):
joint_state_message = JointState()
joint_state_message.name = joint_state.keys()
joint_state_message.position = joint_state.values()
else:
joint_state_message = joint_state
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state_message
return moveit_robot_state
def set_start_state(self, group_name, joint_names, joint_positions):
group = self.get_group(group_name)
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = joint_names
joint_state.position = joint_positions
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
def enforce_bounds(self, robot_state_msg):
""" Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """
s = RobotState()
c_str = self._g.enforce_bounds(
conversions.msg_to_string(robot_state_msg))
conversions.msg_from_string(s, c_str)
return s
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def checkTrajectoryValidity(self, robot_trajectory, groups=[]):
"""Given a robot trajectory, deduce it's groups and check it's validity on each point of the traj
returns True if valid, False otherwise
It's considered not valid if any point is not valid"""
#r = RobotTrajectory()
init_time = time.time()
if len(groups) > 0:
groups_to_check = groups
else:
groups_to_check = [
'both_arms_torso'
] # Automagic group deduction... giving a group that includes everything
for traj_point in robot_trajectory.joint_trajectory.points:
rs = RobotState()
rs.joint_state.name = robot_trajectory.joint_trajectory.joint_names
rs.joint_state.position = traj_point.positions
for group in groups_to_check:
result = self.sv.getStateValidity(rs, group) #, constraints)
if not result.valid: # if one point is not valid, the trajectory is not valid
rospy.logerr(
"Trajectory is not valid at point (RobotState):" +
str(rs) + "with result of StateValidity: " +
str(result))
rospy.logerr(
"published in /robot_collision_state the conflicting state"
)
drs = DisplayRobotState()
drs.state = rs
self.robot_state_collision_pub.publish(drs)
return False
fin_time = time.time()
rospy.logwarn("Trajectory validity of " +
str(len(robot_trajectory.joint_trajectory.points)) +
" points took " + str(fin_time - init_time))
return True
def get_forward_kinematic(joints):
try:
rospy.wait_for_service('compute_fk', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Service call failed:", e)
return None
try:
moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
fk_link = ['base_link', 'tool_link']
joint_names = ['joint_1','joint_2','joint_3','joint_4','joint_5','joint_6']
header = Header(0,rospy.Time.now(),"/world")
rs = RobotState()
rs.joint_state.name = joint_names
rs.joint_state.position = joints
response = moveit_fk(header, fk_link, rs)
except rospy.ServiceException as e:
rospy.logerr("Service call failed:", e)
return(None)
quaternion=[response.pose_stamped[1].pose.orientation.x, response.pose_stamped[1].pose.orientation.y,
response.pose_stamped[1].pose.orientation.z, response.pose_stamped[1].pose.orientation.w]
rpy = get_rpy_from_quaternion(quaternion)
quaternion = Position.Quaternion(round(quaternion[0],3), round(quaternion[1],3), round(quaternion[2],3),
round(quaternion[3],3))
point = Position.Point(round(response.pose_stamped[1].pose.position.x,3), round(response.pose_stamped[1].pose.position.y,3),
round(response.pose_stamped[1].pose.position.z,3))
rpy=Position.RPY(round(rpy[0],3),round(rpy[1],3),round(rpy[2],3))
rospy.loginfo("kinematic forward has been calculated ")
return(point, rpy, quaternion)
def test_get_current_state(self):
expected_state = RobotState()
expected_state.joint_state.header.frame_id = "base_link"
expected_state.multi_dof_joint_state.header.frame_id = "base_link"
expected_state.joint_state.name = self.JOINT_NAMES
expected_state.joint_state.position = [0] * 6
self.assertEqual(self.group.get_current_state(), expected_state)
def plan_and_execute(self, userdata):
### DEBUG PERCEPTION ONLY
#sss.wait_for_input()
#userdata.new_box = True
#return True
### END DEBUG
# print "The pose reference frame is",self.mgc.get_pose_reference_frame()
# self.mgc.set_pose_reference_frame("camera_rgb_optical_frame")
# print "The pose reference frame is",self.mgc.get_pose_reference_frame()
start_plan = rospy.Time.now()
### Set next (virtual) start state
start_state = RobotState()
(pre_grasp_config, error_code) = sss.compose_trajectory("arm","pre_grasp")
if error_code != 0:
rospy.logerr("unable to parse pre_grasp configuration")
return False
start_state.joint_state.name = pre_grasp_config.joint_names
start_state.joint_state.position = pre_grasp_config.points[0].positions
start_state.is_diff = True
# self.mgc.set_start_state(start_state)
self.mgc.set_start_state_to_current_state()
### Plan Approach
approach_pose_offset = PoseStamped()
approach_pose_offset.header.frame_id = "current_box"
approach_pose_offset.header.stamp = rospy.Time(0)
# approach_pose_offset.pose.position.x -= 0.1
approach_pose_offset.pose.position.z += 0.05
#quat = tf.transformations.quaternion_from_euler(0, 1.57, 1.57)
#approach_pose_offset.pose.orientation.x = quat[0]
#approach_pose_offset.pose.orientation.y = quat[1]
#approach_pose_offset.pose.orientation.z = quat[2]
#approach_pose_offset.pose.orientation.w = quat[3]
try:
approach_pose = self.listener.transformPose("table", approach_pose_offset)
approach_pose.pose.position.x += -0.01
# approach_pose = self.listener.transformPose("camera_rgb_optical_frame", approach_pose_offset)
except Exception, e:
rospy.logerr("could not transform pose. Exception: %s", str(e))
return False
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None):
solutions = []
for pose_num, eef_pose in enumerate(eef_poses):
if eef_pose.header.frame_id.strip('/') != self._world.strip('/'):
raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world))
pose = pose_to_list(eef_pose)
resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1],
[seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)]
for joint in self.joint_names()] if len(seeds)>0 else None)
if resp is None:
rs = None
else:
rs = RobotState()
rs.is_diff = False
rs.joint_state.name = self.joint_names()
rs.joint_state.position = resp
solutions.append(rs)
return solutions
def __save_out(self):
rs = RobotState()
current_dict = {}
if self.__hand_or_arm == "both":
current_dict = self.__arm_commander.get_robot_state_bounded()
robot_name = self.__arm_commander.get_robot_name()
elif self.__hand_or_arm == "arm":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
elif self.__hand_or_arm == "hand":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
else:
rospy.logfatal("Unknown save type")
exit(-1)
rospy.loginfo(current_dict)
rs.joint_state = JointState()
rs.joint_state.name = current_dict.keys()
rs.joint_state.position = current_dict.values()
self.__save(self.__name, robot_name, rs)
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
msg = JointState()
msgDisplayRobotState = DisplayRobotState()
msgRobotState = RobotState()
# Publish Joint States
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = (len(self.free_joints.items()) +
len(self.dependent_joints.items()))
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.free_joints:
joint = self.free_joints[name]
factor = 1
offset = 0
# Add Dependent Joint
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
if has_position and 'position' in joint:
pos = self.joint_positions[ name ]
msg.position[i] = pos
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
msgRobotState.joint_state = msg
msgDisplayRobotState.state = msgRobotState
self.pub.publish( msgDisplayRobotState )
r.sleep()
rospy.loginfo("Getting a current joint states message")
current_joint_states = rospy.wait_for_message('/joint_states', JointState)
current_joint_states_modif = copy.deepcopy(current_joint_states)
current_joint_states_modif.position = [2.0] * len(current_joint_states_modif.position)
rs_pub = rospy.Publisher('fk_test_robot_state', DisplayRobotState)
rospy.sleep(0.3) # let it initialize...
ik = InverseKinematics()
correct_js = copy.deepcopy(current_joint_states)
result = fk.getFK('arm_right_7_link', current_joint_states.name, correct_js.position, 'base_link')
rospy.loginfo("Result of current robot pose FK is: " + str(result))
rs = RobotState()
rs.joint_state = correct_js
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published current robot state")
rospy.sleep(2.0)
c = Constraints()
jc = JointConstraint()
jc.joint_name = 'arm_right_1_link'
jc.position = 0.0
jc.tolerance_above = 0.00001
jc.tolerance_below = 0.00001
jc.weight = 1.0
c.joint_constraints.append(jc)
def get_current_state(self):
""" Get a RobotState message describing the current state of the robot"""
s = RobotState()
s.deserialize(self._r.get_current_state())
return s
for i in range(10):
rospy.loginfo("Random pose: " + str(i))
random_js = copy.deepcopy(current_joint_states)
positions_list = []
for id, item in enumerate(current_joint_states_modif.position):
positions_list.append(random.random() * 3.14)
random_js.position = positions_list
# c = Constraints()
# jc = JointConstraint()
# jc.joint_name = 'arm_right_1_link'
# c.joint_constraints.append()
result = fk.getFK('arm_right_7_link', current_joint_states.name, random_js.position, 'base_link')
rospy.loginfo("Result of a disastrous robot pose is: " + str(result))
rs = RobotState()
rs.joint_state = random_js
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published robot state")
rospy.loginfo("Asking for IK with collision avoidance for the pose given")
print "Result is:"
print result
print "And result.pose_stamped is:"
print result.pose_stamped
print "with result.pose_stamped[0]"
print result.pose_stamped[0]
resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], True, 0, rs)
rospy.loginfo(str(resultik))
rospy.sleep(1.0)
