def applyForce(wheel='left', effort=0):
service = '/gazebo/apply_joint_effort'
rospy.wait_for_service(service)
setJointEffort = rospy.ServiceProxy(service, ApplyJointEffort)
effortMsg = ApplyJointEffortRequest()
effortMsg.joint_name = wheel + '_wheel_hinge'
effortMsg.effort = effort
effortMsg.duration.secs = 3
resp = setJointEffort(effortMsg)
def forces():
service = '/gazebo/apply_joint_effort'
rospy.wait_for_service(service)
setJointEffort = rospy.ServiceProxy(service, ApplyJointEffort)
effort = ApplyJointEffortRequest()
effort.joint_name = 'left_wheel_hinge'
effort.effort = 2
effort.duration.secs = -1
resp = setJointEffort(effort)
print resp
def jointeffort(t, apply_joint_effort_srv, joint_clear_srv):
from gazebo_msgs.srv import ApplyJointEffortRequest
## Joint1
effort_msg = ApplyJointEffortRequest()
effort_msg.joint_name = 'joystick_world_joint'
effort_msg.effort = -0.001
effort_msg.start_time.secs = 0.0
effort_msg.start_time.nsecs = 0.0
effort_msg.duration.secs = 2.0
effort_msg.duration.nsecs = 0.0
joint_clear_srv.value('joystick_world_joint')
feedback = apply_joint_effort_srv.value(effort_msg)
def close_gripper(self):
effort = ApplyJointEffortRequest()
effort.joint_name = 'm6'
effort.effort = -0.1
effort.start_time.secs = 0
effort.start_time.nsecs = 0
effort.duration.secs = -1 # no stop
effort.duration.nsecs = 0
try:
resp = self.effort_service(effort)
rospy.loginfo('success: {} status: {}'.format(
resp.success, resp.status_message))
except rospy.ServiceException as exc:
rospy.logerr("Service did not process request: " + str(exc))
def set_gripper(self, srv):
'''
First clear the existing forces on the gripper then apply a new one in the direction
specified by the service call.
'''
self.clear_force(JointRequestRequest(joint_name=self.joint_name))
effort_mod = 1
if not srv.opened:
effort_mod = -1
joint_effort = ApplyJointEffortRequest()
joint_effort.joint_name = self.joint_name
joint_effort.effort = self.force_to_apply * effort_mod
joint_effort.duration = rospy.Duration(-1)
self.apply_force(joint_effort)
def set_gripper(self, srv):
'''
First clear the existing forces on the gripper then apply a new one in the direction
specified by the service call.
'''
self.clear_force(JointRequestRequest(joint_name=self.joint_name))
effort_mod = 1
if not srv.opened:
effort_mod = -1
joint_effort = ApplyJointEffortRequest()
joint_effort.joint_name = self.joint_name
joint_effort.effort = self.force_to_apply * effort_mod
joint_effort.duration = rospy.Duration(-1)
self.apply_force(joint_effort)
def lower_lift(self):
"""
Send joint commands to lower the lift and open
the grippers, which can be used to pick up an object
"""
# First quickly clear any forces on the lift
self.zero_forces("lift")
# Then lower the lift
lift_req = ApplyJointEffortRequest()
lift_req.joint_name = "lift"
lift_req.effort = self.LIFT_LOWER_FORCE
lift_req.duration.secs = -1 # apply the force indefinitely
r3 = self.joint_control_srv(lift_req)
rospy.sleep(0.5) # wait half a sec to be sure the lift is down
# Finally, open the grippers
self.zero_forces("grip_left")
self.zero_forces("grip_right")
left_grip_req = ApplyJointEffortRequest()
left_grip_req.joint_name="grip_left"
left_grip_req.effort = self.GRIP_OPEN_FORCE
left_grip_req.duration.secs = -1
r1 = self.joint_control_srv(left_grip_req)
right_grip_req = ApplyJointEffortRequest()
right_grip_req.joint_name="grip_right"
right_grip_req.effort = -self.GRIP_OPEN_FORCE
right_grip_req.duration.secs = -1
r2 = self.joint_control_srv(right_grip_req)
def raise_lift(self):
"""
Send joint commands to raise the lift and close
the grippers, which can be used to pick up an object
"""
# First quickly clear any forces on the gripper
self.zero_forces("lift")
self.zero_forces("grip_left")
self.zero_forces("grip_right")
# Then close the grippers
left_grip_req = ApplyJointEffortRequest()
left_grip_req.joint_name="grip_left"
left_grip_req.effort = -self.GRIP_CLOSE_FORCE
left_grip_req.duration.secs = -1
r1 = self.joint_control_srv(left_grip_req)
right_grip_req = ApplyJointEffortRequest()
right_grip_req.joint_name="grip_right"
right_grip_req.effort = self.GRIP_CLOSE_FORCE
right_grip_req.duration.secs = -1
r2 = self.joint_control_srv(right_grip_req)
rospy.sleep(0.5) # wait half a sec to make sure the grippers are closed
# Finally, raise the lift
lift_req = ApplyJointEffortRequest()
lift_req.joint_name = "lift"
lift_req.effort = self.LIFT_RAISE_FORCE
lift_req.duration.secs = -1 # apply the force indefinitely
r3 = self.joint_control_srv(lift_req)
#!/usr/bin/env python
import rospy
from gazebo_msgs.srv import ApplyJointEffortRequest, ApplyJointEffort
rospy.init_node("simple_demo")
apply_joint_effort_srv = rospy.ServiceProxy('/gazebo/apply_joint_effort', ApplyJointEffort)
req = ApplyJointEffortRequest()
req.effort = 5.0
req.duration.secs = 30
joints = [ 'right_rear', 'left_rear']
for j in joints:
req.joint_name = j
apply_joint_effort_srv(req)