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 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 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 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 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 __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 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 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 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 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 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 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 _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 _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 cartesian_move(group, waypoints, rate):
ready_state = RobotState()
joint_state = JointState()
joint_state.name = group.get_active_joints()
joint_state.position = group.get_current_joint_values()
ready_state.joint_state = joint_state
# compute a path consised of straight line segments from a series of waypoints
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.0001, 0)
print(len(plan.joint_trajectory.points))
print('path fraction: {}'.format(fraction))
# arg ref_state_in is the start state of robot
new_plan = group.retime_trajectory(ready_state, plan, 0.005)
group.execute(new_plan)
group.stop()
def get_plan(self, trans, z_offset, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z + z_offset
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
self.arm.set_pose_target(self.target_pose)
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
print("---------debug--------{}".format(
len(plan.joint_trajectory.points)))
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state)
def execute(self):
rospy.loginfo("Start Task")
# Get latest task plan
plan = self.task_q[0]
for points in plan.trajectory.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
scaled_vel = []
scaled_acc = []
for vel in points.velocities:
scaled_vel.append(vel * self.exe_speed_rate)
points.velocities = tuple(scaled_vel)
for acc in points.accelerations:
scaled_acc.append(acc * self.exe_speed_rate *
self.exe_speed_rate)
points.accelerations = tuple(scaled_acc)
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.trajectory.joint_trajectory.joint_names[:]
start_state.position = plan.trajectory.joint_trajectory.points[
-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "vs087"
pub_display_msg.trajectory.append(plan.trajectory)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = ExecutePlanAction().action_goal.goal
goal.planning_time = plan.planning_time
goal.start_state = plan.start_state
# goal.start_state.joint_state.header.stamp = rospy.Time.now()
goal.trajectory = plan.trajectory
# rospy.logwarn(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
def get_plan(end, start=None, max_planning_time=100):
global group
if not start:
start = group.get_current_joint_values()
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 = start
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
group.set_joint_value_target(end)
group.set_planning_time(max_planning_time)
return group.plan()
def get_robot_state(self):
# pose = pose_from_trans(self.get_world_pose(BASE_FRAME)) # TODO: could get this transform directly
# transform = Transform(Vector3(*point_from_pose(pose)), Quaternion(*quat_from_pose(pose)))
# transform = self.get_transform()
# if transform is None:
# return None
state = RobotState()
state.joint_state = self.joint_state
# state.multi_dof_joint_state.header.frame_id = '/base_footprint'
# state.multi_dof_joint_state.header.stamp = rospy.Time(0)
# state.multi_dof_joint_state.joints = ['world_joint']
# state.multi_dof_joint_state.transforms = [transform]
# 'world_joint'
# http://cram-system.org/tutorials/intermediate/moveit
state.attached_collision_objects = []
state.is_diff = False
# rostopic info /joint_states
return state
def plan_cartesian_path(self, max_tries, waypoints, start_joints): # Call the planner
move_group = self.move_group
#------------------------------------------------------------------------------------------------------------
#- inforamtions on move_group http://docs.ros.org/jade/api/moveit_commander/html/move__group_8py_source.html
#- http://docs.ros.org/melodic/api/moveit_msgs/html/index-msg.html
#------------------------------------------------------------------------------------------------------------
#-- Check for robot stability
if(n.get_learning_mode()):
print "Make sure that Niryo is not in learning mode and in a safe position close to where you want to print"
raw_input("Then press enter..")
#-- Saving start state
tab_joints = [start_joints[0], start_joints[1],start_joints[2],start_joints[3],start_joints[4],start_joints[5]]
#-- Sending start state
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 = tab_joints
initial_state = RbState()
initial_state.joint_state = joint_state
move_group.set_start_state(initial_state)
#-- Parameters
fraction = 0.0
tries = 0
max_tries = max_tries #maximum tries allowed
eef_step = 1.0 #eef_step at 1.0 considering gcode is already an interpolation
velocity = 0.06 #velocity scaling factor applied to max velocity
#print "\n --- Computing parameters ---"
#print "| Max tries authorized : %2d \n| Eef step : %.4f \n| Velocity :
#%3d %%" %(max_tries,eef_step,velocity*100)
#print " ------------------------------\n"
#-- Call cartesian service
try:
moveit_cartesian_path = rospy.ServiceProxy('compute_cartesian_path', GetCartesianPath)
except rospy.ServiceException,e:
print("Service call failed:",e)
return(None)
def check_config(config, state_validity_checker, limb_name):
"""
Check the validity of a configuration
:param : The configurations to check against.
:param state_validity_checker: The validity checker
:param space: The space to check for
:param limb_name: The name of the limb.
:return: The validty of the configurations.
"""
# Construct a robotstate object from a dictionary
rs = RobotState()
js = JointState()
js.name = get_joint_names('right')
js.position = [config[joint] for joint in js.name]
rs.joint_state = js
result = state_validity_checker.getStateValidity(rs,
group_name=limb_name +
'_arm')
return result.valid
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)
plan = move_group.go(wait=True)
if not plan:
exception_str = """
Trajectory could not be planned for a destination of {} with starting joint angles {}.
Please make sure target and destination are reachable by the robot.
""".format(destination_pose, destination_pose)
raise Exception(exception_str)
return move_group.plan()
def srvOffsetMovementCallback(data):
group_name = data.group
if group_name in groups_names:
start_position = groups[group_name].get_current_joint_values()
target_position = [0] * len(start_position)
offset_position = data.offset
for i in range(len(start_position)):
target_position[i] = start_position[i] + offset_position[i]
try:
groups[group_name].set_joint_value_target(target_position)
except:
rospy.logwarn(rospy.get_caller_id() +
' Incorrect target positions')
return OffsetMovementResponse(3, start_position, target_position)
rospy.loginfo(rospy.get_caller_id() + " Moving group '" + group_name +
"' to " + str(target_position))
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()
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)
return OffsetMovementResponse(0, start_position, target_position)
else:
rospy.logwarn(rospy.get_caller_id() + ' Could not plan')
return OffsetMovementResponse(2, start_position, target_position)
else:
rospy.logwarn(rospy.get_caller_id() + ' Incorrect group name: ' +
group_name)
return OffsetMovementResponse(1, [], [])
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()
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)
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)
