def readInPoses(self):
poses = rospy.get_param('~poses')
rospy.loginfo("Found %d poses on the param server", len(poses))
for key,value in poses.iteritems():
try:
# TODO: handle multiple points in trajectory
trajectory = JointTrajectory()
trajectory.joint_names = value["joint_names"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(value["time_from_start"])
point.positions = value["positions"]
trajectory.points = [point]
self.poseLibrary[key] = trajectory
except:
rospy.logwarn("Could not parse pose \"%s\" from the param server:", key);
rospy.logwarn(sys.exc_info())
# add a default crouching pose:
if not "crouch" in self.poseLibrary:
trajectory = JointTrajectory()
trajectory.joint_names = ["Body"]
point = JointTrajectoryPoint()
point.time_from_start = Duration(1.5)
point.positions = [0.0,0.0, # head
1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm
-0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg
-0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg
1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm
trajectory.points = [point]
self.poseLibrary["crouch"] = trajectory;
def tuck(self):
# prepare a joint trajectory
msg = JointTrajectory()
msg.joint_names = servos
msg.points = list()
point = JointTrajectoryPoint()
point.positions = forward
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(5.0)
msg.points.append(point)
# point = JointTrajectoryPoint()
#point.positions = to_side
#point.velocities = [ 0.0 for servo in msg.joint_names ]
#point.time_from_start = rospy.Duration(8.0)
#msg.points.append(point)
#point = JointTrajectoryPoint()
#point.positions = tucked
#point.velocities = [ 0.0 for servo in msg.joint_names ]
#point.time_from_start = rospy.Duration(11.0)
#msg.points.append(point)
# call action
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self._client.send_goal(goal)
def send_command(self, cmd):
rospy.logdebug(" Sending cmd to controller [%s]"%self._controller_id)
cmd_msg = JointTrajectory()
cmd_msg.header.stamp = rospy.Time().now()
cmd_msg.joint_names = self._config["joint_names"]
cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
self._pub.publish(cmd_msg)
def handle_camera_direction(self, subject):
command = JointTrajectory()
command.joint_names = ["head_2_joint", "head_1_joint"]
point1 = JointTrajectoryPoint()
#point2 = JointTrajectoryPoint()
point1.velocities = [0.0, 0.0]
#point2.velocities = [0]
point1.time_from_start = Duration(2.0, 0.0)
rospy.loginfo(subject)
if subject[0] == 'up':
self.head_ud = self.head_ud + self.head_move_step_size
if subject[0] == 'down':
self.head_ud = self.head_ud - self.head_move_step_size
if subject[0] == 'left':
self.head_lr = self.head_lr + self.head_move_step_size
if subject[0] == 'right':
self.head_lr = self.head_lr - self.head_move_step_size
rospy.loginfo(self.head_ud)
point1.positions = [self.head_ud, self.head_lr]
command.points = [point1]
self.pub_head.publish(command)
return
def CreateTrajectory(self, value):
""" creates custom trajectory for the head
@ returns ROS followjointTrajectory
"""
max_vel = self.GetMaxVelocity()
curr_val = self.GetJointState()
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.header.seq = 1
traj_msg.joint_names = self.GetJointNames()
traj_msg.points = []
orig_point = JointTrajectoryPoint()
orig_point.positions = self.GetJointState()
orig_point.velocities = [0.0, 0.0]
orig_point.accelerations = [0.0, 0.0]
orig_point.time_from_start = rospy.Duration.from_sec(0.0)
traj_msg.points.append(orig_point)
end_point = JointTrajectoryPoint()
end_point.positions = value
end_point.velocities = [0.0, 0.0]
end_point.accelerations = [0.0, 0.0]
pan_transit = numpy.absolute(
(orig_point.positions[0] - value[0]) / max_vel[0])
tilt_transit = numpy.absolute(
(orig_point.positions[1] - value[1]) / max_vel[1])
end_point.time_from_start = rospy.Duration.from_sec(
numpy.amax([pan_transit, tilt_transit]) / 15.)
traj_msg.points.append(end_point)
return traj_msg
def go_home():
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
rate = rospy.Rate(25)
Tmsg = JointTrajectory()
# Tmsg.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint','elbow_joint','wrist_1_joint','wrist_2_joint', 'wrist_3_joint']
Tmsg.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
Tmsg.header.stamp = rospy.Time.now()
Tpointmsg = JointTrajectoryPoint()
Tpointmsg.velocities = [0, 0, 0, 0, 0, 0]
Tpointmsg.time_from_start = rospy.Duration(0.5)
Tpointmsg.positions = [-np.pi / 2, -np.pi / 2, np.pi / 2, 0, 0, 0]
Tmsg.points = [Tpointmsg]
# print "publishing"
joint_states = rospy.wait_for_message("joint_states", JointState)
joint_states.position = [round(num, 3) for num in joint_states.position]
while (joint_states.position[0:3] !=
[round(np.pi / 2, 3),
round(-np.pi / 2, 3),
round(-np.pi / 2, 3)]):
# print joint_states.position[0:3]
pub.publish(Tmsg)
joint_states = rospy.wait_for_message("joint_states", JointState)
joint_states.position = [
round(num, 3) for num in joint_states.position
]
rospy.sleep(1)
def velocitiesPublisher(output):
#### newly added ####
global arm_current
#### newly added ####
# 6x1 matrix for end_effector work-space velocities
# Obtained by multiplying NPF with output
end_effector_velocities = NPS.dot(output)
correction_velocities = Vector3()
formatOutput(end_effector_velocities, correction_velocities)
#### newly added ####
# Frequency which TIAGo runs at
freq = 50
r= rospy.Rate(freq)
duration = Duration(nsecs-1e9/float(freq)*3.0)
# Publisher
arm_pub = rospy.Publisher('/arm_controller/safe_command', JointTrajectory, queue_size=1)
arm_msg = JointTrajectory()
# Test to send velocity commands to TIAGo joint 6 and joint 7
joint_angles_6 = correction_velocities.z
joint_angles_7 = correction_velocities.x
arm_pub.publish(correction_velocities)
arm_msg.joint_names = ["arm_6_joint", "arm_7_joint"]
arm_point = JointTrajectoryPoint()
arm_point.time_from_start = duration
arm_msg.points = [arm_point]
def to_robot_trajectory(self, num_waypoints=300, jointspace=True):
"""
Parameters
----------
num_waypoints : float
how many points in the :obj:`moveit_msgs.msg.RobotTrajectory`
jointspace : bool
What kind of trajectory. Joint space points are 7x' and describe the
angle of each arm. Workspace points are 3x', and describe the x,y,z
position of the end effector.
"""
traj = JointTrajectory()
traj.joint_names = self.limb.joint_names()
points = []
for t in np.linspace(0, self.total_time, num=num_waypoints):
point = self.trajectory_point(t, jointspace) # make sure this point is the right type of message
points.append(point)
#print(point)
# We want to make a final point at the end of the trajectory so that the
# controller has time to converge to the final point.
# extra_point = self.trajectory_point(self.total_time, jointspace)
# extra_point.time_from_start = rospy.Duration.from_sec(self.total_time + 1)
# points.append(extra_point)
traj.points = points
traj.header.frame_id = 'base'
robot_traj = RobotTrajectory()
robot_traj.joint_trajectory = traj
return robot_traj
def send_goal(self, position):
# Message type in JointTrajectory
goal_msg = JointTrajectory()
# Joint trajectory points
jtp = JointTrajectoryPoint()
jtp.velocities = [0.0]
jtp.time_from_start.sec = 0
jtp.time_from_start.nanosec = 0
jtp.positions = [position.data[0]]
if position.data[1] == 1.0:
joint = "eyes_shift_horizontal_joint"
else:
joint = "eyes_shift_vertical_joint"
# Build message
jointNames = []
#print(joint)
jointNames.append(joint)
goal_msg.points = [jtp]
goal_msg.joint_names = jointNames
# Assign goal
goal = FollowJointTrajectory.Goal()
goal.trajectory = goal_msg
self.eye_action_client_horizontal.wait_for_server()
self.get_logger().info('Goal: %f ' % jtp.positions[0])
self.eye_action_client_horizontal.send_goal_async(goal)
def main():
pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
rospy.init_node('traj_test')
p1 = JointTrajectoryPoint()
p1.positions = [0,0,0,0,0,0,0]
p1.velocities = [0,0,0,0,0,0,0]
p1.accelerations = [0,0,0,0,0,0,0]
p2 = JointTrajectoryPoint()
p2.positions = [0,0,1.0,0,-1.0,0,0]
p2.velocities = [0,0,0,0,0,0,0]
p2.accelerations = [0,0,0,0,0,0,0]
p2.time_from_start = rospy.Time(4.0)
p3 = JointTrajectoryPoint()
p3.positions = [0,0,-1.0,0,1.0,0,0]
p3.velocities = [0,0,0,0,0,0,0]
p3.accelerations = [0,0,0,0,0,0,0]
p3.time_from_start = rospy.Time(20.0)
traj = JointTrajectory()
traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
traj.points = [p1,p2,p3]
rospy.loginfo(traj)
r = rospy.Rate(1) # 10hz
pub.publish(traj)
r.sleep()
pub.publish(traj)
def dual_arm_test():
pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command',
JointTrajectory)
rospy.init_node('dual_arm_test')
rospy.sleep(1.0)
while not rospy.is_shutdown():
traj = JointTrajectory()
traj.joint_names = [
'lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint',
'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint'
]
ptn = JointTrajectoryPoint()
ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.points = []
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
def activate(self, config, ff_wrench=Wrench()):
# パラメータが存在するかチェック
configs = rospy.get_param("/hsrb/impedance_control/config_names")
if config not in configs:
rospy.logerr(config + "is invalid name.")
return False
# 現在の姿勢から非常に長い（１日分）現在姿勢の軌道を作成
arm_joint, arm_value = utils.extract_joint_positions(
self._joint_states_cache.data, _ARM_JOINT_NAMES)
odom_joint, odom_value = utils.extract_odom_positions(
self._odom_states_cache.data, _ODOM_JOINT_NAMES)
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.get_rostime()
trajectory.joint_names = arm_joint + odom_joint
trajectory_point = JointTrajectoryPoint()
trajectory_point.time_from_start = rospy.Duration(86400.0)
trajectory_point.positions = arm_value + odom_value
trajectory.points = [trajectory_point]
# 軌道を投げる
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.preset_parameters_name = config
goal.feed_forward_wrench.wrench = ff_wrench
self._client.send_goal(goal)
return True
def on_enter(self, userdata):
self.return_code = None
try:
if (len(userdata.joint_names) != len(userdata.joint_values)):
Logger.logerr(
'Mismatch in joint names and values (%d vs. %d) -' %
(len(userdata.joint_values), len(self.joint_names)))
self.return_code = 'param_error'
return
# Action Initialization
joint_trajectory = JointTrajectory()
joint_trajectory.header.stamp = rospy.Time.now()
joint_trajectory.joint_names = [
name for name in userdata.joint_names
]
joint_trajectory.points = [JointTrajectoryPoint()]
joint_trajectory.points[0].time_from_start = rospy.Duration(
self.duration)
joint_trajectory.points[0].positions = [
value for value in userdata.joint_values
]
# Assign to the user data
userdata.joint_trajectory = joint_trajectory
self.return_code = 'done'
except Exception as e:
Logger.logerr(
'Unable to convert joint values to trajectory - \n%s' % str(e))
self.return_code = 'param_error'
def init_pos():
# Simple script server does not work for raw3-1 therefore using trajectory controller directly
switch_controller = rospy.ServiceProxy(
'/arm/controller_manager/switch_controller', SwitchController)
print(switch_controller([
'joint_trajectory_controller',
], None, 1)) # switch on
# Limits arm_elbow_joint: lower=\"-3.14159265\" upper=\"3.14159265\
# Limits arm_shoulder_lift_joint: lower=\"-3.14159265\" upper=\"3.14159265\"
# Limits arm_shoulder_pan_joint: lower=\"-3.14159265\" upper=\"3.14159265\"
# Limits arm_wrist_1_joint: lower=\"-3.14159265\" upper=\"3.14159265\"\
# Limits arm_wrist_2_joint: lower=\"-3.14159265\" upper=\"3.14159265\"
# Limits arm_wrist_3_joint: lower=\"-3.14159265\" upper=\"3.14159265\"
jnt_traj = JointTrajectory()
jnt_traj.joint_names = [
'arm_elbow_joint', 'arm_shoulder_lift_joint', 'arm_shoulder_pan_joint',
'arm_wrist_1_joint', 'arm_wrist_2_joint', 'arm_wrist_3_joint'
]
jtp = JointTrajectoryPoint()
jtp.positions = [1.69, -1.04, -1.38, -0.67, -0.69, 0.04]
jtp.time_from_start.secs = 1.0
jnt_traj.points = [jtp]
time.sleep(1)
pub.publish(jnt_traj)
time.sleep(1)
def move(self, p, v_scale=0.1, duration_low_bound=1, start_duration=0.5):
target_q = self.get_inverse_kin(p)
if target_q is None:
return False
traj = JointTrajectory()
traj.header = rospy.Header(frame_id="1", stamp=rospy.Time())
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
dist = np.abs(np.subtract(target_q,
self.current_state.positions)).max()
duration = rospy.Duration(
nsecs=int(max(dist / v_scale, duration_low_bound) * 1e9))
# print(dist,duration.secs,duration.nsecs)
end_point = JointTrajectoryPoint(
positions=target_q,
velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
time_from_start=duration)
start_point = JointTrajectoryPoint(
positions=(np.array(self.current_state.positions) +
np.array(self.current_state.velocities) *
(start_duration + 0.01)).tolist(),
velocities=self.current_state.velocities,
time_from_start=rospy.Duration(0, int(start_duration * 1e9)))
traj.points = [start_point, end_point]
self.pub.publish(traj)
return True
def visualize_movement(start, path):
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory,
queue_size=100) # for visualizing the robot movement;
sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = RobotTrajectory()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state, _ in path:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
# print("The joint trajectory is: %s" % trajectory)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
def dual_arm_test():
pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
rospy.init_node('dual_arm_test')
rospy.sleep(1.0)
while not rospy.is_shutdown():
traj = JointTrajectory()
traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
ptn = JointTrajectoryPoint()
ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
ptn.time_from_start = rospy.Duration(2.0)
traj.points = []
traj.header.stamp = rospy.Time.now()
traj.points.append(ptn)
pub.publish(traj)
cob_pub.publish(traj)
rospy.sleep(3.0)
def main():
rospy.init_node('send_joints')
rospy.loginfo("###########################################################")
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
# Joint names for UR5
traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint',
'elbow_joint', 'wrist_1_joint', 'wrist_2_joint',
'wrist_3_joint']
rate = rospy.Rate(1)
pts = JointTrajectoryPoint()
traj.header.stamp = rospy.Time.now()
pts.positions = [0.0, -1.57, 1.57, -1.57, -1.57, 0.0]
pts.time_from_start = rospy.Duration(1.0)
# Set the points to the trajectory
traj.points = []
traj.points.append(pts)
# Publish the message
pub.publish(traj)
rospy.loginfo("-------------------------------------------------------------")
rospy.spin()
def get_trajectory_message(self, action, robot_id=0):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
action_msg = JointTrajectory()
action_msg.joint_names = self.environment['joint_order']
# Create a point to tell the robot to move to.
target = JointTrajectoryPoint()
action_float = [float(i) for i in action]
target.positions = action_float
# These times determine the speed at which the robot moves:
# it tries to reach the specified target position in 'slowness' time.
if (self.slowness_unit == 'sec') or (self.slowness_unit is None):
target.time_from_start.secs = self.slowness
elif (self.slowness_unit == 'nsec'):
target.time_from_start.nsecs = self.slowness
else:
print("Unrecognized unit. Please use sec or nsec.")
# Package the single point into a trajectory of points with length 1.
action_msg.points = [target]
return action_msg
def _publish_joint_trajectory(self, *_):
if not self._enabled:
return
if self._target_values is None:
return
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
point = JointTrajectoryPoint()
point.positions = self._target_values
point.time_from_start = rospy.Duration(self._timer_rate)
msg.points = [point]
self._joint_trajectory_publisher.publish(msg)
# Reset Target
self._target_values = None
# Save state for next time
self._last_target_values = point.positions
self._last_robot_state_published = self._new_robot_state
def pan_and_tilt(self, pan, tilt, duration=1.0, feedback_cb=None, done_cb=None):
"""
Moves the robot's head to the point specified in the duration
specified
:param pan: The pan - expected to be between HeadClient.PAN_LEFT
and HeadClient.PAN_RIGHT
:param tilt: The tilt - expected to be between HeadClient.TILT_UP
and HeadClient.TILT_DOWN
:param duration: The amount of time to take to get the head to
the specified location.
:param feedback_cb: Same as send_trajectory's feedback_cb
:param done_cb: Same as send_trajectory's done_cb
"""
if(pan < self.PAN_RIGHT or pan > self.PAN_LEFT):
return
if(tilt > self.TILT_DOWN or tilt < self.TILT_UP):
return
point = JointTrajectoryPoint()
point.positions = [float(pan), float(tilt)]
point.time_from_start = rospy.Duration(duration)
trajectory = JointTrajectory()
trajectory.points = [point]
trajectory.joint_names = [self.JOINT_PAN, self.JOINT_TILT]
return self.send_trajectory(traj=trajectory, feedback_cb=feedback_cb, done_cb=done_cb)
def velocitiesPublisher(output):
#Newly added
global arm_current
freq = 20
r = rospy.Rate(freq)
# ROS publishers
duration = Duration(nsecs=1e9 / float(freq) * 3.0)
arm_pub = rospy.Publisher('/arm_controller/safe_command',
JointTrajectory,
queue_size=1)
arm_msg = JointTrajectory()
correction_velocities = Vector3()
formatOutput(output, correction_velocities)
print('correction_velocities[] ', correction_velocities)
joint_angles_6 = correction_velocities.z
joint_angles_7 = correction_velocities.x
arm_pub.publish(correction_velocities)
#arm_msg.joint_names = ["arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint",
# "arm_7_joint"]
arm_msg.joint_names = ["arm_6_joint", "arm_7_joint"]
arm_point = JointTrajectoryPoint()
arm_point.time_from_start = duration
arm_msg.points = [arm_point]
# Subscribers
arm_sub = rospy.Subscriber('/arm_controller/state',
JointTrajectoryControllerState, cb_arm_state)
def go_to_q(self, q, dt=1., wait=True, exact=False):
"""
dt deprecated; NEW: scale sucht that maximum joint velocity is 1/4 rad/s
"""
q = np.asarray(q)
if not exact and np.linalg.norm(q - np.asarray(
self.move_group.get_current_joint_values())) < 0.1:
print("Close enough already")
return
client = actionlib.SimpleActionClient(
'/panda_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
#print("Waiting for action server ...")
client.wait_for_server()
goal = FollowJointTrajectoryGoal()
joint_traj = JointTrajectory()
joint_traj.joint_names = [
"panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
"panda_joint5", "panda_joint6", "panda_joint7"
]
point1 = JointTrajectoryPoint()
point1.positions = q.tolist()
point1.velocities = [0., 0., 0., 0., 0., 0., 0.]
delta_joints = np.abs(
np.asarray(point1.positions) -
np.asarray(self.move_group.get_current_joint_values()))
point1.time_from_start = rospy.Duration(
max(0.5,
np.max(delta_joints) / 0.25))
joint_traj.points = [point1]
goal.trajectory = joint_traj
joint_traj.header.stamp = rospy.Time.now() + rospy.Duration(1.0)
client.send_goal_and_wait(goal)
if wait:
self.wait_for_joint_arrival(q)
def talker():
pub = rospy.Publisher('/ur_driver/joint_speed',
JointTrajectory,
queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(125) # 10hz
trajectory = JointTrajectory()
i_seq = 0
qvel = [0, 0, 0, 0, 0, 0]
while not rospy.is_shutdown():
# hello_str = "hello world %s" % rospy.get_time()
for i in range(6):
qvel[i] += (np.random.rand() - 0.5) * 2 * 0.01
# qvel[-2] += (np.random.rand()-0.5)*2* 0.01
current_time = rospy.Time.now()
trajectory.header.seq = i_seq
i_seq += 1
trajectory.header.stamp = current_time
trajectory.joint_names = [
'elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
trajectory.points = [JointTrajectoryPoint(velocities=qvel)]
rospy.loginfo(trajectory)
pub.publish(trajectory)
rate.sleep()
def main(data):
pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
# Create the topic message
arm = JointTrajectory()
arm.header = Header()
# Joint names for UR5
arm.joint_names = [
'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
'arm_5_joint', 'arm_gripper_right_joint', 'arm_gripper_left_joint'
]
rate = rospy.Rate(50) # 50hz
rospy.loginfo(arm)
arm.header.stamp = rospy.Time.now()
pts = JointTrajectoryPoint()
pts.positions = [
data.position[3], data.position[0], data.position[4], data.position[1],
data.position[2]
]
pts.time_from_start = rospy.Duration(1.0)
# Set the points to the trajectory
arm.points = []
arm.points.append(pts)
# Publish the message
pub.publish(arm)
rate.sleep()
def talker():
rospy.init_node('joint_trajectory_publisher', anonymous=True)
pub = rospy.Publisher('arm_controller/command',
JointTrajectory,
queue_size=10)
#pub = rospy.Publisher('/joint_group_position_controller/command', Float64MultiArray, queue_size=10)
rospy.sleep(0.5)
#tes = Float64MultiArray()
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
msg.points = [JointTrajectoryPoint() for i in range(1)]
msg.points[0].positions = [
-math.pi / 2, -2.8 * math.pi / 6, -math.pi / 3, -1 * math.pi / 1.5,
math.pi / 2, -math.pi / 2
]
msg.points[0].time_from_start = rospy.Time(1.0)
#tes.data = [math.pi/2, -math.pi/6.0, -math.pi/2, -math.pi/3, -math.pi/2, 0.0]
#tes.data = [-math.pi/2, -2.8 * math.pi/6, -math.pi/3, -1*math.pi/1.5, math.pi/2, -math.pi/2];
pub.publish(msg)
rospy.sleep(0.5)
def talker_4():
try:
pub = rospy.Publisher('/abb_irb6640_controller/command',
JointTrajectory,
queue_size=10)
rate = rospy.Rate(125)
qvel = [0, 0, 0, 0, 0, 0]
i_seq = 0
global qvel_global, mutex
trajectory = JointTrajectory()
trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
while not rospy.is_shutdown():
if mutex.acquire():
acc_max = 0.01
for i in range(6):
if (qvel_global[i] - qvel[i]) > acc_max:
qvel[i] += acc_max
elif (qvel_global[i] - qvel[i]) < -acc_max:
qvel[i] -= acc_max
else:
qvel[i] = qvel_global[i]
mutex.release()
trajectory.header.seq = i_seq
i_seq += 1
current_time = rospy.Time.now()
trajectory.header.stamp = current_time
trajectory.points = [JointTrajectoryPoint(velocities=qvel)]
pub.publish(trajectory)
rate.sleep()
except rospy.ROSInterruptException:
pass
def main():
rospy.init_node('send_joints')
pub = rospy.Publisher('/position_trajectory_controller/command',
JointTrajectory,
queue_size=10)
sub = rospy.Subscriber('/position_trajectory_controller/state',
JointTrajectoryControllerState,
_observation_callback,
queue_size=10)
# Create the topic message
traj = JointTrajectory()
traj.header = Header()
# Joint names for UR5
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rate = rospy.Rate(10)
while not rospy.is_shutdown():
traj.header.stamp = rospy.Time.now()
pts = JointTrajectoryPoint()
pts.positions = [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]
pts.time_from_start = rospy.Duration(0.1)
# Set the points to the trajectory
traj.points = []
traj.points.append(pts)
# Publish the message
pub.publish(traj)
def tuck(self):
return
rospy.loginfo('tuck_arm tuck')
# prepare a joint trajectory
msg = JointTrajectory()
msg.joint_names = servos
msg.points = list()
point = JointTrajectoryPoint()
point.positions = forward
point.velocities = [0.0 for servo in msg.joint_names]
point.time_from_start = rospy.Duration(5.0)
msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = to_side
point.velocities = [0.0 for servo in msg.joint_names]
point.time_from_start = rospy.Duration(8.0)
msg.points.append(point)
point = JointTrajectoryPoint()
point.positions = tucked
point.velocities = [0.0 for servo in msg.joint_names]
point.time_from_start = rospy.Duration(11.0)
msg.points.append(point)
# call action
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self._client.send_goal(goal)
def move_arm(self, points_list):
# # Unpause the physics
# rospy.wait_for_service('/gazebo/unpause_physics')
# unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
# unpause_gazebo()
self.client.wait_for_server()
goal = FollowJointTrajectoryGoal()
# We need to fill the goal message with its components
#
# check msg structure with: rosmsg info FollowJointTrajectoryGoal
# It is composed of 4 sub-messages:
# "trajectory" of type trajectory_msgs/JointTrajectory
# "path_tolerance" of type control_msgs/JointTolerance
# "goal_tolerance" of type control_msgs/JointTolerance
# "goal_time_tolerance" of type duration
trajectory_msg = JointTrajectory()
# check msg structure with: rosmsg info JointTrajectory
# It is composed of 3 sub-messages:
# "header" of type std_msgs/Header
# "joint_names" of type string
# "points" of type trajectory_msgs/JointTrajectoryPoint
trajectory_msg.joint_names = [
"j2n6s300_joint_1", "j2n6s300_joint_2", "j2n6s300_joint_3",
"j2n6s300_joint_4", "j2n6s300_joint_5", "j2n6s300_joint_6"
]
points_msg = JointTrajectoryPoint()
# check msg structure with: rosmsg info JointTrajectoryPoint
# It is composed of 5 sub-messages:
# "positions" of type float64
# "velocities" of type float64
# "accelerations" of type float64
# "efforts" of type float64
# "time_from_start" of type duration
points_msg.positions = [0, 0, 0, 0, 0, 0]
points_msg.velocities = [0, 0, 0, 0, 0, 0]
points_msg.accelerations = [0, 0, 0, 0, 0, 0]
points_msg.effort = [0, 1, 0, 0, 0, 0]
points_msg.time_from_start = rospy.Duration(0.01)
# fill in points message of the trajectory message
trajectory_msg.points = [points_msg]
# fill in trajectory message of the goal
goal.trajectory = trajectory_msg
print(trajectory_msg)
# self.client.send_goal_and_wait(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
rospy.sleep(2) # wait for 2s
def send_command(self, cmd):
rospy.logdebug(" Sending cmd to controller [%s]" %
self._controller_id)
cmd_msg = JointTrajectory()
cmd_msg.header.stamp = rospy.Time().now()
cmd_msg.joint_names = self._config["joint_names"]
cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
self._pub.publish(cmd_msg)
def head_up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [0]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(1.0)
jtm.points = [jtp]
return jtm
def arm2home_sim():
"""
Moves arm to home position in the simulation
"""
joint_command_pub = rospy.Publisher("/arm_controller/command",
JointTrajectory,
queue_size=10)
finger_pub = rospy.Publisher('/gripper_controller/command',
JointTrajectory,
queue_size=10)
joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
joint_traj = JointTrajectory()
joint_traj.joint_names = joint_names
joint_traj.points = [
JointTrajectoryPoint(
positions=[3 * pi / 4, -pi / 2, pi / 2, -pi / 2, -pi / 2, 0],
velocities=[0] * 6,
time_from_start=rospy.Duration(0.1))
]
finger_names = [
'finger_joint', 'left_inner_finger_joint', 'left_inner_knuckle_joint',
'right_inner_finger_joint', 'right_inner_knuckle_joint',
'right_outer_knuckle_joint'
]
close_level = 0
finger_angle = close_level * (0.77 / 5)
finger_traj = JointTrajectory()
# finger_traj.header = msg.header
finger_traj.joint_names = finger_names
finger_joint_config = np.array([1.0, 1.0, -1.0, 1.0, -1.0, -1.0
]) * finger_angle
finger_traj.points = [
JointTrajectoryPoint(positions=finger_joint_config,
velocities=[0] * 6,
time_from_start=rospy.Duration(3.0))
]
rospy.sleep(0.01)
for i in range(100):
joint_command_pub.publish(joint_traj)
finger_pub.publish(finger_traj)
rospy.sleep(0.01)
def test_controller(self):
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
topic = "/pr2/torso_controller/command"
rospy.init_node('test_pr2_torso_controller',
log_level=rospy.DEBUG,
disable_signals=True)
rospy.loginfo("Preparing to publish on %s" % topic)
ctl = rospy.Publisher(topic, JointTrajectory)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
duration = 0.1
traj = JointTrajectory()
traj.joint_name = "torso_lift_joint"
initialpoint = JointTrajectoryPoint()
initialpoint.positions = 0.0
initialpoint.velocities = 0.0
initialpoint.time_from_start = rospy.Duration(0.0)
finalpoint = JointTrajectoryPoint()
finalpoint.positions = 0.195
finalpoint.velocities = 0.0
finalpoint.time_from_start = rospy.Duration(duration)
# First, move up
traj.points = [initialpoint, finalpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.195)
# Go back to initial position
finalpoint.time_from_start = rospy.Duration(0.0)
initialpoint.time_from_start = rospy.Duration(duration)
traj.points = [finalpoint, initialpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
def random_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = 2*np.random.random(len(self.joint_names)) - 1
jtp.velocities = [0.]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(5.)
jtm.points = [jtp]
return jtm
def up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [1.]*len(self.joint_names)
jtp.velocities = [0.]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(5.0)
jtm.points = [jtp]
return jtm
def moveToJointPosition(self, req):
"""
Callback for the MoveToJointPosition service
Constructs a joint space trajectory plan that interpolates from the current position to
the desired position in joint space
:param req: robot_msgs.srv.MoveToJointPositionRequest
:type req:
:return:
:rtype:
"""
jointStateFinal = req.joint_state
jointStateStart = self.getRobotState()
rospy.loginfo("moving robot to joint position %s",
str(jointStateFinal.position))
# figure out the duration based on max joint degrees per second
print "jointStateStart \n", jointStateStart
print "jointStateFinal \n", jointStateFinal
print "jointStateStart.position = ", jointStateStart.position
q_start = np.array(jointStateStart.position)
q_end = np.array(jointStateFinal.position)
minPlanTime = 1.0
maxDeltaRadians = np.max(np.abs(q_end - q_start))
duration = maxDeltaRadians / np.deg2rad(
req.max_joint_degrees_per_second)
duration = max(duration, minPlanTime)
rospy.loginfo("rescaled plan duration is %.2f seconds", duration)
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
startTime = rospy.Duration.from_sec(0)
endTime = rospy.Duration.from_sec(duration)
startPoint = RobotMovementService.jointTrajectoryPointFromJointState(
jointStateStart, startTime)
endPoint = RobotMovementService.jointTrajectoryPointFromJointState(
jointStateFinal, endTime)
trajectory.points = [startPoint, endPoint]
trajectory.joint_names = self.jointNames
joint_traj_action_goal = robot_msgs.msg.JointTrajectoryGoal()
joint_traj_action_goal.trajectory = trajectory
self.joint_space_trajectory_action.send_goal(joint_traj_action_goal)
self.joint_space_trajectory_action.wait_for_result()
result = self.joint_space_trajectory_action.get_result()
finished_normally = (
result.status.status == result.status.FINISHED_NORMALLY)
return finished_normally
def up_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
jtp.positions = [self.filtered_angle]*len(self.joint_names)
print self.controller, self.filtered_angle
jtp.time_from_start = rospy.Duration(1.0)
jtm.points = [jtp]
return jtm
def test_controller(self):
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
topic = "/pr2/torso_controller/command"
rospy.init_node("test_pr2_torso_controller", log_level=rospy.DEBUG, disable_signals=True)
rospy.loginfo("Preparing to publish on %s" % topic)
ctl = rospy.Publisher(topic, JointTrajectory)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
duration = 0.1
traj = JointTrajectory()
traj.joint_name = "torso_lift_joint"
initialpoint = JointTrajectoryPoint()
initialpoint.positions = 0.0
initialpoint.velocities = 0.0
initialpoint.time_from_start = rospy.Duration(0.0)
finalpoint = JointTrajectoryPoint()
finalpoint.positions = 0.195
finalpoint.velocities = 0.0
finalpoint.time_from_start = rospy.Duration(duration)
# First, move up
traj.points = [initialpoint, finalpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.195)
# Go back to initial position
finalpoint.time_from_start = rospy.Duration(0.0)
initialpoint.time_from_start = rospy.Duration(duration)
traj.points = [finalpoint, initialpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
def make_joint_trajectory(names, points):
jt = JointTrajectory()
jt.joint_names = names
pt = JointTrajectoryPoint()
pt.positions = points
pt.effort = [0]*len(points)
pt.velocities = [0]*len(points)
pt.accelerations = [0]*len(points)
jt.points = [pt]
return jt
def send_msg(self): msg = JointTrajectory() msg.joint_names = self.arm_joint_names point = JointTrajectoryPoint() self.lock.acquire() point.positions = self.position self.lock.release() point.time_from_start = rospy.Duration(1.0) msg.points = [point] if not rospy.is_shutdown(): self.pub.publish(msg)
def jointTrajHelper(self, joint_names, positions):
# Setup the joint trajectory
jtm = JointTrajectory()
jtp = JointTrajectoryPoint()
jtm.joint_names = joint_names
jtp.time_from_start = rospy.Duration(1.0)
jtp.positions = [positions]
jtm.points = [jtp]
return jtm
def zliftCallback(self, msg):
# Setup the joint trajectory
jtm = JointTrajectory()
jtp = JointTrajectoryPoint()
jtm.joint_names = self.zlift_joint_names
jtp.time_from_start = rospy.Duration(1.0)
jtp.positions = [msg.position[0]*0.001] #Convert to meters
jtm.points = [jtp]
self.zlift_pub.publish(jtm)
def publish_trajectory(self, pub, joint_traj_positions): trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() trajectory.points = [JointTrajectoryPoint() for p in joint_traj_positions] for i,position in enumerate(joint_traj_positions): trajectory.points[i].positions = joint_traj_positions[i] trajectory.points[i].velocities = 0.0 trajectory.points[i].time_from_start = rospy.Duration(i*self.time_step) print "Publishing trajectory at ",trajectory.header.stamp pub.publish(trajectory)
def send_joint_angles(self,angles):
pub = rospy.Publisher("/fullbody_controller/command", JointTrajectory)
point = JointTrajectoryPoint()
point.positions = [ x * math.pi / 180.0 for x in angles ]
point.time_from_start = rospy.Duration(5.0)
msg = JointTrajectory()
msg.header.stamp = rospy.Time().now()
msg.joint_names = ["J1","J2","J3","J4","J5","J6","J7"]
msg.points = [point]
pub.publish(msg)
rospy.sleep(rospy.Duration(6.0))
def handle_head_pitch(self, pitch_degree):
self.head_pitch_cmd_deg = pitch_degree
self.head_pitch_pub.publish(data=math.radians(pitch_degree))
#Publish neck trajectory
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ['neck_ry']
trajectory.points = [JointTrajectoryPoint()]
trajectory.points[0].positions = [math.radians(pitch_degree)]
trajectory.points[0].velocities = [0.0]
trajectory.points[0].time_from_start = rospy.Duration(0.75)
self.head_trajectory_pub.publish(trajectory)
def publish_robot_joint_state(self):
print "publishing new joint states for " + self.topic + " to Atlas"
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = [joint.name for joint in self.joints]
joint_trajectory.points = [JointTrajectoryPoint()]
# joint_trajectory.points[0].positions = [joint.current_position for joint in self.joints]
joint_trajectory.points[0].positions = [joint.position for joint in self.joints]
# joint_trajectory.points[0].time_from_start = rospy.Duration(0.000)
joint_trajectory.points[0].time_from_start = rospy.Duration(self.duration)
# joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.25)
# print joint_trajectory
self.pub_robot.publish(joint_trajectory)
def sendRightArmTrajectory():
msg = JointTrajectory()
msg.joint_names = RIGHT_NAMES
trajectoryPoints = [createTrajectoryPoint(2.0, FLYING_RIGHT),
createTrajectoryPoint(4.0, DOWN_RIGHT),
createTrajectoryPoint(6.0, FLYING_RIGHT),
createTrajectoryPoint(8.0, DOWN_RIGHT),
createTrajectoryPoint(10.0, HOME_RIGHT)]
msg.points = trajectoryPoints
rospy.loginfo( 'publishing right trajectory')
armTrajectoryPublisher.publish(msg)
def send_cmd_msg(self):
jtm = JointTrajectory()
jtm.joint_names = self.joint_names
jtp = JointTrajectoryPoint()
# Check if we're dealing with one single float value
if len(self.joint_names) > 1:
jtp.positions = self.position
else:
jtp.positions = [self.position]*len(self.joint_names)
jtp.time_from_start = rospy.Duration(1.0)
jtm.points = [jtp]
return jtm
def publishThumbPosition(self, hand, position):
'''
Publish for single value (thumb)
'''
jtm = JointTrajectory()
jtp = JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(1.0)
jtp.positions = [position]
jtm.points = [jtp]
if hand == MekaControllerConverter.RIGHT_HAND:
jtm.joint_names = ['right_hand_j0']
else:
jtm.joint_names = ['left_hand_j0']
self.hand_thumb_publishers[hand].publish(jtm)
def publishRobotPelvisPose(self):
print "publishing new pelvis pose ..."
bdi_pelvis_pose = PoseStamped()
bdi_pelvis_pose.header.stamp = rospy.Time.now()
bdi_pelvis_pose.pose.position.x = self.forward_position
bdi_pelvis_pose.pose.position.y = self.lateral_position
bdi_pelvis_pose.pose.position.z = self.height_position
# Use BDI yaw*roll*pitch concatenation
xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
Rx = tf.transformations.rotation_matrix(self.roll_position, xaxis)
Ry = tf.transformations.rotation_matrix(self.pitch_position, yaxis)
Rz = tf.transformations.rotation_matrix(self.yaw_position, zaxis)
R = tf.transformations.concatenate_matrices(Rz, Rx, Ry)
q = tf.transformations.quaternion_from_matrix(R)
bdi_pelvis_pose.pose.orientation.w = q[3]
bdi_pelvis_pose.pose.orientation.x = q[0]
bdi_pelvis_pose.pose.orientation.y = q[1]
bdi_pelvis_pose.pose.orientation.z = q[2]
# w = math.cos(self.yaw_position*0.5)
# bdi_pelvis_pose.pose.orientation.x = 0.0
# bdi_pelvis_pose.pose.orientation.y = 0.0
# bdi_pelvis_pose.pose.orientation.z = math.sin(self.yaw_position*0.5)
print bdi_pelvis_pose
print q
euler = tf.transformations.euler_from_quaternion(q)
print euler
self.pub_robot.publish(bdi_pelvis_pose)
# Now publish the trajectory form for the new controllers
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ["com_v1", "com_v0", "pelvis_height", "pelvis_roll", "pelvis_pitch", "pelvis_yaw"]
trajectory.points = [JointTrajectoryPoint()]
trajectory.points[0].positions = [
self.lateral_position,
self.forward_position,
self.height_position,
self.roll_position,
self.pitch_position,
self.yaw_position,
]
trajectory.points[0].velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
trajectory.points[0].time_from_start = rospy.Duration(0.75)
self.pelvis_trajectory_pub.publish(trajectory)
def look_at_board(self):
msg = JointTrajectory()
msg.joint_names = self.joints
msg.points = list()
point = JointTrajectoryPoint()
point.positions = [0.0, self.home_tilt]
point.time_from_start = rospy.Duration(3.0)
msg.points.append(point)
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self._client.send_goal(goal)
def publish_jvrc1_jointtrajectory(jname='NECK_Y', jdegree=0):
rad = float('{0:.4f}'.format(math.radians(float(jdegree))))
pub = rospy.Publisher('/JVRC_1/torque_control/set_joint_trajectory', JointTrajectory, latch=True, queue_size=10)
rospy.init_node('jvrc1_choreonoid_ros_test', anonymous=True)
msg = JointTrajectory()
msg.joint_names = [ jname ]
msg.points = []
p = JointTrajectoryPoint()
p.time_from_start = rospy.rostime.Duration(0)
p.positions = [ rad ]
msg.points.append(p)
rospy.loginfo(msg)
pub.publish(msg)
rospy.Rate(1).sleep()
def get2PointTrajectory( self, positions, time_from_start ):
if self.positions is None or positions is None:
return None
jt = JointTrajectory()
jt.joint_names = self.joint_names
jt.points = []
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_seconds( 0.0 )
jp.positions = self.positions
jt.points.append( jp )
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_seconds( time_from_start )
jp.positions = positions
jt.points.append( jp )
jt.header.stamp = rospy.Time.now()
return jt
def actionCb(self, req):
goal = FollowJointTrajectoryGoal()
msg = JointTrajectory()
msg.joint_names = self.joints
msg.points = list()
point = JointTrajectoryPoint()
point.positions = [ 0.0 for servo in msg.joint_names ]
point.velocities = [ 0.0 for servo in msg.joint_names ]
point.time_from_start = rospy.Duration(2.0)
msg.points.append(point)
msg.header.stamp = rospy.Time.now() + rospy.Duration(0.01)
goal.trajectory = msg
self.client.send_goal(goal)
self.client.wait_for_result()
self.server.set_succeeded( ResetArmResult() )
def humanoidCallback(self, msg):
'''
When callback receives a humanoid control command
convert it into the proper controllers (arms, head, etc.)
for gazebo
'''
trajectory_store = dict()
# Initialize the dictionary for all of the parts
for part in self.joint_controllers:
jtm = JointTrajectory()
jtp = JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(1.0)
jtp.positions = []
jtm.points = [jtp]
trajectory_store[part] = jtm
chain_values = []
# Go through the message and fill in each command separately
for i in range(len(msg.chain)):
chain_num = ord(msg.chain[i])
chain_values.append(chain_num)
jtm = trajectory_store[chain_num]
jtm.points[0].positions.append(msg.position[i])
trajectory_store[chain_num] = jtm
# Figure out the unique joints that were called
sent_controllers = np.unique(chain_values)
# Go through the actual unique commands and populate the fields
for chain_cmd in sent_controllers:
jtm = trajectory_store[chain_cmd]
jtm.joint_names = get_param(self.joint_controllers[chain_cmd]+'joints','')
# Special case for fingers - add zeros to command
if chain_cmd == MekaControllerConverter.RIGHT_HAND or chain_cmd == MekaControllerConverter.LEFT_HAND:
jtm.points[0].positions.extend([0.0]*(len(jtm.joint_names)-len(jtm.points[0].positions)))
trajectory_store[chain_cmd] = jtm
# Only go through the controllers that were actually sent
for part in sent_controllers:
# Get the actual controller name
controller = self.joint_controllers[part]
pub = self.publishers[controller]
pub.publish(trajectory_store[part])
def talker():
pub_traj = rospy.Publisher('joint_path_command', JointTrajectory)
rospy.init_node('path_publisher')
traj = JointTrajectory()
traj.header = Header(frame_id='base_link', stamp=rospy.Time.now() + rospy.Duration(1.0))
traj.joint_names = ['joint_1', 'joint_2', 'joint_3',
'joint_4', 'joint_5', 'joint_6']
traj.points = [JointTrajectoryPoint(positions=[0, 0, 0, 0, 0, 0],
velocities=[0, 0, 0, 0, 0, 0],
time_from_start=rospy.Duration(1)),
JointTrajectoryPoint(positions=[1, 0, 0, 0, 0, 0],
velocities=[0, 0, 0, 0, 0, 0])]
rospy.loginfo(traj)
pub_traj.publish(traj)
rospy.sleep(1.0)
def jointtrajectory_publisher():
pub = rospy.Publisher('/HRP1/set_joint_trajectory', JointTrajectory, queue_size=10)
rospy.init_node('pa10test', anonymous=True)
r = rospy.Rate(0.1)
while not rospy.is_shutdown():
now = rospy.get_time()
msg = JointTrajectory()
msg.joint_names = ["J1", "J2"]
msg.points = []
for i in [0, 1]:
p = JointTrajectoryPoint()
p.time_from_start = rospy.rostime.Duration(2)
p.positions = [i, i]
msg.points.append(p)
rospy.loginfo("hello world %s" % now)
pub.publish(msg)
r.sleep()
def move_arm( positions, time_from_start = 3.0, arm = 'r', client = None ):
if (client == None):
client = init_jt_client(arm)
else:
arm = client.action_client.ns[0:1]; # ignore arm argument
rospy.loginfo( 'move arm \'%s\' to position %s'%( arm, positions ) )
joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )
jt = JointTrajectory()
jt.joint_names = joint_names
jt.points = []
jp = JointTrajectoryPoint()
jp.time_from_start = rospy.Time.from_seconds( time_from_start )
jp.positions = positions
jt.points.append( jp )
# push trajectory goal to ActionServer
jt_goal = JointTrajectoryGoal()
jt_goal.trajectory = jt
jt_goal.trajectory.header.stamp = rospy.Time.now() # + rospy.Duration.from_sec( time_from_start )
client.send_goal( jt_goal )
client.wait_for_result()
return client.get_state()
