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 get_current_state(self, list_joint_names=[]):
"""
Returns the current RobotState describing all joint states
:param list_joint_names: If not empty, returns only the state of the requested joints
:return: a RobotState corresponding to the current state read on /robot/joint_states
"""
if len(list_joint_names) == 0:
list_joint_names = self.joint_names()
state = RobotState()
state.joint_state.name = list_joint_names
state.joint_state.position = map(self.joint_angle, list_joint_names)
state.joint_state.velocity = map(self.joint_velocity, list_joint_names)
state.joint_state.effort = map(self.joint_effort, list_joint_names)
return state
def get_fk(self, tooltip, positions):
header = Header(0, rospy.Time.now(), 'world')
robot_state = RobotState()
robot_state.joint_state.name = self._active_joitns
robot_state.joint_state.position = positions
response = self.__call_service(
'compute_fk', {
'header': header,
'fk_link_names': [tooltip],
'robot_state': robot_state
})
if len(response.pose_stamped) == 0:
return None
return response.pose_stamped[0].pose
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 = [
"joint_1",
"joint_2",
"joint_3",
"joint_4",
"joint_5",
"joint_6",
]
expected_state.joint_state.position = [0] * 6
self.assertEqual(self.group.get_current_state(), expected_state)
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 "============ Current joint values: ", joints_values_str
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.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()
start_joint_state.header.stamp = rospy.Time.now()
display_robot_state_publisher.publish(start_joint_state)
print "============ Display start state..."
print "============ Start state: ", joints_values_str
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]
#group.set_start_state(moveit_robot_state)
rate.sleep()
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 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 computeTraj(self, moveGroup, start, goal, timeout=8.0):
startState = RobotState()
keyValuePairs = start.items()
startState.joint_state.name = map(lambda x: x[0], keyValuePairs)
startState.joint_state.position = map(lambda x: x[1], keyValuePairs)
try:
moveGroup.set_start_state(startState)
moveGroup.set_joint_value_target(goal)
moveGroup.set_planning_time(timeout)
moveGroup.set_planner_id('RRTStarkConfigDefault')
moveit_traj = moveGroup.plan()
return (len(moveit_traj.joint_trajectory.points) > 0, moveit_traj)
except moveit_commander.MoveItCommanderException as err:
rospy.logwarn('MoveIt planning failed: ' + str(err))
return (False, None)
def __call__(self, world, state, actor, prev_state=None):
js = JointState(
name=actor.joints,
position=state.q,
)
rs = RobotState(joint_state=js)
# TODO(cpaxton): this is about 75% of the current speed. Commenting
# this out makes a huge difference. We should be able to drastically
# reduce it by adding a C++ interface to run the check.
res = self.srv(robot_state=rs)
if not res.valid:
print "[ValidStateCondition] Invalid state!"
print res
return res.valid
def execute(self):
# Get latest task plan
plan = self.task_q[0].trajectory
for points in plan.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
self.grasp_[0] = self.task_q[0].grasp
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
def compute_forward_kinematics(self,joint_values,goal_link):
'''
compute the forward kinematics of the given joint values with reference to the reference link, return a posestamped
'''
fk_request=GetPositionFKRequest()
links=self.robot.get_link_names()
fk_request.fk_link_names=links
state=RobotState()
joint_names=['wx_agv2_1','agv2_virx','agv2_viry','agv2_virz','joint_a1','joint_a2','joint_a3','joint_a4','joint_a5','joint_a6','joint_a7']
state.joint_state.name=joint_names
state.joint_state.position=joint_values
fk_request.robot_state=state
fk_response=self.compute_fk(fk_request)
index=fk_response.fk_link_names.index(goal_link)
end_effector_pose=fk_response.pose_stamped[index]
return end_effector_pose
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 load_pose_2(pose):
p = RobotState()
p.multi_dof_joint_state.header = Header()
js = []
for i in range(1,7):
js.append('robocol_joint'+str(i))
p.multi_dof_joint_state.joint_names = js
# p.multi_dof_joint_state[0].transforms.translations.x = 0.0
# p.multi_dof_joint_state.transforms = [0.0,0.0,0.0]
v1 = Vector3(1, 3, 5)
# print(len(p.multi_dof_joint_state.transforms))
# p.multi_dof_joint_state.transforms = [0.0,0.0,0.0]
# p.multi_dof_joint_state = [0.0,0.0,0.0]
# p.joint_state.velocity = [0.0]
# p.joint_state.effort = [0.0]
return p
def compute_f_k(self, compute_fk, joint_values):
'''
compute the forward kinematics of the given joint values,the given joint values should be an array of (1,6),this function returns the fk response with the type of PoseStamped
'''
fk_request = GetPositionFKRequest()
links = self.move_group_link_names
fk_request.fk_link_names = links
state = RobotState()
joint_names = self.active_joints
state.joint_state.name = joint_names
state.joint_state.position = joint_values
fk_request.robot_state = state
fk_response = compute_fk(fk_request)
end_effector_pose = fk_response.pose_stamped[
len(fk_response.pose_stamped) - 1]
return end_effector_pose
def get_plan(self, trans, z_offset, start_state, grasp):
# Set argument start state
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
self.arm.set_start_state(moveit_start_state)
# Calculate goal pose
self.target_pose.position.x = trans.transform.translation.x
self.target_pose.position.y = trans.transform.translation.y
self.target_pose.position.z = trans.transform.translation.z + z_offset
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q)
pitch += pi / 2.0
tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
self.target_pose.orientation.x = tar_q[0]
self.target_pose.orientation.y = tar_q[1]
self.target_pose.orientation.z = tar_q[2]
self.target_pose.orientation.w = tar_q[3]
self.arm.set_pose_target(self.target_pose)
# plan
plan = RobotTrajectory()
counter = 0
while len(plan.joint_trajectory.points) == 0:
plan = self.arm.plan()
counter += 1
self.arm.set_planning_time(self.planning_limitation_time +
counter * 5.0)
if counter > 1:
return (False, start_state)
self.arm.set_planning_time(self.planning_limitation_time)
rospy.loginfo("!! Got a plan !!")
# publish the plan
pub_msg = HandringPlan()
pub_msg.grasp = grasp
print("---------debug--------{}".format(
len(plan.joint_trajectory.points)))
pub_msg.trajectory = plan
self.hp_pub.publish(pub_msg)
self.arm.clear_pose_targets()
# return goal state from generated trajectory
goal_state = JointState()
goal_state.header = Header()
goal_state.header.stamp = rospy.Time.now()
goal_state.name = plan.joint_trajectory.joint_names[:]
goal_state.position = plan.joint_trajectory.points[-1].positions[:]
return (True, goal_state)
def get_fk(self, joints):
"""
Gets forward kinematics to the end effector
joints: size 6 list. Joint angles for desired pose
returns pose: StackedPose of the end effector in the 'root' frame
"""
header = Header()
header.frame_id = self.ik_solver.base_link
robot_state = RobotState()
robot_state.joint_state.name = self.ik_solver.joint_names
robot_state.joint_state.position = joints
links = [self.ik_solver.tip_link]
return self.fk_solver(header, links, robot_state).pose_stamped[0]
def get_tool_position(self, joint_positions):
robot = moveit_commander.RobotCommander()
#l_names = robot.get_link_names()
#rospy.loginfo(["l_names:", l_names])
header = Header(0, rospy.Time.now(), "/world")
#fkln = l_names
fkln = ['tool0']
rs = RobotState()
rs.joint_state.name = self.ur_arm.get_active_joints()
rs.joint_state.position = joint_positions
res = self.moveit_fk(header, fkln, rs)
#rospy.loginfo(["FK LOOKUP:", self.moveit_fk(header, fkln, rs)])
#rospy.loginfo(["fk pose= ", +res.pose_stamped[0].pose.position])
return res.pose_stamped[0].pose.position
def __init__(self, limb):
self._baxter = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
self._right_limb_interface = baxter_interface.Limb('right')
self._base_link = self._baxter.get_root()
self._tip_link = limb + '_gripper'
self.solver = KDLKinematics(self._baxter, self._base_link,
self._tip_link)
self.rs = RobotState()
self.rs.joint_state.name = [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
self.rs.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Moveit group setup
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_python.PlanningSceneInterface(
self.robot.get_planning_frame())
# self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "right_arm"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
# service
self.set_model_config = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state",
GetLinkState)
self.sv_srv = rospy.ServiceProxy('/check_state_validity',
GetStateValidity)
# human target
self.target_pos_start = np.asarray(
[0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame
self.target_line_start = np.empty([22, 3], float)
for i in range(11):
self.target_line_start[i] = self.target_pos_start + [
0, -0.0, 1.8
] - (np.asarray([0, -0.0, 1.8]) -
np.asarray([0, -0.0, 0.5])) / 10 * i
self.target_line_start[i + 11] = self.target_pos_start + [
0, -0.5, 1.3
] + (np.asarray([0, 0.5, 1.3]) -
np.asarray([0, -0.5, 1.3])) / 10 * i
self.target_line = self.target_line_start
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 compute_f_k(self, compute_fk, joint_values):
fk_request = GetPositionFKRequest()
links = self.move_group_link_names
fk_request.fk_link_names = links
fk_request.header.frame_id = links[0]
state = RobotState()
joint_names = self.active_joints
state.joint_state.name = joint_names
state.joint_state.position = joint_values
fk_request.robot_state = state
fk_response = compute_fk(fk_request)
#manipulator_first_link_pose=fk_response.pose_stamped[0]
#print(manipulator_first_link_pose)
end_effector_pose = fk_response.pose_stamped[
len(fk_response.pose_stamped) - 1]
#print(end_effector_pose)
return end_effector_pose
def get_random_pose(self):
# get joint names
joint_names = self.joint_names()
# create a random joint state
bounds = []
for key, value in self.joint_limits().iteritems():
bounds.append(value)
bounds = np.array(bounds)
joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1],
len(joint_names))
# append it in a robot state
goal = RobotState()
goal.joint_state.name = joint_names
goal.joint_state.position = joint_state
goal.joint_state.header.stamp = rospy.Time.now()
goal.joint_state.header.frame_id = 'base'
return goal
def __init__(self):
# prepare msg to interface with moveit
self.rs = RobotState()
self.rs.joint_state.name = JOINT_NAMES
self.rs.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.joint_states_received = False
# subscribe to joint joint states
self.joint_state_subscriber = rospy.Subscriber("/joint_states",
JointState,
self.jointStatesCB,
queue_size=1)
# prepare service for collision check
self.sv_srv = rospy.ServiceProxy('/check_state_validity',
GetStateValidity)
# wait for service to become available
self.sv_srv.wait_for_service()
rospy.loginfo('service is avaiable')
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 test_enforce_bounds(self):
in_msg = RobotState()
in_msg.joint_state.header.frame_id = "base_link"
in_msg.joint_state.name = [
"joint_1",
"joint_2",
"joint_3",
"joint_4",
"joint_5",
"joint_6",
]
in_msg.joint_state.position = [0] * 6
in_msg.joint_state.position[0] = 1000
out_msg = self.group.enforce_bounds(in_msg)
self.assertEqual(in_msg.joint_state.position[0], 1000)
self.assertLess(out_msg.joint_state.position[0], 1000)
def _get_ik_robot(self, eef_poses, seeds=(), params=None):
ik_req = SolvePositionIKRequest()
for eef_pose in eef_poses:
ik_req.pose_stamp.append(eef_pose)
ik_req.seed_mode = ik_req.SEED_USER if len(
seeds) > 0 else ik_req.SEED_CURRENT
for seed in seeds:
ik_req.seed_angles.append(seed.joint_state)
resp = self._kinematics_services['ik']['robot']['service'].call(ik_req)
solutions = []
for j, v in zip(resp.joints, resp.isValid):
solutions.append(
RobotState(is_diff=False, joint_state=j) if v else None)
return solutions
def compute_fk_client(self, group, joint_values, links):
rospy.wait_for_service('compute_fk')
try:
compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = group.get_pose_reference_frame()
rs = RobotState()
rs.joint_state.header = header
rs.joint_state.name = group.get_active_joints()
rs.joint_state.position = joint_values
res = compute_fk(header, links, rs)
return res.pose_stamped
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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 get_fk(self, joints):
"""
Uses the MoveIt service to get the end effector pose from joints
Note that this is the pose of the beginning of the hand, not the hand itself
------
joints: a list of size 6. Joint states for the calculation
------
returns: a rigid transform from autolab? See plan_to_pose
"""
header = Header()
header.frame_id = self.ik_solver.base_link
robot_state = RobotState()
robot_state.joint_state.name = self.ik_solver.joint_names
robot_state.joint_state.position = joints
links = [self.ik_solver.tip_link]
return self.fk_solver(header, links, robot_state).pose_stamped[0]
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 __init__(self):
rospy.init_node('rotate_valve', anonymous=False)
rospy.Subscriber("/joint_states",
JointState,
self.cb_joint,
queue_size=1)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander("ur5_arm")
self.jt = RobotState()
self.jt.joint_state.header.frame_id = '/base_link'
self.jt.joint_state.name = [
'ur5_arm_shoulder_pan_joint', 'ur5_arm_shoulder_lift_joint',
'ur5_arm_elbow_joint', 'ur5_arm_wrist_1_joint',
'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint'
]
self.arm.set_start_state_to_current_state()
