def cmd_cb(self, cmd_msg):
cmd_string = cmd_msg.data
rospy.loginfo('Trying to get motion under ~motions/' + cmd_string +
': ' + str(rospy.has_param('~motions/' + cmd_string)))
if rospy.has_param('~motions/' + cmd_string):
# cancel running repeat timer
if self.timer:
self.timer.shutdown()
self.timer = None
# get motion trajectory
command = rospy.get_param('~motions/' + cmd_string)
repeat = command['repeat']
self.trajectory_msg = trajectory_msgs.JointTrajectory()
rostopic._fillMessageArgs(self.trajectory_msg, command['motion'])
# execute motion
if repeat:
period = sum([
p.time_from_start.secs +
p.time_from_start.nsecs / 1000000000.0
for p in self.trajectory_msg.points
])
self.timer = rospy.Timer(rospy.Duration(period),
self.cmd_repeat_cb)
self.trajectory_msg.header.stamp = rospy.Time.now()
self.relay.trajectory_cb(copy.deepcopy(self.trajectory_msg))
else:
if cmd_string == 'init':
self.trajectory_msg = trajectory_msgs.JointTrajectory()
self.trajectory_msg.header.stamp = rospy.Time.now()
self.relay.trajectory_cb(copy.deepcopy(self.trajectory_msg))
def to_joint_trajectory_msg(self):
"""
Create trajectory msg for the publisher.
:returns
a ROS msg for the joint trajectory
"""
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = [
joint.name for joint in self.joints
]
timestamps = self.get_unique_timestamps()
for timestamp in timestamps:
joint_trajectory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajectory_point.time_from_start = timestamp.to_msg()
for joint in self.joints:
interpolated_setpoint = joint.get_interpolated_setpoint(
timestamp)
joint_trajectory_point.positions.append(
interpolated_setpoint.position)
joint_trajectory_point.velocities.append(
interpolated_setpoint.velocity)
joint_trajectory_msg.points.append(joint_trajectory_point)
return joint_trajectory_msg
def to_joint_trajectory_msg(self):
"""Create trajectory msg for the publisher.
:returns
a ROS msg for the joint trajectory
"""
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = [joint.name for joint in self.joints]
timestamps = self.get_unique_timestamps()
for timestamp in timestamps:
joint_trajectory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajectory_point.time_from_start = rospy.Duration.from_sec(timestamp)
for joint in self.joints:
interpolated_setpoint = joint.get_interpolated_setpoint(timestamp)
if interpolated_setpoint.time != timestamp:
rospy.logwarn('Time mismatch in joint {jn} at timestamp {ts}, '
'got time {ti}'.format(jn=joint.name, ts=timestamp, ti=interpolated_setpoint.time))
joint_trajectory_point.positions.append(interpolated_setpoint.position)
joint_trajectory_point.velocities.append(interpolated_setpoint.velocity)
joint_trajectory_msg.points.append(joint_trajectory_point)
return joint_trajectory_msg
def goto_joint_positions(self, positions_goal):
positions_cur = self.get_joint_positions()
assert len(positions_goal) == len(positions_cur)
duration = norm(
(r_[positions_goal] - r_[positions_cur]) / self.vel_limits,
ord=inf)
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jtp = tm.JointTrajectoryPoint()
jtp.positions = positions_goal
jtp.velocities = zeros(len(positions_goal))
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
#jt.header.stamp = rospy.Time.now()
jt.header.stamp = rospy.Time.from_sec(rospy.Time.now().to_sec() + 0.3)
self.controller_pub.publish(jt)
print 'goto_joint_positions', self.vel_limits
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
duration)
self.pr2.start_thread(JustWaitThread(duration))
def goto_joint_positions(self, positions_goal, unwrap = False):
positions_cur = self.get_joint_positions()
assert len(positions_goal) == len(positions_cur)
duration = norm((r_[positions_goal] - r_[positions_cur])/self.vel_limits, ord=inf)
if unwrap:
positions_goal = positions_goal.copy()
for i in [2,4,6]:
positions_goal[i] = np.unwrap([positions_cur[i], positions_goal[i]])[1]
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = positions_goal
jtp.velocities = zeros(len(positions_goal))
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, duration)
self.pr2.start_thread(JustWaitThread(duration))
def goto_joint_positions(self, positions_goal):
"""
This method is eventually called by the Cartesian controller (for servoing)
"""
positions_cur = self.get_joint_positions()
assert len(positions_goal) == len(positions_cur)
duration = norm(
(r_[positions_goal] - r_[positions_cur]) / self.vel_limits,
ord=inf)
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = positions_goal
############################
if ADD_NOISE:
jtp.positions += self.joint_noise
############################
jtp.velocities = zeros(len(positions_goal))
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
duration)
self.pr2.start_thread(JustWaitThread(duration))
def publish_joint_traj(self, pub, joint_traj):
joint_traj_msg = tm.JointTrajectory()
joint_traj_msg.header.stamp = rospy.Time.now()
joint_traj_msg.points = [
tm.JointTrajectoryPoint(positions=joints) for joints in joint_traj
]
pub.publish(joint_traj_msg)
def follow_timed_trajectory(self, times, xyas, frame_id):
jt = tm.JointTrajectory()
jt.header.frame_id = frame_id
n = len(xyas)
assert len(times) == n
for i in xrange(n):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(times[i])
jtp.positions = xyas[i]
jt.points.append(jtp)
self.traj_pub.publish(jt)
def command_pan_tilt_vel(self, pan_vel, tilt_vel, dt=0.2):
pan, tilt = self.get_joint_positions()
jt = tm.JointTrajectory()
jt.header.stamp = rospy.Time.now() + rospy.Duration(0.01)
jt.joint_names = ["head_pan_joint", "head_tilt_joint"]
jtp = tm.JointTrajectoryPoint(
positions=[pan + pan_vel * dt, tilt + tilt_vel * dt],
velocities=[pan_vel, tilt_vel],
time_from_start=rospy.Duration(dt))
jt.points = [jtp]
self.controller_pub.publish(jt)
self.pr2.start_thread(JustWaitThread(dt))
def follow_timed_trajectory(self, times, angs):
times_up = np.arange(0, times[-1] + 1e-4, .1)
angs_up = np.interp(times_up, times, angs)
jt = tm.JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = ["%s_gripper_joint" % self.lr]
for (t, a) in zip(times, angs):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(t)
jtp.positions = [a]
jt.points.append(jtp)
self.controller_pub.publish(jt)
def createGripperPositionCommand(newPosition):
msg = tm.JointTrajectory()
point = tm.JointTrajectoryPoint()
point.positions = [newPosition, newPosition]
point.velocities = ones(2)
point.accelerations = zeros(2)
point.time_from_start = rospy.Duration(0.5)
msg.points = [point]
msg.joint_names = ["gripper_finger_joint_l", "gripper_finger_joint_r"]
msg.header.frame_id = "gripper_finger_joint_l"
msg.header.stamp = rospy.Time.now()
return msg
def follow_timed_joint_trajectory(self, positions, velocities, times):
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
for (position, velocity, time) in zip(positions, velocities, times):
jtp = tm.JointTrajectoryPoint()
jtp.positions = position
jtp.velocities = velocity
jtp.time_from_start = rospy.Duration(time)
jt.points.append(jtp)
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name, times[-1])
self.pr2.start_thread(JustWaitThread(times[-1]))
def createArmPositionCommand(newPositions):
msg = tm.JointTrajectory()
point = tm.JointTrajectoryPoint()
point.positions = newPositions
point.velocities = zeros(len(newPositions))
point.accelerations = zeros(len(newPositions))
point.time_from_start = rospy.Duration(0.5)
msg.points = [point]
jointNames = []
for i in range(5):
jointNames.append("arm_joint_" + str(i + 1))
msg.joint_names = jointNames
msg.header.frame_id = "arm_link_0"
msg.header.stamp = rospy.Time.now()
return msg
def set_angle(self, a, max_effort=default_max_effort):
a_cur = self.get_angle()
duration = abs((a - a_cur) / self.vel_limits[0])
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = [a]
jtp.velocities = [0]
jtp.time_from_start = rospy.Duration(duration)
jt.points = [jtp]
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
duration)
self.pr2.start_thread(JustWaitThread(duration))
def angle_change(self,data):
self.angle = data.data
if self.angle<-2.094:
self.angle = -2.094
elif self.angle > 0:
self.angle = 0.0
tilt = tm.JointTrajectory()
tilt.header.frame_id = "joint_trajectory"
tilt.header.stamp = rospy.Time.now()
tilt.joint_names = ["tilt"]
jtp = tm.JointTrajectoryPoint()
jtp.positions = [self.angle]
jtp.velocities = [0.5]
jtp.effort = [0.3]
jtp.time_from_start = rospy.Duration(2)
tilt.points = [jtp]
self.pub.publish(tilt)
def send_joint_positions(self, joint_positions, duration):
"""
Publishes joint_positions to be executed
in time duration
"""
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
jtp = tm.JointTrajectoryPoint()
jtp.positions = joint_positions
jtp.time_from_start = duration
################################
if ADD_NOISE:
jtp.positions += self.joint_noise
################################
jt.points = [jtp]
self.controller_pub.publish(jt)
def _create_trajectory(self, pos_mat, times, vel_mat=None):
#Make JointTrajectoryPoints
points = [tm.JointTrajectoryPoint() for i in range(pos_mat.shape[1])]
for i in range(pos_mat.shape[1]):
points[i].positions = pos_mat[:, i].A1.tolist()
points[i].accelerations = self.zeros
if vel_mat == None:
points[i].velocities = self.zeros
else:
points[i].velocities = vel_mat[:, i].A1.tolist()
for i in range(pos_mat.shape[1]):
points[i].time_from_start = rospy.Duration(times[i])
#Create JointTrajectory
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.points = points
jt.header.stamp = rospy.get_rostime()
return jt
def get_joint_trajectory_msg(self) -> trajectory_msg.JointTrajectory:
"""Return a joint_trajectory_msg containing the interpolated
trajectories for each joint
:returns: A joint_trajectory_msg
:rtype: joint_trajectory_msg
"""
# Update pose:
pose_list = [joint.position for joint in self.starting_position.values()]
self.pose = Pose(pose_list)
self._solve_middle_setpoint()
self._solve_desired_setpoint()
self._get_extra_ankle_setpoint()
# Create joint_trajectory_msg
self._to_joint_trajectory_class()
joint_trajectory_msg = trajectory_msg.JointTrajectory()
joint_trajectory_msg.joint_names = self.joint_names
timestamps = np.linspace(self.time[0], self.time[-1], INTERPOLATION_POINTS)
for timestamp in timestamps:
joint_trajecory_point = trajectory_msg.JointTrajectoryPoint()
joint_trajecory_point.time_from_start = Duration(timestamp).to_msg()
for joint_index, joint_trajectory in enumerate(self.joint_trajectory_list):
interpolated_setpoint = joint_trajectory.get_interpolated_setpoint(
timestamp
)
joint_trajecory_point.positions.append(interpolated_setpoint.position)
joint_trajecory_point.velocities.append(interpolated_setpoint.velocity)
self._check_joint_limits(joint_index, joint_trajecory_point)
joint_trajectory_msg.points.append(joint_trajecory_point)
return joint_trajectory_msg
def follow_timed_joint_trajectory(self, positions, velocities, times):
"""
This method is eventually called using trajopt output
"""
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
for (position, velocity, time) in zip(positions, velocities, times):
jtp = tm.JointTrajectoryPoint()
#################################
if ADD_NOISE:
position = position + self.joint_noise
#################################
jtp.positions = position
jtp.velocities = velocity
jtp.time_from_start = rospy.Duration(time)
jt.points.append(jtp)
self.controller_pub.publish(jt)
rospy.loginfo("%s: starting %.2f sec traj", self.controller_name,
times[-1])
self.pr2.start_thread(JustWaitThread(times[-1]))
def stop(self):
jt = tm.JointTrajectory()
jt.joint_names = self.joint_names
jt.header.stamp = rospy.Time.now()
self.controller_pub.publish(jt)
topics_list = [
'/gps/odom', '/gps/data', '/gps/str', '/um7', '/cam_time', '/ekf/Odom',
'/odom_alpha', '/velocity', '/UL', '/UR', '/WL', '/WL_ref', '/WR',
'/WR_ref', '/ctrl_flag', '/ekf/Vl', '/ekf/Vr', '/iL', '/iR', '/voltage',
'/diagnostics', '/diagnostics_agg', '/diagnostics_toplevel_state',
'/echoes', '/imu/data', '/imu/mag', '/joy', '/SaveData_flag',
'/dynamixel_workbench/joint_states', '/camera/rgb/image_color'
]
topic2save = ' '.join(topics_list)
# print(topic2save)
pub = rospy.Publisher('/dynamixel_workbench/joint_trajectory',
tm.JointTrajectory,
queue_size=10)
pub_flag = rospy.Publisher('/SaveData_flag', Bool, queue_size=10)
tilt = tm.JointTrajectory()
tilt.header.frame_id = "joint_trajectory"
tilt.joint_names = ["tilt"]
Flag_data = Bool()
# %% Functions
def q2pitch(q):
sinp = 2 * (q.w * q.y - q.z * q.x)
if np.abs(sinp) >= 1:
pitch = copysign(np.pi / 2, sinp)
else:
pitch = asin(sinp)
return (pitch + motor_offset)
def __init__(self, relay):
self.relay = relay
self.cmd_sub = rospy.Subscriber('rapiro_cmd', std_msgs.String,
self.cmd_cb)
self.timer = None
self.trajectory_msg = trajectory_msgs.JointTrajectory()
while iter < 20:
iter += 1
print iter
# z_dist += 0.02
pos_mat[2,3] -= z_dist
j = ghost.get_joints_from_cart("l", j,
bot.larm.rave_joint_inds,
pos_mat,
'base_link',MAX_ERROR=0.005,MAX_NUM_ITERS=5000)
Joints.append(j.tolist())
print Joints
bot.larm.follow_joint_trajectory(Joints)
rospy.sleep(0.5)
bot.larm.controller_pub.publish(tm.JointTrajectory())
#bot.larm.follow_joint_trajectory([j,j,j])
# bot.larm.goto_joint_positions(j.tolist())
# rospy.sleep(0.3)
"""
rospy.sleep(5)
z1 = bot.larm.get_pose_matrix('base_footprint','r_gripper_palm_link')
z2 = bot.larm.get_pose_matrix('base_footprint','l_gripper_palm_link')
y1 = conv.hmat_to_pose(z1)
y2 = conv.hmat_to_pose(z2)
