def callback(self, msg):
source_coords = Coordinates(pos=[
msg.pose.position.x, msg.pose.position.y, msg.pose.position.z
],
rot=[
msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z
])
source2dist_coords = Coordinates(pos=[self.x, self.y, self.z],
rot=self.q)
dist_coords \
= source_coords.copy_worldcoords().transform(
source2dist_coords)
self.pose_stamped.header = msg.header
self.pose_stamped.pose.position.x = dist_coords.translation[0]
self.pose_stamped.pose.position.y = dist_coords.translation[1]
self.pose_stamped.pose.position.z = dist_coords.translation[2]
self.pose_stamped.pose.orientation.w = dist_coords.quaternion[0]
self.pose_stamped.pose.orientation.x = dist_coords.quaternion[1]
self.pose_stamped.pose.orientation.y = dist_coords.quaternion[2]
self.pose_stamped.pose.orientation.z = dist_coords.quaternion[3]
self.pub.publish(self.pose_stamped)
def geometry_pose_to_coords(tf_pose):
"""Convert geometry_msgs.msg.Pose to Coordinates
Parameters
----------
tf_pose : geometry_msgs.msg.Pose or geometry_msgs.msg.PoseStamped
pose.
Returns
-------
ret : skrobot.coordinates.Coordinates
converted coordinates.
"""
if isinstance(tf_pose, geometry_msgs.msg.Pose):
pose = tf_pose
elif isinstance(tf_pose, geometry_msgs.msg.PoseStamped):
pose = tf_pose.pose
else:
raise TypeError('{} not supported'.format(type(tf_pose)))
if pose.orientation.w == 0.0 and \
pose.orientation.x == 0.0 and \
pose.orientation.y == 0.0 and \
pose.orientation.z == 0.0:
pose.orientation.w = 1.0
return Coordinates(pos=[pose.position.x, pose.position.y, pose.position.z],
rot=[
pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z
])
def tf_pose_to_coords(tf_pose):
"""Convert TransformStamped to Coordinates
Parameters
----------
tf_pose : geometry_msgs.msg.Transform or geometry_msgs.msg.TransformStamped
transform pose.
Returns
-------
ret : skrobot.coordinates.Coordinates
converted coordinates.
"""
if isinstance(tf_pose, geometry_msgs.msg.Transform):
transform = tf_pose
elif isinstance(tf_pose, geometry_msgs.msg.TransformStamped):
transform = tf_pose.transform
else:
raise TypeError('{} not supported'.format(type(tf_pose)))
if transform.rotation.w == 0.0 and \
transform.rotation.x == 0.0 and \
transform.rotation.y == 0.0 and \
transform.rotation.z == 0.0:
transform.rotation.w = 1.0
return Coordinates(pos=[
transform.translation.x, transform.translation.y,
transform.translation.z
],
rot=[
transform.rotation.w, transform.rotation.x,
transform.rotation.y, transform.rotation.z
])
def set_robot_config(robot_model, joint_list, av, with_base=False):
"""A utility function for setting robot state
Parameters
----------
robot_model : skrobot.model.CascadedLink
robot model
joint_list : list[skrobot.model.Joint]
joint list to be set
av : numpy.ndarray[float](n_dof,)
angle vector which has n_dof dims.
with_base : bool
If `with_base=False`, `n_dof` is the number of joints `n_joint`,
but if `with_base=True`, `n_dof = n_joint + 3`.
"""
if with_base:
assert len(joint_list) + 3 == len(av)
else:
assert len(joint_list) == len(av)
if with_base:
av_joint, av_base = av[:-3], av[-3:]
x, y, theta = av_base
co = Coordinates(pos=[x, y, 0.0], rot=rpy_matrix(theta, 0.0, 0.0))
robot_model.newcoords(co)
else:
av_joint = av
for joint, angle in zip(joint_list, av_joint):
joint.joint_angle(angle)
def test_coords_to_tf_pose(self):
c = Coordinates(pos=(1, 2, 3))
pose = coords_to_tf_pose(c)
testing.assert_equal(
[pose.translation.x, pose.translation.y, pose.translation.z],
(1, 2, 3))
testing.assert_equal([
pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z
], (1, 0, 0, 0))
def test_coords_to_geometry_pose(self):
c = Coordinates(pos=(1, 2, 3))
pose = coords_to_geometry_pose(c)
testing.assert_equal(
[pose.position.x, pose.position.y, pose.position.z], (1, 2, 3))
testing.assert_equal([
pose.orientation.w, pose.orientation.x, pose.orientation.y,
pose.orientation.z
], (1, 0, 0, 0))
def pose(self):
"""Getter of Pose in pybullet phsyics simulator.
Wrapper of pybullet.getBasePositionAndOrientation.
Returns
-------
pose : skrobot.coordinates.Coordinates
pose of this robot in the phsyics simulator.
"""
pos, q_xyzw = p.getBasePositionAndOrientation(self.robot_id)
q_wxyz = xyzw2wxyz(q_xyzw)
return Coordinates(pos=pos, rot=q_wxyz)
def go_pos(self, x=0.0, y=0.0, yaw=0.0, wait=True):
"""Move Robot using MoveBase
Parameters
----------
x : float
move distance with respect to x axis. unit is [m].
y : float
move distance with respect to y axis. unit is [m].
yaw : float
rotate angle. unit is [rad].
wait : bool
if wait is True, wait until move base done.
"""
c = Coordinates(pos=(x, y, 0)).rotate(yaw, 'z')
return self.move_to(c, wait=wait, frame_id=self.base_frame_id)
def geometry_pose_to_coords(tf_pose):
"""Convert geometry_msgs.msg.Pose to Coordinates
"""
if tf_pose.pose.orientation.w == 0.0 and \
tf_pose.pose.orientation.x == 0.0 and \
tf_pose.pose.orientation.y == 0.0 and \
tf_pose.pose.orientation.z == 0.0:
tf_pose.pose.orientation.w = 1.0
return Coordinates(pos=[
tf_pose.pose.position.x, tf_pose.pose.position.y,
tf_pose.pose.position.z
],
rot=[
tf_pose.pose.orientation.w,
tf_pose.pose.orientation.x,
tf_pose.pose.orientation.y,
tf_pose.pose.orientation.z
])
def tf_pose_to_coords(tf_pose):
"""Convert TransformStamped to Coordinates
"""
if tf_pose.transform.rotation.w == 0.0 and \
tf_pose.transform.rotation.x == 0.0 and \
tf_pose.transform.rotation.y == 0.0 and \
tf_pose.transform.rotation.z == 0.0:
tf_pose.transform.rotation.w = 1.0
return Coordinates(pos=[
tf_pose.transform.translation.x, tf_pose.transform.translation.y,
tf_pose.transform.translation.z
],
rot=[
tf_pose.transform.rotation.w,
tf_pose.transform.rotation.x,
tf_pose.transform.rotation.y,
tf_pose.transform.rotation.z
])
def odom(self):
"""Return Coordinates of this odom
Returns
-------
odom_coords : skrobot.coordinates.Coordinates
coordinates of odom.
"""
if self.odom_msg is None:
raise RuntimeError('odom is not set. Please check odom topic {}'
' is published.'.format(self.odom_topic))
pos = (self.odom_msg.pose.pose.position.x,
self.odom_msg.pose.pose.position.y,
self.odom_msg.pose.pose.position.z)
q_wxyz = (self.odom_msg.pose.pose.orientation.w,
self.odom_msg.pose.pose.orientation.x,
self.odom_msg.pose.pose.orientation.y,
self.odom_msg.pose.pose.orientation.z)
return Coordinates(pos=pos, rot=q_wxyz)
import skrobot
import numpy as np
from skrobot.coordinates.math import rpy_matrix
from skrobot.coordinates import Coordinates
robot_model = skrobot.models.urdf.RobotModelFromURDF(
urdf_file=skrobot.data.pr2_urdfpath())
rarm_end_coords = skrobot.coordinates.CascadedCoords(
parent=robot_model.r_gripper_tool_frame, name='rarm_end_coords')
print(rarm_end_coords.worldpos())
co = Coordinates(pos=[1.0, 1.0, 1.0], rot=rpy_matrix(0.3, 0.3, 0.3))
robot_model.newcoords(co)
print(rarm_end_coords.worldpos())
mylink = Link(pos=[0.1, 0.1, 0.1], rot=[0.1, 0.2, 0.3], name="mylink")
robot_model.r_upper_arm_link.assoc(mylink, mylink)
link_list = [
robot_model.r_shoulder_pan_link, robot_model.r_shoulder_lift_link,
robot_model.r_upper_arm_roll_link, robot_model.r_elbow_flex_link,
robot_model.r_forearm_roll_link, robot_model.r_wrist_flex_link,
robot_model.r_wrist_roll_link, robot_model.base_link,
robot_model.r_upper_arm_link, mylink
]
joint_angles = [0.564, 0.35, -0.74, -0.7, -0.7, -0.17, -0.63]
for j, a in zip(joint_list, joint_angles):
j.joint_angle(a)
x, y, theta = 0.3, 0.5, 0.7
co = Coordinates(pos=[x, y, 0.0], rot=rpy_matrix(theta, 0.0, 0.0))
robot_model.newcoords(co)
angle_vector = joint_angles + [x, y, theta]
# compute pose of links
world_coordinate = CascadedCoords()
def extract_pose(co):
return np.hstack((co.worldpos(), co.worldcoords().rpy_angle()[0]))
pose_list = [extract_pose(co).tolist() for co in link_list]
# compute jacobian of links
def move_trajectory_sequence(self, trajectory_points, time_list, stop=True,
start_time=None, send_action=None):
"""Move base following the trajectory points at each time points
trajectory-points [ list of #f(x y yaw) ([m] for x, y; [rad] for yaw) ]
time-list [list of time span [sec] ]
stop [ stop after msec moveing ]
start-time [ robot will move at start-time [sec or ros::Time] ]
send-action [ send message to action server, it means robot will move ]
"""
if len(trajectory_points) != len(time_list):
raise ValueError
while self.odom_msg is None:
rospy.sleep(0.01)
odom_coords = geometry_pose_to_coords(self.odom_msg.pose)
goal = control_msgs.msg.FollowJointTrajectoryActionGoal()
msg = trajectory_msgs.msg.JointTrajectory()
msg.joint_names = self.move_base_trajectory_joint_names
if start_time is not None:
msg.header.stamp = start_time
else:
msg.header.stamp = rospy.Time.now()
coords_list = [odom_coords]
for traj_point in trajectory_points:
cds = Coordinates(pos=odom_coords.translation,
rot=odom_coords.rotation)
cds.translate((traj_point[0], traj_point[1], 0.0))
cds.rotate(traj_point[2], 'z')
coords_list.append(cds)
cur_cds = odom_coords
cur_yaw = cur_cds.rpy_angle()[0][0]
cur_time = 0
pts_msg_list = []
for i, (cur_cds, next_cds) in enumerate(zip(
coords_list[:-1],
coords_list[1:])):
next_time = time_list[i]
tra = cur_cds.transformation(next_cds)
rot = cur_cds.rotate_vector(tra.translation)
diff_yaw = rotation_distance(cur_cds.rotation, next_cds.rotation)
pts_msg_list.append(
trajectory_msgs.msg.JointTrajectoryPoint(
positions=[cur_cds.translation[0],
cur_cds.translation[1],
cur_yaw],
velocities=[rot[0] / next_time,
rot[1] / next_time,
tra.rpy_angle()[0][0] / next_time],
time_from_start=rospy.Time(cur_time)))
cur_time += next_time
cur_cds = next_cds
if tra.rpy_angle()[0][0] > 0:
cur_yaw += abs(diff_yaw)
else:
cur_yaw -= abs(diff_yaw)
# append last point
if stop:
velocities = [0, 0, 0]
else:
velocities = [rot[0] / next_time,
rot[1] / next_time,
tra.rpy_angle()[0][0] / next_time]
pts_msg_list.append(
trajectory_msgs.msg.JointTrajectoryPoint(
positions=[cur_cds.translation[0],
cur_cds.translation[1],
cur_yaw],
velocities=velocities,
time_from_start=rospy.Time(cur_time)))
msg.points = pts_msg_list
goal.goal.trajectory = msg
if send_action:
if self.move_base_trajectory_action is None:
rospy.logerror(
'send_action is True, '
'but move_base_trajectory_action is not found')
return False
self.move_base_trajectory_action.send_goal(goal.goal)
if self.move_base_trajectory_action.wait_for_result():
return self.move_base_trajectory_action.get_result()
else:
return False
return goal
output_file = osp.splitext(input_file)[0] + '_circle.txt'
radius = args.radius
interval = args.interval
gui = args.gui
label_list = []
pos_list = []
coords_list = []
with open(input_file)as f:
for line in f.readlines():
values = [float(v) for v in line.split()]
label = int(values[0])
pos = values[1:4]
quaternion = values[4:]
c = Coordinates(pos=pos, rot=quaternion)
label_list.append(label)
pos_list.append(pos)
coords_list.append(c)
print('the number of exsiting points: ',
len(coords_list))
dummy_labels = [0] * len(coords_list)
base_coords = make_average_coords_list(
coords_list, dummy_labels, average_pos=True)[0]
circle_coords_list = []
dy_dz_list = make_circle_coords(
r=radius, interval=interval)