def construct_hold_pose_task(): """ Helper function for holding the pose of the hand Use this if you want to hold the position of the hand, without leaving constrained manipulation. Output: + command: api command object """ frame_name = "hand" force_lim = 80 torque_lim = 10 force_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0) torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_HOLD_POSE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=0.0, frame_name=frame_name) return command
def construct_right_handed_ballvalve_task(velocity_normalized, force_limit=40, torque_limit=5): """ Helper function for manipulating right-handed ball valves Use this when the hand is to the right of the pivot of the ball valve And when hand x axis is roughly parallel to the axis of rotation of the ball valve params: + velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0] + force_limit (optional): positive value denoting max force robot will exert along task dimension + torque_limit (optional): positive value denoting max torque robot will exert along the axis of rotation of the task Output: + command: api command object Notes: If the grasp is such that the hand x axis is not parallel to the axis of rotation of the ball valve, then use the lever task. """ velocity_normalized = max(min(velocity_normalized, 1.0), -1.0) velocity_limit = scale_velocity_lim_given_force_lim(force_limit) tangential_velocity = velocity_normalized * velocity_limit frame_name = "hand" force_lim = force_limit torque_lim = torque_limit # Force/torque signs are opposite for right-handed ball valve force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=1.0) # The torque vector is provided as additional information denoting the # axis of rotation of the task. torque_direction = geometry_pb2.Vec3(x=-1.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_SE3_CIRCLE_FORCE_TORQUE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=tangential_velocity, frame_name=frame_name) return command
def construct_lever_task(velocity_normalized, force_limit=40, torque_limit=5): """ Helper function for manipulating levers params: + velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0] + force_limit (optional): positive value denoting max force robot will exert along task dimension + torque_limit (optional): positive value denoting max torque robot will exert along the axis of rotation of the task Output: + command: api command object Notes: In this function, we assume the initial motion of the lever is along the z axis of the hand (up and down). If the initial grasp is such that the initial motion needs to be something else, change the force direction. This function assumes we don't know the plane_normal (i.e. torque_direction) of the lever. If we do know that, we can specify it as torque_direction or use the ball valve task types, which assume a specific grasp and specify what the initial torque_direction is. """ velocity_normalized = max(min(velocity_normalized, 1.0), -1.0) velocity_limit = scale_velocity_lim_given_force_lim(force_limit) tangential_velocity = velocity_normalized * velocity_limit frame_name = "hand" force_lim = force_limit torque_lim = torque_limit force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=1.0) torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_SE3_CIRCLE_FORCE_TORQUE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=tangential_velocity, frame_name=frame_name) return command
def construct_crank_task(velocity_normalized, force_limit=40): """ Helper function for manipulating cranks with a free to rotate handle params: + velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0] + force_limit (optional): positive value denoting max force robot will exert along task dimension Output: + command: api command object Notes: In this function, we assume the initial motion of the crank is along the y axis of the hand (left and right). If the initial grasp is such that the initial motion needs to be something else, change the force direction. """ velocity_normalized = max(min(velocity_normalized, 1.0), -1.0) velocity_limit = scale_velocity_lim_given_force_lim(force_limit) tangential_velocity = velocity_normalized * velocity_limit frame_name = "hand" force_lim = force_limit # Setting a placeholder value that doesn't matter, since we don't # apply a pure torque in this task. torque_lim = 5.0 # This assumes the grasp of crank is such that the crank will initially # move along the hand y axis. Change if that is not the case. force_direction = geometry_pb2.Vec3(x=0.0, y=1.0, z=0.0) torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_R3_CIRCLE_EXTRADOF_FORCE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=tangential_velocity, frame_name=frame_name) return command
def construct_wheel_task(velocity_normalized, force_limit=40): """ Helper function for turning wheels while grasping the rim Use this when the wheel is grasped on the rim. If the grasp is on a handle that is free to rotate, use the crank task type. If the handle is not free to rotate, use this task type. params: + velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0] + force_limit (optional): positive value denoting max force robot will exert along task dimension Output: + command: api command object Notes: This assumes initial motion will be along the y axis of the hand, which is often the case. Change force_direction if that is not true. """ velocity_normalized = max(min(velocity_normalized, 1.0), -1.0) velocity_limit = scale_velocity_lim_given_force_lim(force_limit) tangential_velocity = velocity_normalized * velocity_limit frame_name = "hand" force_lim = force_limit # Setting a placeholder value that doesn't matter, since we don't # apply a pure torque in this task. torque_lim = 5.0 force_direction = geometry_pb2.Vec3(x=0.0, y=1.0, z=0.0) torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_R3_CIRCLE_FORCE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=tangential_velocity, frame_name=frame_name) return command
def construct_knob_task(velocity_normalized, torque_limit=5): """ Helper function for turning purely rotational knobs Use this for turning knobs/valves that do not have a lever arm params: + velocity_normalized: normalized task rotational velocity in range [-1.0, 1.0] + torque_limit (optional): positive value denoting max torque robot will exert along axis of rotation of the task Output: + command: api command object Notes: This assumes that the axis of rotation of of the knob is roughly parallel to the x axis of the hand. Change torque_direction if that is not the case. """ velocity_normalized = max(min(velocity_normalized, 1.0), -1.0) rot_velocity_limit = scale_rot_velocity_lim_given_torque_lim(torque_limit) rotational_velocity = velocity_normalized * rot_velocity_limit frame_name = "hand" # Setting a placeholder value that doesn't matter, since we don't # apply a pure force in this task. force_lim = 40.0 torque_lim = torque_limit force_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) torque_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_SE3_ROTATIONAL_TORQUE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, rotational_speed=rotational_velocity, frame_name=frame_name) return command
def construct_drawer_task(velocity_normalized, force_limit=40): """ Helper function for opening/closing drawers params: + velocity_normalized: normalized task tangential velocity in range [-1.0, 1.0] + force_limit (optional): positive value denoting max force robot will exert along task dimension Output: + command: api command object Notes: In this function, we assume the initial motion of the drawer is along the x axis of the hand (forward and backward). If the initial grasp is such that the initial motion needs to be something else, change the force direction. """ velocity_normalized = max(min(velocity_normalized, 1.0), -1.0) velocity_limit = scale_velocity_lim_given_force_lim(force_limit) tangential_velocity = velocity_normalized * velocity_limit frame_name = "hand" force_lim = force_limit # Setting a placeholder value that doesn't matter, since we don't # apply a pure torque in this task. torque_lim = 5.0 force_direction = geometry_pb2.Vec3(x=1.0, y=0.0, z=0.0) torque_direction = geometry_pb2.Vec3(x=0.0, y=0.0, z=0.0) init_wrench_dir = geometry_pb2.Wrench(force=force_direction, torque=torque_direction) task_type = basic_command_pb2.ConstrainedManipulationCommand.Request.TASK_TYPE_R3_LINEAR_FORCE command = RobotCommandBuilder.constrained_manipulation_command( task_type=task_type, init_wrench_direction_in_frame_name=init_wrench_dir, force_limit=force_lim, torque_limit=torque_lim, tangential_speed=tangential_velocity, frame_name=frame_name) return command