def command_joint_position(self, desired_pose):
"""
Command a specific desired hand pose.
The desired pose must be the correct dimensionality (self._num_joints).
Only the pose is commanded, and **no bound-checking happens here**:
any commanded pose must be valid or Bad Things May Happen. (Generally,
values between 0.0 and 1.5 are fine, but use this at your own risk.)
:param desired_pose: The desired joint configurations.
:return: True if pose is published, False otherwise.
"""
# Check that the desired pose can have len() applied to it, and that
# the number of dimensions is the same as the number of hand joints.
if (not hasattr(desired_pose, '__len__') or
len(desired_pose) != self._num_joints):
rospy.logwarn('Desired pose must be a {}-d array: got {}.'
.format(self._num_joints, desired_pose))
return False
msg = JointState() # Create and publish
try:
msg.position = desired_pose
self.pub_joint.publish(msg)
rospy.logdebug('Published desired pose.')
return True
except rospy.exceptions.ROSSerializationException:
rospy.logwarn('Incorrect type for desired pose: {}.'.format(
desired_pose))
return False
def gripper_cb(self, data):
js = JointState()
js.header = data.header
js.name = ["Gripper"]
js.position = [data.current_pos]
js.velocity = [data.velocity]
self.gripper_pub.publish(js)
def handstate_callback(handstate_msg):
assert len(handstate_msg.positions) == 4
assert len(handstate_msg.inner_links) == 3
assert len(handstate_msg.outer_links) == 3
jointstate_msg = JointState()
jointstate_msg.header.stamp = handstate_msg.header.stamp
jointstate_msg.name = [
joint_prefix + '00',
# Joint j01 is a mimic joint that is published by robot_state_publisher.
joint_prefix + '01',
joint_prefix + '11',
joint_prefix + '21',
# TODO: These are not currently being published. See below.
#joint_prefix + '02',
#joint_prefix + '12',
#joint_prefix + '22',
]
jointstate_msg.position += [ handstate_msg.positions[3] ]
jointstate_msg.position += handstate_msg.inner_links
# TODO: These positions look very wrong, so we'll let robot_state_publisher
# use the mimic ratios in HERB's URDF to compute the distal joint angles.
#jointstate_msg.position += handstate_msg.outer_links
jointstate_pub.publish(jointstate_msg)
def command_joint_torques(self, desired_torques):
"""
Command a desired torque for each joint.
The desired torque must be the correct dimensionality
(self._num_joints). Similarly to poses, we do not sanity-check
the inputs. As a rule of thumb, values between +- 0.5 are fine.
:param desired_torques: The desired joint torques.
:return: True if message is published, False otherwise.
"""
# Check that the desired torque vector can have len() applied to it,
# and that the number of dimensions is the same as the number of
# joints. This prevents passing singletons or incorrectly-shaped lists
# to the message creation (which does no checking).
if (not hasattr(desired_torques, '__len__') or
len(desired_torques) != self._num_joints):
rospy.logwarn('Desired torques must be a {}-d array: got {}.'
.format(self._num_joints, desired_torques))
return False
msg = JointState() # Create and publish
try:
msg.effort = desired_torques
self.pub_joint.publish(msg)
rospy.logdebug('Published desired torques.')
return True
except rospy.exceptions.ROSSerializationException:
rospy.logwarn('Incorrect type for desired torques: {}.'.format(
desired_torques))
return False
def joint_trajectory_to_joint_state(self, joint_trajectory_msg, index):
joint_states_msg = JointState()
joint_states_msg.name = joint_trajectory_msg.joint_names
joint_states_msg.position = joint_trajectory_msg.points[index].positions
joint_states_msg.velocity = joint_trajectory_msg.points[index].velocities
joint_states_msg.effort = joint_trajectory_msg.points[index].effort
return joint_states_msg
def sorted_joint_state_msg(msg, joint_names):
"""
Returns a sorted C{sensor_msgs/JointState} for the given joint names
@type msg: sensor_msgs/JointState
@param msg: The input message
@type joint_names: list
@param joint_names: The sorted joint names
@rtype: sensor_msgs/JointState
@return: The C{JointState} message with the fields in the order given by joint names
"""
valid_names = set(joint_names).intersection(set(msg.name))
valid_position = len(msg.name) == len(msg.position)
valid_velocity = len(msg.name) == len(msg.velocity)
valid_effort = len(msg.name) == len(msg.effort)
num_joints = len(valid_names)
retmsg = JointState()
retmsg.header = copy.deepcopy(msg.header)
for name in joint_names:
if name not in valid_names:
continue
idx = msg.name.index(name)
retmsg.name.append(name)
if valid_position:
retmsg.position.append(msg.position[idx])
if valid_velocity:
retmsg.velocity.append(msg.velocity[idx])
if valid_effort:
retmsg.effort.append(msg.effort[idx])
return retmsg
def execute(self, goal):
#pickle.dump(goal, open("trajecoryGoalMsg3.data", "wb"))
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
num_points = len(trajectory_points)
rospy.loginfo("Received %i trajectory points" % num_points)
pprint(goal.trajectory)
end_time = trajectory_points[-1].time_from_start.to_sec()
control_rate = rospy.Rate(self._control_rate)
i=num_points
rospy.loginfo("the num of point is %i" % i )
self.jointTrajectoryPublisher.publish(goal.trajectory)
for trajectory_point in trajectory_points:
js = JointState()
#rospy.loginfo("the length of js.name is %i" %len(js.name))
js.name=copy.deepcopy(joint_names)
for j in range(len(joint_names)):
#rospy.loginfo("j is %i" % j)
js.name[j] = js.name[j]+'_'+str(i)
i=i-1
#js.position = trajectory_point.positions
#js.velocity=trajectory_point.velocities
#self.jointStatePublisher.publish(js)
control_rate.sleep()
start_time = rospy.get_time()
now_from_start = rospy.get_time() - start_time
last_idx = -1
self._result.error_code = self._result.SUCCESSFUL
self.server.set_succeeded(self._result)
def publish_position():
'''joint position publisher'''
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = JOINT_NAME_ARRAY
msg.position = joints_pos
pub_js.publish(msg)
def eyelidServosPut(self,params):
msg = JointState()
msg.name = list()
msg.position = list()
msg.velocity = list()
msg.effort = list()
if params.has_key('leftEyelid_position'):
msg.name.append('left_eyelid_joint')
msg.position.append(math.radians(float(params['leftEyelid_position'])-float(90)))
if params.has_key('leftEyelid_speed'):
#Anadir velocuidad
msg.velocity.append(params['leftEyelid_speed'])
else:
msg.velocity.append(3.0)
if params.has_key('rightEyelid_position'):
msg.name.append('right_eyelid_joint')
msg.position.append(math.radians(float(params['rightEyelid_position'])-float(90)))
if params.has_key('rightEyelid_speed'):
#Anadir velocuidad
msg.velocity.append(params['rightEyelid_speed'])
else:
msg.velocity.append(3.0)
msg.header.stamp = rospy.Time.now()
self.cmd_joints_pub.publish(msg)
def test_fk_easy4(self):
chain_state = JointState()
chain_state.position = [0, 0, 0]
eef = self.chain.fk(chain_state, -1)
print eef
eef_expected = numpy.matrix([[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 2], [0, 0, 0, 1]])
self.assertAlmostEqual(numpy.linalg.norm(eef - eef_expected), 0.0, 6)
def generateJointStatesFromRegisterStateAndPublish():
rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, updateLocalJointState)
global currentJointState
currentJointState = JointState()
currentJointState.header.frame_id = "robotiq_s_model from real-time state data"
currentJointState.name = ["finger_1_joint_1",
"finger_1_joint_2",
"finger_1_joint_3",
"finger_2_joint_1",
"finger_2_joint_2",
"finger_2_joint_3",
"finger_middle_joint_1",
"finger_middle_joint_2",
"finger_middle_joint_3",
"palm_finger_1_joint",
"palm_finger_2_joint"]
currentJointState.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
currentJointState.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
currentJointState.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pub = rospy.Publisher('joint_states', JointState, queue_size=1) #no buffering?
rospy.init_node('robotiq_s_model_joint_state_publisher', anonymous=False)
rate = rospy.Rate(100) # publishes at 100Hz
while not rospy.is_shutdown():
currentJointState.header.stamp = rospy.Time.now()
pub.publish(currentJointState)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def js_cb(self, a):
rospy.loginfo('Received array')
js = JointState()
js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
jList = a.data
jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
i = 0
for pos in jMatrix:
rospy.loginfo(pos)
js.position = pos
gsvr = GetStateValidityRequest()
rs = RobotState()
rs.joint_state = js
gsvr.robot_state = rs
gsvr.group_name = "both_arms"
resp = self.coll_client(gsvr)
if (resp.valid == False):
rospy.loginfo('false')
rospy.loginfo(i)
self.js_pub.publish(0)
return
i = i + 1
self.js_pub.publish(1)
rospy.loginfo('true')
return
def set_stiffness(self):
stiffness_message = JointState()
stiffness_message.name = ["HeadYaw", "HeadPitch"]
stiffness_message.position = [0,0]
stiffness_message.velocity = [0,0]
stiffness_message.effort = [1,1]
self.stiffness_publisher.publish(stiffness_message)
def loop(self):
hz = 20
r = rospy.Rate(hz)
msg = JointState()
for i in range(8):
msg.name.append("q"+str(i+1))
while not rospy.is_shutdown() and not self.endthread:
br = tf.TransformBroadcaster()
msg.header.stamp=rospy.Time.now()
msg.position = 8 * [0.0]
msg.effort = 8 * [0.0]
self.torque[6]=0.1
for i in range(8):
msg.position[i]=self.q_desired[i]
msg.effort[i]=self.torque[i]
if(self.publishInfo):
self.publisher.publish(msg)
if(self.publishDesiredInfo):
br.sendTransform((self.wTp_desired[0],
self.wTp_desired[1],
self.wTp_desired[2]),
tf.transformations.quaternion_from_euler(self.wTp_desired[3],
self.wTp_desired[4],
self.wTp_desired[5]),
rospy.Time.now(),
self.child_frame,
self.parent_frame)
# Add in the tf transform now
self.master.update_idletasks()
r.sleep()
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
def resethead(): # rotate the head(not PTU)
pub = rospy.Publisher('/head/commanded_state', JointState)
head_command = JointState()
head_command.name=["HeadPan"]
head_command.position=[0.0] #forwards
pub.publish(head_command)
eyeplay(0)# reset eyes
def head_wander():
rospy.init_node('head_wander')
joints_pub = rospy.Publisher('/cmd_joints', JointState)
say = rospy.ServiceProxy('/qbo_talk/festival_say', Text2Speach)
seq = 0
while not rospy.is_shutdown():
seq += 1
msg = JointState()
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_footprint'
msg.name = ['head_pan_joint','head_tilt_joint']
msg.position = [random.uniform(-0.9,0.9), random.uniform(-0.8,0.3)]
joints_pub.publish(msg)
if random.random() < 0.3:
try:
say(random.choice(UTTERANCES))
except rospy.ServiceException:
# Ignore errors
pass
rospy.sleep(2.0)
def publishJointState(self):
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.jointNames
msg.position = self.getJointAngles()
msg.effort = self.getLoads()
self.jointStatePub.publish(msg)
def talker():
joint1 = radians(90)
pub = rospy.Publisher('/kukaAllegroHand/robot_cmd', JointState)
rospy.init_node('talker', anonymous=True)
r = rospy.Rate(10000) # 10hz
msg = JointState()
msg.position = [0,joint1,0,0,0,0,0]
pub.publish(msg)
r.sleep()
time.sleep(.4)
while True:
for acc in range(600,601):
print '************************* acceleratioin: %d *************************' % acc
# acc = 200
trjFile = '../trj/acc_new_' + str(acc) + '.dat'
trj = np.genfromtxt(trjFile, delimiter="\t")
done = True
i = 1
while not rospy.is_shutdown() and i < len(trj):
print i
msg = JointState()
msg.position = [0,joint1,0,0,0,0,0]
msg.position[0] = radians(trj[i][0])
msg.position[2] = radians(trj[i][0])
msg.position[3] = radians(trj[i][0])
msg.position[4] = radians(trj[i][0])
msg.position[5] = radians(trj[i][0])
msg.position[6] = radians(trj[i][0])
pub.publish(msg)
r.sleep()
time.sleep((trj[i][2] - trj[i-1][2])/10)
i += 1
def generate_views(self):
rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
try:
resp = self.nav_goals(10, self.inflation_radius, self.roi)
if not resp.goals.poses:
self.views = []
return
for j in range(len(resp.goals.poses)):
for i in range(0,self.views_at_pose):
pose = resp.goals.poses[j]
pan = random.uniform(self.min_pan, self.max_pan)
tilt = random.uniform(self.min_tilt, self.max_tilt)
jointstate = JointState()
jointstate.name = ['pan', 'tilt']
jointstate.position = [pan, tilt]
jointstate.velocity = [self.velocity, self.velocity]
jointstate.effort = [float(1.0), float(1.0)]
resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
p = Pose()
view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
self.views.append(view)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def update(self):
if not self.driver_status == 'DISCONNECTED':
# Get Joint Positions
self.current_joint_positions = self.rob.getj()
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "robot_secondary_interface_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0]*6
msg.effort = [0]*6
self.joint_state_publisher.publish(msg)
# Get TCP Position
tcp_angle_axis = self.rob.getl()
# Create Frame from XYZ and Angle Axis
T = PyKDL.Frame()
axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
# Get norm and normalized axis
angle = axis.Normalize()
# Make frame
T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
T.M = PyKDL.Rotation.Rot(axis,angle)
# Create Pose
self.current_tcp_pose = tf_c.toMsg(T)
self.current_tcp_frame = T
self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def default(self, ci="unused"):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
js.name = [
"kuka_arm_0_joint",
"kuka_arm_1_joint",
"kuka_arm_2_joint",
"kuka_arm_3_joint",
"kuka_arm_4_joint",
"kuka_arm_5_joint",
"kuka_arm_6_joint",
"head_pan_joint",
"head_tilt_joint",
]
js.position = [
self.data["seg0"],
self.data["seg1"],
self.data["seg2"],
self.data["seg3"],
self.data["seg4"],
self.data["seg5"],
self.data["seg6"],
self.data["pan"],
self.data["tilt"],
]
# js.velocity = [1, 1, 1, 1, 1, 1, 1]
# js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def _move_to(self, point, dur):
rospy.sleep(dur)
msg = JointState()
with self.lock:
if not self.sig_stop:
self.joint_positions = point.positions[:]
self.joint_velocities = point.velocities[:]
msg.name = ['joint_1',
'joint_2',
'joint_3',
'joint_4',
'joint_5',
'joint_6',
'joint_7']
#msg.name = self.joint_names
msg.position = self.joint_positions
msg.velocity = self.joint_velocities #[0.2]*7
msg.header.stamp = rospy.Time.now()
self.joint_command_pub.publish(msg)
#rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur))
else:
rospy.loginfo('Stoping motion immediately, clearing stop signal')
self.sig_stop = False
def joint_state_publisher(self):
try:
joint_state_msg = JointState()
joint_fb_msg = FollowJointTrajectoryFeedback()
time = rospy.Time.now()
with self.lock:
#Joint states
joint_state_msg.header.stamp = time
joint_state_msg.name = self.joint_names
joint_state_msg.position = self.motion_ctrl.get_joint_positions()
#self.joint_state_pub.publish(joint_state_msg)
#Joint feedback
joint_fb_msg.header.stamp = time
joint_fb_msg.joint_names = self.joint_names
joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
#self.joint_feedback_pub.publish(joint_fb_msg)
except Exception as e:
rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def arm_joint_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm in the current planning scene state or the passed in state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
A sensor_msgs.msg.JointState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
if not robot_state:
robot_state = self._psi.get_robot_state()
joint_state = JointState()
joint_state.header = robot_state.joint_state.header
for n in self.joint_names:
found = False
for i in range(len(robot_state.joint_state.name)):
if robot_state.joint_state.name[i] == n:
joint_state.name.append(n)
joint_state.position.append(robot_state.joint_state.position[i])
found = True
break
if not found and fail_if_joint_not_found:
raise ValueError('Joint '+n+' is missing from robot state')
return joint_state
def rc_talker():
'''init pyxhook'''
#Create hookmanager
hookman = pyxhook.HookManager()
#Define our callback to fire when a key is pressed down
hookman.KeyDown = key_down_event
hookman.KeyUp = key_up_event
#Hook the keyboard
hookman.HookKeyboard()
#Start our listener
hookman.start()
pub = rospy.Publisher('rc_sender', String, queue_size=10)
# 2016-10-26 test urdf joint control
# controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch
#roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True
pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10)
rospy.init_node('rc_talker')
rate = rospy.Rate(50)
while not rospy.is_shutdown():
#hello_str = "hello world %s" % rospy.get_time()
pub.publish(''.join(rc_keys))
# 2016-10-26 test urdf joint control
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ['base_to_left_arm', 'base_to_right_arm']
msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])]
pub_2.publish(msg)
#print rc_keys
rate.sleep()
if running == False:
#Close the listener when we are done
hookman.cancel()
break
def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list):
if len(jointsToSweep) != 0:
for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping
currPositions[joint_list.index(jointsToSweep[0])]
currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])])
if (len(jointsToSweep) > 1):
self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list)
# Combine message and publish
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.joint_list
# print self.free_joints
msg.position = currPositions
msg.velocity = [0.0]*len(maxs)
msg.effort = [0.0]*len(maxs)
time.sleep(0.0001)
self.pub.publish(msg)
# Get End Effector Positions
rate = rospy.Rate(20.0)
try:
(trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0))
if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]):
self.csvWriter.writerow(trans)
self.currX = trans[0]
self.currY = trans[1]
self.currZ = trans[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "ERROR: looking up tf"
time.sleep(0.5) # Sleep for a bit to let system catch up
continue
def build_joint_state_msg(self):
"""
@brief JointState message builder.
Builds a JointState message with the current position of the finger
joints and its names.
"""
js_msg = JointState()
js_msg.header.stamp = rospy.Time.now()
if self.joint_names == []:
self.joint_names = ["{}.{}".format('hand', attr)
for attr in ORI_ATTRIBUTES] + \
["{}.{}.{}".format(finger, bone, ori)
for finger in FINGER_NAMES
for bone in FINGER_BONES
for ori in ORI_ATTRIBUTES]
LOG.v("Publishing JointState for the following joints: {}".format(self.joint_names), "start_transmit")
js_msg.position = [0.0] * len(self.joint_names)
pos = 0
# Build JointState. First the hand...
for i, attr in enumerate(ORI_ATTRIBUTES):
js_msg.name.append('hand.' + str(attr))
# Roll precision hack
if attr == 'roll':
vector = self.hand.palm_normal
else:
vector = self.hand.direction
js_msg.position[pos] = getattr(vector, attr)
pos += 1
# ...then the fingers
for i, finger_name, finger in \
[(i, finger_name, self.fingers[finger_name]) \
for i, finger_name in enumerate(FINGER_NAMES)]:
# LEAP API v2.0: Skeletal model
# Get bones
for j, bone_name, bone in \
[(j, bone_name, finger.bone(j)) \
for j, bone_name in enumerate(FINGER_BONES)]:
# Fill the joint values one by one
for k, attr in enumerate(ORI_ATTRIBUTES):
joint_name = "{}.{}.{}".format(finger_name, bone_name, attr)
joint_value = getattr(bone.direction, attr)
js_msg.name.append(joint_name)
js_msg.position[pos] = joint_value
pos += 1
# return the JointState message
return js_msg
def get_new_JointState(pan, tilt): j = JointState() j.header.stamp = rospy.Time.now() j.name = ["panservo", "tiltservo"] j.position = [pan, tilt] j.velocity = [] j.effort = [] return j
def __init__(self):
global jspr2msg
jspr2msg = JointState()
jspr2msg.name = [ "left_shoulder_tilt_joint", "left_lin_act_cyl_joint", "left_upper_arm_hinge_joint","left_linear_actuator_joint", "right_shoulder_tilt_joint", "right_lin_act_cyl_joint", "right_upper_arm_hinge_joint","right_linear_actuator_joint", "torso_lift_joint", "wheel_linear_actuator_joint", "fl_anchor_rod_joint", "fl_push_rod_joint", "fl_caster_rotation_joint", "fr_anchor_rod_joint", "fr_push_rod_joint", "fr_caster_rotation_joint", "bl_anchor_rod_joint", "bl_push_rod_joint", "bl_caster_rotation_joint", "br_anchor_rod_joint", "br_push_rod_joint", "br_caster_rotation_joint"]
jspr2msg.position = [0.0 for name in jspr2msg.name]
jspr2msg.velocity = [0.0 for name in jspr2msg.name]
rospy.loginfo("done init")
def _ros_init(self):
# Can check log msgs according to log_level {rospy.DEBUG, rospy.INFO, rospy.WARN, rospy.ERROR}
rospy.init_node('RLkitUR', anonymous=True, log_level=rospy.DEBUG)
rospy.logdebug("Starting RLkitUR Class object...")
# Init GAZEBO Objects
self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.get_world_state = rospy.ServiceProxy(
'/gazebo/get_world_properties', GetWorldProperties)
# Subscribe joint state and target pose
rospy.Subscriber("/joint_states", JointState,
self.joints_state_callback)
rospy.Subscriber("/target_blocks_pose", Point,
self.target_point_callback)
rospy.Subscriber("/gazebo/link_states", LinkStates,
self.link_state_callback)
rospy.Subscriber("/collision_status", Bool, self.collision_status)
# Gets training parameters from param server
self.desired_pose = Pose()
self.running_step = rospy.get_param("/running_step")
self.max_height = rospy.get_param("/max_height")
self.min_height = rospy.get_param("/min_height")
self.observations = rospy.get_param("/observations")
self.joint_names = rospy.get_param("/joint_names")
# Joint limitation
shp_max = rospy.get_param("/joint_limits_array/shp_max")
shp_min = rospy.get_param("/joint_limits_array/shp_min")
shl_max = rospy.get_param("/joint_limits_array/shl_max")
shl_min = rospy.get_param("/joint_limits_array/shl_min")
elb_max = rospy.get_param("/joint_limits_array/elb_max")
elb_min = rospy.get_param("/joint_limits_array/elb_min")
wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
self.joint_limits = {
"shp_max": shp_max,
"shp_min": shp_min,
"shl_max": shl_max,
"shl_min": shl_min,
"elb_max": elb_max,
"elb_min": elb_min,
"wr1_max": wr1_max,
"wr1_min": wr1_min,
"wr2_max": wr2_max,
"wr2_min": wr2_min,
"wr3_max": wr3_max,
"wr3_min": wr3_min
}
# Joint Velocity limitation
shp_vel_max = rospy.get_param("/joint_velocity_limits_array/shp_max")
shp_vel_min = rospy.get_param("/joint_velocity_limits_array/shp_min")
shl_vel_max = rospy.get_param("/joint_velocity_limits_array/shl_max")
shl_vel_min = rospy.get_param("/joint_velocity_limits_array/shl_min")
elb_vel_max = rospy.get_param("/joint_velocity_limits_array/elb_max")
elb_vel_min = rospy.get_param("/joint_velocity_limits_array/elb_min")
wr1_vel_max = rospy.get_param("/joint_velocity_limits_array/wr1_max")
wr1_vel_min = rospy.get_param("/joint_velocity_limits_array/wr1_min")
wr2_vel_max = rospy.get_param("/joint_velocity_limits_array/wr2_max")
wr2_vel_min = rospy.get_param("/joint_velocity_limits_array/wr2_min")
wr3_vel_max = rospy.get_param("/joint_velocity_limits_array/wr3_max")
wr3_vel_min = rospy.get_param("/joint_velocity_limits_array/wr3_min")
self.joint_velocty_limits = {
"shp_vel_max": shp_vel_max,
"shp_vel_min": shp_vel_min,
"shl_vel_max": shl_vel_max,
"shl_vel_min": shl_vel_min,
"elb_vel_max": elb_vel_max,
"elb_vel_min": elb_vel_min,
"wr1_vel_max": wr1_vel_max,
"wr1_vel_min": wr1_vel_min,
"wr2_vel_max": wr2_vel_max,
"wr2_vel_min": wr2_vel_min,
"wr3_vel_max": wr3_vel_max,
"wr3_vel_min": wr3_vel_min
}
# Init joint pose
shp_init_value = rospy.get_param("/init_joint_pose/shp")
shl_init_value = rospy.get_param("/init_joint_pose/shl")
elb_init_value = rospy.get_param("/init_joint_pose/elb")
wr1_init_value = rospy.get_param("/init_joint_pose/wr1")
wr2_init_value = rospy.get_param("/init_joint_pose/wr2")
wr3_init_value = rospy.get_param("/init_joint_pose/wr3")
self.init_joint_angles = [
shp_init_value, shl_init_value, elb_init_value, wr1_init_value,
wr2_init_value, wr3_init_value
]
# 3D coordinate limits
x_max = rospy.get_param("/cartesian_limits/x_max")
x_min = rospy.get_param("/cartesian_limits/x_min")
y_max = rospy.get_param("/cartesian_limits/y_max")
y_min = rospy.get_param("/cartesian_limits/y_min")
z_max = rospy.get_param("/cartesian_limits/z_max")
z_min = rospy.get_param("/cartesian_limits/z_min")
self.xyz_limits = {
"x_max": x_max,
"x_min": shp_vel_min,
"y_max": y_max,
"y_min": y_min,
"z_max": z_max,
"z_min": z_min
}
# Fill in the Done Episode Criteria list
self.episode_done_criteria = rospy.get_param("/episode_done_criteria")
# Stablishes connection with simulator
self._gz_conn = GazeboConnection()
self._ctrl_conn = ControllersConnection(namespace="")
# Controller type for ros_control
self.current_controller_type = rospy.get_param("/control_type")
self.pre_controller_type = self.current_controller_type
# Init the observations, target, ...
self.base_orientation = Quaternion()
self.target_point = Point()
self.link_state = LinkStates()
self.joints_state = JointState()
self.end_effector = Point()
self.distance = 0.
# Arm/Control parameters
self._ik_params = setups['UR5_6dof']['ik_params']
# ROS msg type
self._joint_pubisher = JointPub()
self._joint_traj_pubisher = JointTrajPub()
self.reset_publisher = rospy.Publisher("/ur/reset",
String,
queue_size=1)
self.target_point = Point()
self._target_point_pubisher = rospy.Publisher("/target_goal",
Point,
queue_size=1)
# Controller list
self.vel_traj_controller = [
'joint_state_controller', 'gripper_controller',
'vel_traj_controller'
]
self.pos_traj_controller = [
'joint_state_controller', 'gripper_controller',
'pos_traj_controller'
]
self.vel_controller = [
"joint_state_controller", "gripper_controller",
"ur_shoulder_pan_vel_controller",
"ur_shoulder_lift_vel_controller", "ur_elbow_vel_controller",
"ur_wrist_1_vel_controller", "ur_wrist_2_vel_controller",
"ur_wrist_3_vel_controller"
]
self.pos_controller = [
"joint_state_controller", "gripper_controller",
"ur_shoulder_pan_pos_controller",
"ur_shoulder_lift_pos_controller", "ur_elbow_pos_controller",
"ur_wrist_1_pos_controller", "ur_wrist_2_pos_controller",
"ur_wrist_3_pos_controller"
]
# Stop flag durning training
self.stop_flag = False
global currentState
currentState = msg.position
def desiredStateCB(msg):
global desiredState
desiredState = msg.position
if __name__ == '__main__':
rospy.init_node('velocity_control')
rate = rospy.Rate(60)
joint_pub = rospy.Publisher('/jaco/joint_control', JointState, queue_size=1000)
current_sub = rospy.Subscriber('/jaco/joint_state', JointState, currentStateCB)
desired_sub = rospy.Subscriber('desired_position', JointState, desiredStateCB)
msg = JointState()
msg.name = ['jaco_arm_0_joint', 'jaco_arm_1_joint', 'jaco_arm_2_joint', 'jaco_arm_3_joint', 'jaco_arm_4_joint', 'jaco_arm_5_joint', 'jaco_finger_joint_0', 'jaco_finger_joint_2', 'jaco_finger_joint_4']
while not rospy.is_shutdown():
error = desiredState - currentState
for e in error:
if e < 0.01:
e = 0
vel = P*error
msg.velocity = vel
joint_pub.publish(msg)
rate.sleep()
server.insert(int_marker, processFeedback)
menu_handler.apply(server, int_marker.name)
if __name__ == "__main__":
rospy.init_node("visual_control")
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
pub2 = rospy.Publisher('read_coords', Int32, queue_size=10)
pub3 = rospy.Publisher('move_to_time', CoordsWithTime, queue_size=100)
time.sleep(1)
pub2.publish(1)
time.sleep(1)
joint_state_send = JointState()
joint_state_send.header = Header()
joint_state_send.name = [
'base_to_base_rot', 'base_rot_to_link_1', 'link_1_to_link_2',
'link_2_to_link_3', 'link_3_to_link_end'
]
joint_state_send.name = joint_state_send.name + [
'link_1_to_add_1', 'link_2_to_add_4', 'link_3_to_add_2',
'base_rot_to_add_3', 'link_2_to_add_5'
]
br = TransformBroadcaster()
# create a timer to update the published transforms
server = InteractiveMarkerServer("uarm_controller")
def main():
print "INITIALIZING HEAD NODE IN SIMULATION MODE BY MARCOSOFT..."
###Connection with ROS
rospy.init_node("head")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = ["pan_connect", "tilt_connect"]
jointStates.position = [0, 0]
#Stablishing suscribers & plublishers
subPosition = rospy.Subscriber(
"head/goal_pose", Float32MultiArray, callbackPosHead
) #Se corre esta linea para ajustar el angulo del kinect
pubHeadPose = rospy.Publisher("head/current_pose",
Float32MultiArray,
queue_size=1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubHeadBattery = rospy.Publisher("/hardware/robot_state/head_battery",
Float32,
queue_size=1)
#Rate at which ROS will loop (in Hz)
loop = rospy.Rate(30)
global goalPan
global goalTilt
goalPan = 0
goalTilt = 0
pan = 0
tilt = 0
speedPan = 0.1 #These values should represent the Dynamixel's moving_speed
speedTilt = 0.1
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0, 0]
while not rospy.is_shutdown():
deltaPan = goalPan - pan
deltaTilt = goalTilt - tilt
if deltaPan > speedPan:
deltaPan = speedPan
if deltaPan < -speedPan:
deltaPan = -speedPan
if deltaTilt > speedTilt:
deltaTilt = speedTilt
if deltaTilt < -speedTilt:
deltaTilt = -speedTilt
pan += deltaPan
tilt += deltaTilt
jointStates.header.stamp = rospy.Time.now()
jointStates.position[0] = pan
jointStates.position[
1] = -tilt #A tilt > 0 goes upwards, but to keep a dextereous system, positive tilt should go downwards
pubJointStates.publish(jointStates)
#print "Poses: " + str(panPose) + " " + str(tiltPose)
msgCurrentPose.data = [pan, tilt]
pubHeadPose.publish(msgCurrentPose)
msgBattery = Float32()
msgBattery.data = 12.0
pubHeadBattery.publish(msgBattery)
loop.sleep()
def cb(msg):
state_msg = JointState()
state_msg.name = msg.joint_names
state_msg.position = msg.points[0].positions
state_msg.header = msg.header
joint_state_pub.publish(state_msg)
if start_flag == True:
return
for i in range(0, len(data.position)):
if "joint" in data.name[i]:
ctrl_joint.position.append(data.position[i])
start_flag = True
if __name__ == "__main__":
rospy.init_node("joint_control_test")
start_flag = False
joint_state = JointState()
joint_size = 0
duration = rospy.get_param("~duration", 0.05) #20Hz
joint_control_topic_name = rospy.get_param("~joint_control_topic_name",
"/dragon/joints_ctrl")
pub = rospy.Publisher(joint_control_topic_name, JointState, queue_size=10)
joint_state_topic_name = rospy.get_param("~joint_state_topic_name",
"/dragon/joint_states")
rospy.Subscriber(joint_state_topic_name, JointState, joint_state_cb)
start_time = 0
target_max_pitch = -0.34
ctrl_joint = JointState()
def _feedback_cb(self, msg):
for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity,
msg.effort):
if name not in self.hebi_mapping:
print("WARNING: arm_callback - unrecognized name!!!")
else:
self.current_jt_pos[name] = pos
self.current_jt_vel[name] = vel
self.current_jt_eff[name] = eff
if not rospy.is_shutdown(
) and self._joint_state_pub is not None: # Publish JointState() for RViz
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self._active_joints
jointstate.position = self._get_joint_angles()
jointstate.velocity = [0.0] * len(jointstate.name)
jointstate.effort = [0.0] * len(jointstate.name)
self._joint_state_pub.publish(jointstate)
# TODO: Make this less hacky
if not rospy.is_shutdown() and self._joint_state_pub is not None:
if not self._executing_trajectory:
joint_grav_torques = self.kdl_kin.get_joint_torques_from_gravity(
self._get_joint_angles(),
grav=[0, 0, -9.81]) # TODO: FIXME: Hardcoded
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self.hebi_mapping
if self._hold_position:
# jointstate.position = self.waypoints[-1].positions # jerky
jointstate.position = self._hold_joint_states
else:
jointstate.position = []
jointstate.velocity = []
jointstate.effort = joint_grav_torques
self.cmd_pub.publish(jointstate)
positions.append(0.0)
joints.append('head_j2')
positions.append(0.0)
joints.append('head_j3')
positions.append(0.0)
joints.append('head_j4')
positions.append(0.0)
joints.append('head_j5')
positions.append(0.0)
joints.append('head_j6')
positions.append(0.0)
header = Header(0,rospy.Time.now(),'0')
pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions)))
try:
while not rospy.is_shutdown():
time.sleep(1.0)
header = Header(0,rospy.Time.now(),'0')
pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions)))
except (KeyboardInterrupt,EOFError,rospy.ROSInterruptException):
pass
#!/usr/bin/python3
import rospy
import time
from std_msgs.msg import Int32
from sensor_msgs.msg import JointState
pub = rospy.Publisher('motor_data', JointState, queue_size=10)
rospy.init_node('initialize_position')
joint = JointState()
joint.header.stamp = rospy.Time.now()
joint.position = [10, 10, 90, 0, 0]
joint.velocity = [10, 10, 10, 10, 10]
joint.name = ["1", "2", "3", "4", "5"]
pub.publish(joint)
def __init__(self):
rospy.init_node('thymio')
# load script on the Thymio
rospy.wait_for_service('aseba/get_node_list')
get_aseba_nodes = rospy.ServiceProxy('aseba/get_node_list',
GetNodeList)
while True:
if 'thymio-II' in get_aseba_nodes().nodeList:
break
rospy.loginfo('Waiting for thymio node ...')
rospy.sleep(1)
rospy.wait_for_service('aseba/load_script')
load_script = rospy.ServiceProxy('aseba/load_script', LoadScripts)
default_script_path = os.path.join(
roslib.packages.get_pkg_dir('thymio_driver'),
'aseba/thymio_ros.aesl')
script_path = rospy.get_param('~script', default_script_path)
rospy.loginfo("Load aseba script %s", script_path)
load_script(script_path)
# initialize parameters
self.tf_prefix = rospy.get_param('tf_prefix', '')
self.odom_frame = self.frame_name(
rospy.get_param('~odom_frame', 'odom'))
self.robot_frame = self.frame_name(
rospy.get_param('~robot_frame', 'base_link'))
self.x = 0
self.y = 0
self.th = 0
self.then = rospy.Time.now()
odom_rate = rospy.get_param('~odom_max_rate', -1)
if odom_rate == 0:
self.odom_min_period = -1
else:
self.odom_min_period = 1.0 / odom_rate
self.odom_msg = Odometry(header=rospy.Header(frame_id=self.odom_frame),
child_frame_id=self.robot_frame)
self.odom_msg.pose.pose.position.z = 0
self.odom_msg.pose.covariance[0] = -1
self.odom_msg.header.stamp = rospy.Time.now()
# subscribe to topics
self.aseba_pub = rospy.Publisher('aseba/events/set_speed',
AsebaEvent,
queue_size=1)
self.left_wheel_angle = 0
self.right_wheel_angle = 0
self.axis = rospy.get_param('~axis_length', BASE_WIDTH / 1000.0)
self.motor_speed_deadband = rospy.get_param('~motor_speed_deadband',
10)
# self.ticks2mm = rospy.get_param('~ticks2mm', 1.0 / SPEED_COEF)
# self.diff_factor = rospy.get_param('~diff_factor', 1.0)
def_cal = [0.001 / SPEED_COEF, 0]
left_wheel_calibration = rospy.get_param('~left_wheel_calibration/q',
def_cal)
self.left_wheel_speed, self.left_wheel_motor_speed = motor_speed_conversion(
*left_wheel_calibration)
rospy.loginfo('Init left wheel with calibration %s',
left_wheel_calibration)
right_wheel_calibration = rospy.get_param('~right_wheel_calibration/q',
def_cal)
self.right_wheel_speed, self.right_wheel_motor_speed = motor_speed_conversion(
*right_wheel_calibration)
rospy.loginfo('Init right wheel with calibration %s',
right_wheel_calibration)
left_wheel_joint = rospy.get_param('~left_wheel_joint',
'left_wheel_joint')
right_wheel_joint = rospy.get_param('~right_wheel_joint',
'right_wheel_joint')
self.wheel_state_msg = JointState()
self.wheel_state_msg.name = [left_wheel_joint, right_wheel_joint]
self.wheel_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self.odom_broadcaster = TransformBroadcaster()
rospy.Subscriber('aseba/events/odometry', AsebaEvent,
self.on_aseba_odometry_event)
rospy.Subscriber("cmd_vel", Twist, self.on_cmd_vel)
self.buttons = Joy()
self.buttons_pub = rospy.Publisher('buttons', Joy, queue_size=1)
rospy.Subscriber("aseba/events/buttons", AsebaEvent,
self.on_aseba_buttons_event)
for button in BUTTONS:
rospy.Subscriber('aseba/events/button_' + button, AsebaEvent,
self.on_aseba_button_event(button))
proximity_range_min = rospy.get_param('~proximity/range_min', 0.0215)
proximity_range_max = rospy.get_param('~proximity/range_min', 0.14)
proximity_field_of_view = rospy.get_param('~proximity/field_of_view',
0.3)
self.proximity_sensors = [{
'publisher':
rospy.Publisher('proximity/' + name, Range, queue_size=1),
'msg':
Range(header=rospy.Header(
frame_id=self.frame_name('proximity_{name}_link'.format(
name=name))),
radiation_type=Range.INFRARED,
field_of_view=proximity_field_of_view,
min_range=proximity_range_min,
max_range=proximity_range_max)
} for name in PROXIMITY_NAMES]
self.proximityToLaserPublisher = rospy.Publisher('proximity/laser',
LaserScan,
queue_size=1)
self.proximityToLaser = LaserScan(
header=rospy.Header(frame_id=self.frame_name('laser_link')),
angle_min=-0.64,
angle_max=0.64,
angle_increment=0.32,
time_increment=0,
scan_time=0,
range_min=proximity_range_min + 0.08,
range_max=proximity_range_max + 0.08)
rospy.Subscriber('aseba/events/proximity', AsebaEvent,
self.on_aseba_proximity_event)
self.ground_sensors = [{
'publisher':
rospy.Publisher('ground/' + name, Range, queue_size=1),
'msg':
Range(header=rospy.Header(
frame_id=self.frame_name('ground_{name}_link'.format(
name=name))),
radiation_type=Range.INFRARED,
field_of_view=proximity_field_of_view,
min_range=(GROUND_MIN_RANGE / 1000.0),
max_range=(GROUND_MAX_RANGE / 1000.0))
} for name in GROUND_NAMES]
ground_threshold = rospy.get_param('~ground/threshold', 200)
rospy.Subscriber('aseba/events/ground', AsebaEvent,
self.on_aseba_ground_event)
rospy.set_param("~ground_threshold", ground_threshold)
self.imu = Imu(header=rospy.Header(frame_id=self.robot_frame))
# no orientation or angular velocity information
self.imu.orientation_covariance[0] = -1
self.imu.angular_velocity_covariance[0] = -1
# just an accelerometer
self.imu.linear_acceleration_covariance[0] = 0.07
self.imu.linear_acceleration_covariance[4] = 0.07
self.imu.linear_acceleration_covariance[8] = 0.07
self.imu_publisher = rospy.Publisher('imu', Imu, queue_size=1)
rospy.Subscriber('aseba/events/accelerometer', AsebaEvent,
self.on_aseba_accelerometer_event)
self.tap_publisher = rospy.Publisher('tap', Empty, queue_size=1)
rospy.Subscriber('aseba/events/tap', AsebaEvent,
self.on_aseba_tap_event)
self.temperature = Temperature(header=rospy.Header(
frame_id=self.robot_frame))
self.temperature.variance = 0.01
self.temperature_publisher = rospy.Publisher('temperature',
Temperature,
queue_size=1)
rospy.Subscriber('aseba/events/temperature', AsebaEvent,
self.on_aseba_temperature_event)
self.sound_publisher = rospy.Publisher('sound', Float32, queue_size=1)
self.sound_threshold_publisher = rospy.Publisher(
'aseba/events/set_sound_threshold', AsebaEvent, queue_size=1)
rospy.Subscriber('aseba/events/sound', AsebaEvent,
self.on_aseba_sound_event)
rospy.Subscriber('sound_threshold', Float32, self.on_sound_threshold)
self.remote_publisher = rospy.Publisher('remote', Int8, queue_size=1)
rospy.Subscriber('aseba/events/remote', AsebaEvent,
self.on_aseba_remote_event)
rospy.Subscriber('comm/transmit', Int16, self.on_sound_threshold)
self.comm_publisher = rospy.Publisher('comm/receive',
Int16,
queue_size=1)
self.aseba_set_comm_publisher = rospy.Publisher(
'aseba/events/set_comm', AsebaEvent, queue_size=1)
rospy.Subscriber('aseba/events/comm', AsebaEvent,
self.on_aseba_comm_event)
# actuators
for name in BODY_LEDS:
rospy.Subscriber('led/body/' + name, ColorRGBA,
self.on_body_led(name))
rospy.Subscriber('led', Led, self.on_led)
self.aseba_led_publisher = rospy.Publisher('aseba/events/set_led',
AsebaEvent,
queue_size=6)
rospy.Subscriber('led/off', Empty, self.on_led_off)
rospy.Subscriber('led/gesture', LedGesture, self.on_led_gesture)
self.aseba_led_gesture_publisher = rospy.Publisher(
'aseba/events/set_led_gesture', AsebaEvent, queue_size=6)
rospy.Subscriber('led/gesture/circle', Float32,
self.on_led_gesture_circle)
rospy.Subscriber('led/gesture/off', Empty, self.on_led_gesture_off)
rospy.Subscriber('led/gesture/blink', Float32,
self.on_led_gesture_blink)
rospy.Subscriber('led/gesture/kit', Float32, self.on_led_gesture_kit)
rospy.Subscriber('led/gesture/alive', Empty, self.on_led_gesture_alive)
rospy.Subscriber('sound/play', Sound, self.on_sound_play)
self.aseba_led_gesture_publisher = rospy.Publisher(
'aseba/events/set_led_gesture', AsebaEvent, queue_size=6)
rospy.Subscriber('sound/play/system', SystemSound,
self.on_system_sound_play)
self.aseba_play_sound_publisher = rospy.Publisher(
'aseba/events/play_sound', AsebaEvent, queue_size=1)
self.aseba_play_system_sound_publisher = rospy.Publisher(
'aseba/events/play_system_sound', AsebaEvent, queue_size=1)
rospy.Subscriber('alarm', Bool, self.on_alarm)
self.alarm_timer = None
rospy.Subscriber('shutdown', Empty, self.on_shutdown_msg)
self.aseba_shutdown_publisher = rospy.Publisher(
'aseba/events/shutdown', AsebaEvent, queue_size=1)
rospy.on_shutdown(self.shutdown)
Server(ThymioConfig, self.change_config)
# tell ros that we are ready
rospy.Service('thymio_is_ready', std_srvs.srv.Empty, self.ready)
#!/usr/bin/env python3
#include libraries
import rospy
import tf
from math import cos, sin, pi
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from nav_msgs.msg import Odometry
jointstates = JointState()
jointstates.name = ['wheel0', 'wheel1', 'wheel2', 'wheel3']
pubodom = rospy.Publisher('odom', Odometry, queue_size=10)
broadcaster = tf.TransformBroadcaster()
map_broadcaster = tf.TransformBroadcaster()
timeold = 0
vel = Twist()
x = y = theta = 0
counter = 0
def jstatecb(jointstatesraw):
global jointstates
global timeold
global x, y, theta, counter
noise = np.random.normal(0, 0.25, 7)
'''print right.joint_angles().values()
print right.joint_angles().keys()
print right.joint_angles()
print "\r\n"
'''
'''seeds_random={
'right': JointState(
header=hdr,
name= right.joint_angles().keys(),
position= (right.joint_angles().values() + noise).tolist()
)
}
'''
js = JointState()
js.header = hdr
i = 0
for key, val in right.joint_angles().iteritems():
js.name.append(key)
js.position.append(val + noise[i])
i += 1
ikreq.seed_angles = [js]
resp = iksvc(ikreq)
try:
return limb_joints
except:
mess = 1
smile.publish(mess)
def handleUpdateResponse(r):
"""Handles the response for an update request
Parses the message received from the robot, then publish the result
Arguments:
r {bytearray} -- response from the robot
Returns:
RobotState -- state object
"""
global last_state
global update_response
global stored_pose_count
global name
global do_update
global p_joint_states
global p_robot_state
try:
i = 8
state = rosweld_drivers.msg.RobotState()
state.speed = unpack('>f', r[i:i + 4])[0]
i = i + 4
state.pose.position.x = unpack('>f', r[i:i + 4])[0] / 1000
i = i + 4
state.pose.position.y = unpack('>f', r[i:i + 4])[0] / 1000
i = i + 4
state.pose.position.z = unpack('>f', r[i:i + 4])[0] / 1000
i = i + 4
state.euler_angles.rx = unpack('>f', r[i:i + 4])[0]
i = i + 4
state.euler_angles.ry = unpack('>f', r[i:i + 4])[0]
i = i + 4
state.euler_angles.rz = unpack('>f', r[i:i + 4])[0]
i = i + 4
orientation = tf.transformations.quaternion_from_euler(
radians(state.euler_angles.rz), radians(state.euler_angles.ry),
radians(state.euler_angles.rx), axes)
state.pose.orientation.x = orientation[0]
state.pose.orientation.y = orientation[1]
state.pose.orientation.z = orientation[2]
state.pose.orientation.w = orientation[3]
# -1 from the step index, because NACHI's list indexing is from 1, not zero
state.step = unpack('>i', r[i:i + 4])[0] - 1
i = i + 4
state.storedPoses = unpack('>i', r[i:i + 4])[0]
i = i + 4
stateId = unpack('>i', r[i:i + 4])[0]
state.robotProgramState = RobotStates.toString(stateId)
i = i + 4
state.mode = ["Teach", "Playback",
"High-speed Teach"][unpack('>i', r[i:i + 4])[0]]
for _ in range(7):
i = i + 4
state.joints.append(unpack('>f', r[i:i + 4])[0])
#print "Update received"
#print seq_back, fingerY, currentMove, speed, RobotStates.toString(robotProgramState)
update_response = True
state.isMoving = last_state is not None and last_state.pose != state.pose
if last_state != state or do_update:
p_robot_state.publish(state)
js = JointState()
js.name = [
"joint_1", "joint_2", "joint_7", "joint_3", "joint_4",
"joint_5", "joint_6", "joint_tip"
]
js.position = [
state.joints[0], -1 * (state.joints[1] - pi / 2),
state.joints[6], -1 * state.joints[2], state.joints[3],
-1 * state.joints[4], state.joints[5], 0.0
]
p_joint_states.publish(js)
do_update = False
last_state = state
stored_pose_count = state.storedPoses
except Exception as e:
status(name, "Robot update failed: %s" % (str(e)), STATE.ERROR)
return state
if mapped >= jointRange[1]:
return jointRange[1]
elif mapped <= jointRange[0]:
return jointRange[0]
else:
return mapped
if __name__ == '__main__':
gp = Grasper()
gp.readGloveCalibration()
rospy.init_node('contact_publisher')
rate = rospy.Rate(60)
while not rospy.is_shutdown():
msg = JointState()
msg.name = [
'jaco_arm_0_joint', 'jaco_arm_1_joint', 'jaco_arm_2_joint',
'jaco_arm_3_joint', 'jaco_arm_4_joint', 'jaco_arm_5_joint',
'jaco_finger_joint_0', 'jaco_finger_joint_2', 'jaco_finger_joint_4'
]
msg.position[0:6] = gp.jointsPos
msg.position[6:9] = gp.fingersPos
print(msg.position)
gp.pub.publish(msg)
rate.sleep()
def publish_joints(self, joints):
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = self.group.get_joints()[:-1]
joint_state.position = joints
self.joint_state_publisher.publish(joint_state)
def IK(self, T):
# getting the desired base to end effector pose from the Transform message of geometry_msgs message
x_desired = T
# a list to store the final joint position guess
final_joint_pos = []
current_joint_state = JointState()
# index the right joint names with the correct joint state
current_joint_state.name = self.active_joint_names
# initialize a zeros list for the guess
q_c_rand = numpy.zeros((self.active_num_joints))
# iteration variables
l = 0
m = 0
n = 0
# boolean condition
found = False
# main loop for number of tries
while not found and n < 50:
m = 0
n = n + 1
# loop for getting random position values for performing inverse kinematics
for k in range(self.active_num_joints):
N = random.uniform(-1 * math.pi, 1 * math.pi)
q_c_rand[k] = N
q = True
# loop for performing convergence of delta x which is the displacement between the desired end effector pose and the current end effector pose
while q == True and m < 250:
# updates the position in the joint state with every new guess
current_joint_state.position = q_c_rand
# gets new base to end effector and joint transforms from the forward kinematics using the joint state of inverse kinematics
x_cur, joint_trans = self.forward_kinematics(
current_joint_state)
L = len(joint_trans)
# calculation of the end effector to base for calculation purposes
x_cur_inv = tf.transformations.inverse_matrix(x_cur)
# getting the difference in the target end effector pose to the current end effector pose
ee_ee_ik = tf.transformations.concatenate_matrices(
x_cur_inv, x_desired)
# getting the rotational component of the transform
r_ee_ik = ee_ee_ik[:3, :3]
A, B = rotation_from_matrix(r_ee_ik)
# getting the rotational part of delta x for a gain of 1
r_v_ee_ik = numpy.dot(A, B)
# getting the translational part of delta x for a gain of 1
t_v_ee_ik = tf.transformations.translation_from_matrix(
ee_ee_ik)
# list of both the translational and rotational parts of delta x
del_x_ik = numpy.append(t_v_ee_ik, r_v_ee_ik)
# Jacobian calculation and assembly loop
Jacobian = numpy.empty((6, 0))
for i in range(L):
b_j = joint_trans[i]
j_b = tf.transformations.inverse_matrix(b_j)
j_ee = numpy.dot(j_b, x_cur)
ee_j = tf.transformations.inverse_matrix(j_ee)
R_ee_j = ee_j[:3, :3]
t_j_ee = tf.transformations.translation_from_matrix(j_ee)
s = skew_mat(t_j_ee)
S = numpy.dot(-1 * R_ee_j, s)
A = numpy.append(R_ee_j, S, axis=1)
B = numpy.append(numpy.zeros([3, 3]), R_ee_j, axis=1)
# velocity transformation matrix
vj = numpy.append(A, B, axis=0)
# x axis alignment
if (self.joint_axes[i] == ([1, 0, 0])):
Jacobian = numpy.column_stack((Jacobian, vj[:, 3]))
# y axis alignment
elif (self.joint_axes[i] == ([0, 1, 0])):
Jacobian = numpy.column_stack((Jacobian, vj[:, 4]))
# z axis alignment
elif (self.joint_axes[i] == ([0, 0, 1])):
Jacobian = numpy.column_stack((Jacobian, vj[:, 5]))
# negative x axis alignment
elif (self.joint_axes[i] == ([-1, 0, 0])):
Jacobian = numpy.column_stack(
(Jacobian, -1 * vj[:, 3]))
# negative y axis alignment
elif (self.joint_axes[i] == ([0, -1, 0])):
Jacobian = numpy.column_stack(
(Jacobian, -1 * vj[:, 4]))
# negative z axis alignment
elif (self.joint_axes[i] == ([0, 0, -1])):
Jacobian = numpy.column_stack(
(Jacobian, -1 * vj[:, 5]))
# fixed axis ignorance
elif (self.joint_axes[i] == 'None'):
continue
# Calculation of pseudo Jacobian inverse
J_pseudo_inv_ik = numpy.linalg.pinv(Jacobian)
# calculation of joint displacement
del_q = numpy.dot(J_pseudo_inv_ik, del_x_ik)
# print del_q
# calculation of new increment in joint value
q_c_rand = q_c_rand + del_q
# to check if delta x is within allowable tolerance ; tolerance = 0.01 (can be changed)
for j in range(len(del_x_ik)):
if (numpy.linalg.norm(del_x_ik) < 0.0001):
l = l + 1
if l == len(del_x_ik):
final_joint_pos = q_c_rand
if self.is_state_valid(final_joint_pos) == True:
found = True
for i in range(7):
if final_joint_pos[i] > 2 * math.pi:
final_joint_pos[i] = numpy.mod(
final_joint_pos[i], 2 * math.pi)
elif final_joint_pos[i] < -2 * math.pi:
final_joint_pos[i] = numpy.mod(
final_joint_pos[i], -2 * math.pi)
if final_joint_pos[i] > math.pi:
final_joint_pos[i] = final_joint_pos[i] - (2 *
math.pi)
elif final_joint_pos[i] < -1 * math.pi:
final_joint_pos[i] = final_joint_pos[i] + (2 *
math.pi)
return final_joint_pos
else:
# iteration count
m = m + 1
if m == 250:
if n < 20:
print 'taking new guess'
q = False
# allowing maximum tries of 20
if n == 20:
print 'reached maximum number of tries please check if the end effector is in the workspace of the robot and try again'
return 0
def __init__(self):
rospy.logdebug("Starting URSimReaching Class object...")
# Init GAZEBO Objects
self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.get_world_state = rospy.ServiceProxy(
'/gazebo/get_world_properties', GetWorldProperties)
# Subscribe joint state and target pose
rospy.Subscriber("/joint_states", JointState,
self.joints_state_callback)
rospy.Subscriber("/target_blocks_pose", Point,
self.target_point_callback)
rospy.Subscriber("/gazebo/link_states", LinkStates,
self.link_state_callback)
# Gets training parameters from param server
self.desired_pose = Pose()
self.running_step = rospy.get_param("/running_step")
self.max_height = rospy.get_param("/max_height")
self.min_height = rospy.get_param("/min_height")
self.observations = rospy.get_param("/observations")
# Joint limitation
shp_max = rospy.get_param("/joint_limits_array/shp_max")
shp_min = rospy.get_param("/joint_limits_array/shp_min")
shl_max = rospy.get_param("/joint_limits_array/shl_max")
shl_min = rospy.get_param("/joint_limits_array/shl_min")
elb_max = rospy.get_param("/joint_limits_array/elb_max")
elb_min = rospy.get_param("/joint_limits_array/elb_min")
wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
self.joint_limits = {
"shp_max": shp_max,
"shp_min": shp_min,
"shl_max": shl_max,
"shl_min": shl_min,
"elb_max": elb_max,
"elb_min": elb_min,
"wr1_max": wr1_max,
"wr1_min": wr1_min,
"wr2_max": wr2_max,
"wr2_min": wr2_min,
"wr3_max": wr3_max,
"wr3_min": wr3_min
}
shp_init_value = rospy.get_param("/init_joint_pose/shp")
shl_init_value = rospy.get_param("/init_joint_pose/shl")
elb_init_value = rospy.get_param("/init_joint_pose/elb")
wr1_init_value = rospy.get_param("/init_joint_pose/wr1")
wr2_init_value = rospy.get_param("/init_joint_pose/wr2")
wr3_init_value = rospy.get_param("/init_joint_pose/wr3")
self.init_joint_pose = [
shp_init_value, shl_init_value, elb_init_value, wr1_init_value,
wr2_init_value, wr3_init_value
]
# Fill in the Done Episode Criteria list
self.episode_done_criteria = rospy.get_param("/episode_done_criteria")
# stablishes connection with simulator
self._gz_conn = GazeboConnection()
self._ctrl_conn = ControllersConnection(namespace="")
# Controller type for ros_control
self._ctrl_type = rospy.get_param("/control_type")
self.pre_ctrl_type = self._ctrl_type
# We init the observations
self.base_orientation = Quaternion()
self.target_point = Point()
self.link_state = LinkStates()
self.joints_state = JointState()
self.end_effector = Point()
self.distance = 0.
# Arm/Control parameters
self._ik_params = setups['UR5_6dof']['ik_params']
# ROS msg type
self._joint_pubisher = JointPub()
self._joint_traj_pubisher = JointTrajPub()
# Gym interface and action
self.action_space = spaces.Discrete(6)
self.observation_space = 15 #np.arange(self.get_observations().shape[0])
self.reward_range = (-np.inf, np.inf)
self._seed()
# Change the controller type
set_joint_vel_server = rospy.Service('/set_velocity_controller',
SetBool, self._set_vel_ctrl)
set_joint_traj_vel_server = rospy.Service(
'/set_trajectory_velocity_controller', SetBool,
self._set_traj_vel_ctrl)
self.vel_traj_controller = [
'joint_state_controller', 'gripper_controller',
'vel_traj_controller'
]
self.vel_controller = [
"joint_state_controller", "gripper_controller",
"ur_shoulder_pan_vel_controller",
"ur_shoulder_lift_vel_controller", "ur_elbow_vel_controller",
"ur_wrist_1_vel_controller", "ur_wrist_2_vel_controller",
"ur_wrist_3_vel_controller"
]
# Helpful False
self.stop_flag = False
stop_trainning_server = rospy.Service('/stop_training', SetBool,
self._stop_trainnig)
start_trainning_server = rospy.Service('/start_training', SetBool,
self._start_trainnig)
#!/usr/bin/env python
# Original code https://github.com/corot/turtlebot_arm.git
import rospy
from sensor_msgs.msg import JointState
rospy.init_node("fake_joint_pub")
p = rospy.Publisher('joint_states', JointState, queue_size=5)
msg = JointState()
msg.name = ["gripper_link_joint"]
msg.position = [0.0 for name in msg.name]
msg.velocity = [0.0 for name in msg.name]
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
p.publish(msg)
rospy.sleep(0.1)
def ik_service_client(limb = "right", use_advanced_options = False):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'right': PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=0.450628752997,
y=0.161615832271,
z=0.217447307078,
),
orientation=Quaternion(
x=0.704020578925,
y=0.710172716916,
z=0.00244101361829,
w=0.00194372088834,
),
),
),
}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
if (use_advanced_options):
# Optional Advanced IK parameters
rospy.loginfo("Running Advanced IK Service Client example.")
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
seed.position = [0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
goal = JointState()
goal.name = ['right_j1', 'right_j2', 'right_j3']
goal.position = [0.1, -0.3, 0.5]
ikreq.nullspace_goal.append(goal)
# The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
# If empty, the default gain of 0.4 will be used
ikreq.nullspace_gain.append(0.4)
else:
rospy.loginfo("Running Simple IK Service Client example.")
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Service call failed: %s" % (e,))
return False
# Check if result valid, and type of seed ultimately used to get solution
if (resp.result_type[0] > 0):
seed_str = {
ikreq.SEED_USER: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp.result_type[0], 'None')
rospy.loginfo("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
(seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(list(zip(resp.joints[0].name, resp.joints[0].position)))
rospy.loginfo("\nIK Joint Solution:\n%s", limb_joints)
rospy.loginfo("------------------")
rospy.loginfo("Response Message:\n%s", resp)
else:
rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
rospy.logerr("Result Error %d", resp.result_type[0])
return False
return True
def initializeHeadLiftJoint(self):
lift_goal = JointState()
head_goal = JointState()
lift_goal.name = ["torso_lift_joint"]
head_goal.name = ["head_2_joint", "head_1_joint"]
lift_goal.position = [0.3]
head_goal.position = [-0.9, 0.0]
head_goal.effort = [0.0, 0.0]
lift_goal.effort = [0.0]
head_goal.velocity = [0.0, 0.0]
lift_goal.velocity = [0.0]
self.lift_goal_pub.publish(lift_goal)
self.head_goal_pub.publish(head_goal)
def main():
print "INITIALIZING ARM NODE IN SIMULATION BY [EDD-II]"
###Connection with ROS
rospy.init_node("head_simul_node")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = [
"arm_flex_joint", "arm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
jointStates.position = [0, 0, 0, 0]
subPosition = rospy.Subscriber("/hardware/arm/goal_pose",
Float32MultiArray, callbackPos)
pubArmPose = rospy.Publisher("/hardware/arm/current_pose",
Float32MultiArray,
queue_size=1)
#For test, these lines can be unneeded
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubArmBattery = rospy.Publisher("/hardware/robot_state/right_arm_battery",
Float32,
queue_size=1)
loop = rospy.Rate(10)
global goalAngles
global goalGripper
global speeds
goalAngles = [0, 0, 0, 0]
angles = [0, 0, 0, 0]
speeds = [0.01, 0.01, 0.01, 0.01]
goalGripper = 0
gripper = 0
gripperSpeed = 0.1
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0, 0, 0, 0]
msgCurrentGripper = Float32()
msgCurrentGripper.data = 0
deltaAngles = [0, 0, 0, 0]
deltaGripper = 0
while not rospy.is_shutdown():
for i in range(len(deltaAngles)):
deltaAngles[i] = goalAngles[i] - angles[i]
if deltaAngles[i] > speeds[i]:
deltaAngles[i] = speeds[i]
if deltaAngles[i] < -speeds[i]:
deltaAngles[i] = -speeds[i]
angles[i] += deltaAngles[i]
jointStates.position[i] = angles[i]
msgCurrentPose.data[i] = angles[i]
deltaGripper = goalGripper - gripper
if deltaGripper > gripperSpeed:
deltaGripper = gripperSpeed
if deltaGripper < -gripperSpeed:
deltaGripper = -gripperSpeed
gripper += deltaGripper
jointStates.header.stamp = rospy.Time.now()
pubJointStates.publish(jointStates)
pubArmPose.publish(msgCurrentPose)
msgBattery = Float32()
msgBattery.data = 11.6
pubArmBattery.publish(msgBattery)
loop.sleep()
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
from tocabi_controller.msg import model
import pinocchio
import numpy as np
from sys import argv
from os.path import dirname, join, abspath
qmsg = JointState()
modelmsg = model()
start = False
def callback(data):
global qmsg
global start
qmsg = data
start = True
def talker():
rospy.init_node("tocabi_pinocchio", anonymous = False)
rospy.Subscriber("/tocabi/pinocchio/jointstates", JointState, callback)
global model
pub = rospy.Publisher("/tocabi/pinocchio", model, queue_size=1)
modelmsg.CMM = [0 for i in range(33*6)]
modelmsg.COR = [0 for i in range(33*33)]
modelmsg.M = [0 for i in range(33*33)]
modelmsg.g = [0 for i in range(33)]
def __init__(self, sock=None, ip="192.168.1.108"):
'''
constructor
:param socket: can optionally pass in a socket if this class is used in a larger framework
:param ip: ip address of robot
:return:
'''
if not sock == None:
self.sock = sock
else:
self.HOST = ip
self.PORT = 30003
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.sock.connect((self.HOST, self.PORT))
self.sock.settimeout(2) # very important!
# print self.sock.recv(1024)
# variables
self.line = ''
self.messageSize = 1060
self.time = 0.0 # time elapsed since the controller was started
self.q_target = 6 * [0.0] # target joint positions
self.qd_target = 6 * [0.0] # target joint velocities
self.qdd_target = 6 * [0.0] # target joint accelerations
self.i_target = 6 * [0.0] # target joint currents
self.m_target = 6 * [0.0] # target torques
self.q_actual = 6 * [0.0] # actual joint positions
self.qd_actual = 6 * [0.0] # actual joint velocities
self.i_actual = 6 * [0.0] # actual joint currents
self.i_control = 6 * [0.0] # joint control currents
self.tool_vec_actual = 6 * [
0.0
] # actual cartesian coordinates of the tool (x,y,z,rx,ry,rz)
self.tcp_speed_actual = 6 * [
0.0
] # actual speed of the tool given in cartesian coords
self.tcp_force = 6 * [0.0] # genearlized forces in the tcp
self.tool_vec_target = 6 * [
0.0
] # target cartesian coordinates of the tool (x,y,z,rx,ry,rz)
self.tcp_speed_target = 6 * [
0.0
] # target speed of the tool given in cartesian coords
self.digital_input_bits = 0.0 # crrent state of the digital inputs
self.motor_temps = 6 * [0.0] # motor temperatures (celsius)
self.controller_timer = 0.0 # controller realtime thread execution time
self.test_value = 0.0 # a value used by UR software only
self.robot_mode = 0.0 # robot mode
self.joint_modes = 6 * [0.0] # joint control modes
self.safety_mode = 0.0
self.tool_accel_values = 3 * [0.0] # tool accelerometer values
self.speed_scaling = 0.0 # speed scaling of the trajectory limiter
self.lin_momentum_norm = 0.0 # norm of cartesian linear momentum
self.v_main = 0.0 # masterboard: main voltage
self.v_robot = 0.0 #masterboard:robot voltage (48v)
self.i_robot = 0.0 # materboard: robot current
self.v_actual = 6 * [0.0] # actual joint voltages
self.digital_outputs = 0.0 # digitial outputs
self.program_state = 0.0 # program state
self.joint_state = JointState()
self.joint_state.name = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
def set_joint_state(self,joint_target):
self.joint_target = joint_target
joint_state = JointState()
joint_state.position = tuple(joint_target)
self.pub.publish(joint_state)
robot = RobotCommander()
scene = PlanningSceneInterface()
group = MoveGroupCommander("manipulator")
directory = "/home/abd-alrahman/catkin_ws/src/attrobot/plotting/1_55_23_pos/itr_0.csv"
time_algo, time, thetas, dthetas, thetas_sp, dthetas_sp = Read_Loggings(
directory)
rospy.wait_for_service('/compute_fk')
fk_solution = rospy.ServiceProxy('/compute_fk', GetPositionFK)
ref_frame = Header()
ref_frame.frame_id = "base_link"
ref_frame.stamp = rospy.Time.now()
# fk_link_names
fk_link_names = ["racket"]
joint_state = JointState()
joint_state.header = Header()
joint_state.header.frame_id = "racket"
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
]
x_sp_list = []
y_sp_list = []
z_sp_list = []
x_list = []
y_list = []
z_list = []
for i in range(len(thetas_sp)):
joint_state.position = [
def __init__(self):
super().__init__("ros2_dorna_utilities")
self.namespace = ""
if len(sys.argv) != 2:
self.namespace = sys.argv[1]
else:
pass
self.utils_to_gui_msg = DornaUtilsToGui()
self.joint_state = JointState()
self.utils_to_gui_timer_period = 0.5
# self.utils_to_esd_msg.actual_pose = ""
self.utils_to_gui_msg.saved_poses = []
# self.utils_to__msg.echo_utility_action = ""
# self.utils_to_esd_msg.echo_utility_pose_name = ""
self.act_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.prev_action = ""
self.prev_pose_name = ""
if self.namespace != "":
self.joint_names = [
self.namespace + "/" + "dorna_axis_1_joint",
self.namespace + "/" + "dorna_axis_2_joint",
self.namespace + "/" + "dorna_axis_3_joint",
self.namespace + "/" + "dorna_axis_4_joint",
self.namespace + "/" + "dorna_axis_5_joint", "nonexistent"
]
else:
self.joint_names = [
"dorna_axis_1_joint", "dorna_axis_2_joint",
"dorna_axis_3_joint", "dorna_axis_4_joint",
"dorna_axis_5_joint", "nonexistent"
]
self.joints_input = os.path.join(
get_package_share_directory('ros2_dorna_utilities'), 'poses',
'joint_poses.csv')
self.joints_oldposes = os.path.join(
get_package_share_directory('ros2_dorna_utilities'), 'poses',
'joint_oldposes.csv')
self.joints_newposes = os.path.join(
get_package_share_directory('ros2_dorna_utilities'), 'poses',
'joint_newposes.csv')
self.joint_state_subscriber = self.create_subscription(
JointState, "/dorna_joint_states", self.joint_callback, 10)
self.gui_to_utils_subscriber = self.create_subscription(
DornaGuiToUtils, "/dorna_gui_to_utils", self.gui_to_utils_callback,
10)
time.sleep(2)
self.utils_to_gui_publisher_ = self.create_publisher(
DornaUtilsToGui, "/dorna_utils_to_gui", 10)
self.utils_to_gui_timer = self.create_timer(
self.utils_to_gui_timer_period, self.utils_to_gui_callback)
def sign(value):
if value >= 0:
return 1
else:
return -1
if __name__ == '__main__':
ser = serial.Serial('/dev/ttyUSB0', timeout=1.0)
dif = DifControl(ser)
dif.init_motors(10, 20, 20)
v_X_targ = 0
w_Z_targ = 0
rospy.init_node("differential_control")
sub_cmd_vel = rospy.Subscriber("/cmd_vel", Twist, cb_cmd_vel, queue_size = 4)
joints_states_pub = rospy.Publisher("/joint_states_wheels", JointState, queue_size = 4)
msg = JointState()
msg.name.append("right")
msg.name.append("left")
msg.velocity.append(0)
msg.velocity.append(0)
while not rospy.is_shutdown():
try:
dif.set_speed(v_X_targ, w_Z_targ)
v_r, v_l = dif.get_motors_speed()
msg.velocity[0] = v_r
msg.velocity[1] = v_l
joints_states_pub.publish(msg)
sleep(0.1)
except KeyboardInterrupt:
break
rospy.sleep(1)
#mk_fig.mk_figures()
break
if __name__ == '__main__':
try:
rospy.init_node('gripper_node')
#spin_rate = rospy.Rate(1)
# 各クラスのインスタンス化
force_sensor_capture = ForceSensorCapture()
joint_controller = JointController()
#sub_flag = Sub_handing_end_flag()
initial_position = JointState()
initial_position.name.extend([
'arm_lift_joint', 'arm_flex_joint', 'arm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint', 'hand_motor_joint'
])
initial_position.position.extend([0.0, 0.0, 0.0, -1.57, 0.0, 1.2])
release = JointState()
release.name.extend(['hand_motor_joint'])
release.position.extend([1.1])
#csvファイルの準備
path = '/home/naoyamada/catkin_ws3/src/pre_experiment/scripts/csv/f/raw_data.csv'
path2 = '/home/naoyamada/catkin_ws3/src/pre_experiment/scripts/csv/f/ave_data.csv'
os.remove(path)
f = open(path, 'w')
#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import JointState
rospy.init_node("sim_velocity")
pub = rospy.Publisher('/joint_state',JointState,queue_size=10)
step = 0.1
pose = [0,0,0,0]
vel = [step,-step,step,-step]
js_msg = JointState()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
js_msg.header.stamp = rospy.Time.now()
js_msg.position = pose
js_msg.velocity = vel
pose = [el+step for el in pose]
pub.publish(js_msg)
rate.sleep()
def np_array_to_joint_state_effort(array):
data = JointState()
for i in range(0, array.size):
data.effort.append(array[i])
return data
